第五章 SLAM与导航

class LaserSLAMFrontend:
    def __init__(self):
        self.current_scan = None
        self.previous_scan = None

    def extract_features(self, scan):
        """提取角点特征"""
        features = []

        for i in range(1, len(scan) - 1):
            # 计算曲率
            diff = scan[i-1] + scan[i+1] - 2*scan[i]
            curvature = abs(diff)

            if curvature > threshold:
                features.append(i)

        return features

    def match_scans(self, scan1, scan2):
        """扫描匹配(ICP)"""
        # 转换为点云
        points1 = self.scan_to_points(scan1)
        points2 = self.scan_to_points(scan2)

        # ICP配准
        R, t = self.icp(points1, points2)

        return R, t

    def scan_to_points(self, scan):
        """扫描转点云"""
        angles = np.linspace(-np.pi, np.pi, len(scan))
        x = scan * np.cos(angles)
        y = scan * np.sin(angles)
        return np.column_stack([x, y])

class VisualSLAMFrontend:
    def __init__(self):
        self.detector = cv2.ORB_create(1000)
        self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING)

    def extract_features(self, image):
        """提取特征点"""
        kp, desc = self.detector.detectAndCompute(image, None)
        return kp, desc

    def match_frames(self, desc1, desc2):
        """帧间匹配"""
        matches = self.matcher.knnMatch(desc1, desc2, k=2)

        # Lowe's ratio test
        good_matches = []
        for m, n in matches:
            if m.distance < 0.75 * n.distance:
                good_matches.append(m)

        return good_matches

    def estimate_motion(self, kp1, kp2, matches, K):
        """估计相机运动"""
        # 提取匹配点
        pts1 = np.float32([kp1[m.queryIdx].pt for m in matches])
        pts2 = np.float32([kp2[m.trainIdx].pt for m in matches])

        # 本质矩阵
        E, mask = cv2.findEssentialMat(pts1, pts2, K)

        # 恢复位姿
        _, R, t, mask = cv2.recoverPose(E, pts1, pts2, K)

        return R, t

import g2o

class PoseGraphOptimizer:
    def __init__(self):
        self.optimizer = g2o.SparseOptimizer()
        solver = g2o.BlockSolverSE2(g2o.LinearSolverCholmodSE2())
        solver = g2o.OptimizationAlgorithmLevenberg(solver)
        self.optimizer.set_algorithm(solver)

    def add_vertex(self, id, pose):
        """添加位姿节点"""
        v = g2o.VertexSE2()
        v.set_id(id)
        v.set_estimate(pose)

        if id == 0:
            v.set_fixed(True)  # 固定第一个节点

        self.optimizer.add_vertex(v)

    def add_edge(self, id1, id2, measurement, information):
        """添加约束边"""
        edge = g2o.EdgeSE2()
        edge.set_vertex(0, self.optimizer.vertex(id1))
        edge.set_vertex(1, self.optimizer.vertex(id2))
        edge.set_measurement(measurement)
        edge.set_information(information)

        self.optimizer.add_edge(edge)

    def optimize(self, iterations=10):
        """执行优化"""
        self.optimizer.initialize_optimization()
        self.optimizer.optimize(iterations)

    def get_pose(self, id):
        """获取优化后位姿"""
        return self.optimizer.vertex(id).estimate()

class LoopClosureDetector:
    def __init__(self):
        self.keyframes = []
        self.descriptors = []

    def add_keyframe(self, image, pose):
        """添加关键帧"""
        # 提取BoW特征
        desc = self.extract_bow_features(image)

        self.keyframes.append({'image': image, 'pose': pose})
        self.descriptors.append(desc)

    def detect_loop(self, current_image, current_id):
        """检测回环"""
        current_desc = self.extract_bow_features(current_image)

        candidates = []

        # 搜索历史帧
        for i in range(max(0, current_id - 50)):  # 跳过最近的帧
            similarity = self.compute_similarity(current_desc, self.descriptors[i])

            if similarity > threshold:
                candidates.append(i)

        if candidates:
            # 验证几何一致性
            best_match = self.verify_geometry(current_image,
                                              [self.keyframes[i]['image'] for i in candidates])
            return best_match

        return None

#!/usr/bin/env python
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import PoseStamped

class NavigationController:
    def __init__(self):
        rospy.init_node('navigation_controller')

        # Action客户端
        self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
        self.client.wait_for_server()

    def goto(self, x, y, theta):
        """导航到目标点"""
        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = "map"
        goal.target_pose.header.stamp = rospy.Time.now()

        goal.target_pose.pose.position.x = x
        goal.target_pose.pose.position.y = y
        goal.target_pose.pose.orientation.z = np.sin(theta / 2)
        goal.target_pose.pose.orientation.w = np.cos(theta / 2)

        rospy.loginfo(f"导航到: ({x}, {y}, {theta})")
        self.client.send_goal(goal)

        # 等待结果
        wait = self.client.wait_for_result()

        if not wait:
            rospy.logerr("Action服务器不可用")
            return False
        else:
            state = self.client.get_state()
            if state == actionlib.GoalStatus.SUCCEEDED:
                rospy.loginfo("导航成功!")
                return True
            else:
                rospy.logwarn("导航失败")
                return False

    def cancel(self):
        """取消导航"""
        self.client.cancel_goal()

    def patrol(self, waypoints):
        """巡逻"""
        for i, (x, y, theta) in enumerate(waypoints):
            rospy.loginfo(f"前往航点 {i+1}/{len(waypoints)}")
            success = self.goto(x, y, theta)

            if not success:
                rospy.logwarn(f"航点{i+1}导航失败")
                break

            rospy.sleep(2)  # 停留2秒

# 使用示例
if __name__ == '__main__':
    nav = NavigationController()

    # 单点导航
    nav.goto(x=2.0, y=3.0, theta=0.0)

    # 巡逻
    waypoints = [
        (1.0, 1.0, 0.0),
        (2.0, 1.0, 1.57),
        (2.0, 2.0, 3.14),
        (1.0, 2.0, -1.57)
    ]
    nav.patrol(waypoints)

class CustomRecovery:
    def execute(self):
        # 1. 后退
        self.move_backward(0.5)  # 后退0.5m

        # 2. 旋转
        self.rotate(np.pi)  # 转180度

        # 3. 重新规划
        self.replan()

def adaptive_navigation(distance_to_goal):
    if distance_to_goal > 5.0:
        # 远距离: 快速
        set_max_vel(0.8)
        set_tolerance(0.2)
    else:
        # 近距离: 精确
        set_max_vel(0.3)
        set_tolerance(0.05)

class RobustNavigation:
    def navigate_with_retry(self, goal, max_retries=3):
        for attempt in range(max_retries):
            result = self.goto(goal)

            if result == SUCCESS:
                return True
            elif result == BLOCKED:
                # 被困,换路径
                self.find_alternative_path()
            elif result == TIMEOUT:
                # 超时,降低速度
                self.reduce_speed()

        return False  # 失败