第10章 SLAM基本概念与贝叶斯框架

import numpy as np
from scipy.stats import norm
 
class BayesianFilter:
    """贝叶斯滤波基础框架"""
    def __init__(self, dim_state: int = 3):
        self.dim_state = dim_state
        self.belief_mean = np.zeros(dim_state)
        self.belief_cov = np.eye(dim_state) * 0.1
        
    def predict(self, control: np.ndarray, motion_noise: np.ndarray):
        """预测步骤：根据运动模型更新信念"""
        # 运动模型：x_t = f(x_{t-1}, u_t) + noise
        G = np.eye(self.dim_state)  # 运动模型雅可比
        self.belief_mean = self.motion_model(self.belief_mean, control)
        self.belief_cov = G @ self.belief_cov @ G.T + np.diag(motion_noise)
        
    def update(self, observation: np.ndarray, obs_noise: np.ndarray):
        """更新步骤：根据观测修正信念"""
        H = np.eye(self.dim_state)  # 观测模型雅可比
        innovation = observation - self.observation_model(self.belief_mean)
        S = H @ self.belief_cov @ H.T + np.diag(obs_noise)
        K = self.belief_cov @ H.T @ np.linalg.inv(S)
        self.belief_mean = self.belief_mean + K @ innovation
        self.belief_cov = (np.eye(self.dim_state) - K @ H) @ self.belief_cov
        
    def motion_model(self, state: np.ndarray, control: np.ndarray) -> np.ndarray:
        """运动模型：根据控制输入更新位姿"""
        x, y, theta = state
        v, omega = control
        dt = 0.1
        new_x = x + v * np.cos(theta) * dt
        new_y = y + v * np.sin(theta) * dt
        new_theta = theta + omega * dt
        return np.array([new_x, new_y, new_theta])
    
    def observation_model(self, state: np.ndarray) -> np.ndarray:
        """观测模型：从位姿预测观测值"""
        return state  # 简化：直接观测位姿

import numpy as np
 
class SLAMBayesianFilter:
    """基于贝叶斯滤波的SLAM框架"""
    def __init__(self, num_landmarks: int = 100):
        self.num_landmarks = num_landmarks
        # 状态向量：[robot_x, robot_y, robot_theta, lm1_x, lm1_y, lm2_x, lm2_y, ...]
        self.state_dim = 3 + 2 * num_landmarks
        self.mean = np.zeros(self.state_dim)
        self.cov = np.eye(self.state_dim) * 0.1
        self.landmarks_seen = np.zeros(num_landmarks, dtype=bool)
        
    def predict(self, odom: np.ndarray, noise: np.ndarray):
        """运动模型预测"""
        x, y, theta = self.mean[:3]
        v = odom[0]
        omega = odom[1]
        dt = 0.1
        
        # 更新机器人位姿
        self.mean[0] += v * np.cos(theta) * dt
        self.mean[1] += v * np.sin(theta) * dt
        self.mean[2] += omega * dt
        
        # 运动雅可比
        G = np.eye(self.state_dim)
        G[0, 2] = -v * np.sin(theta) * dt
        G[1, 2] = v * np.cos(theta) * dt
        
        # 协方差更新
        motion_cov = np.zeros((self.state_dim, self.state_dim))
        motion_cov[:3, :3] = np.diag(noise)
        self.cov = G @ self.cov @ G.T + motion_cov
        
    def update_with_landmark(self, z: np.ndarray, landmark_id: int, 
                              sensor_noise: np.ndarray):
        """使用路标点观测更新"""
        if not self.landmarks_seen[landmark_id]:
            # 初始化新路标点
            idx = 3 + 2 * landmark_id
            x_r, y_r, theta_r = self.mean[:3]
            range_meas, bearing_meas = z
            
            # 将极坐标观测转换为全局坐标
            lm_x = x_r + range_meas * np.cos(theta_r + bearing_meas)
            lm_y = y_r + range_meas * np.sin(theta_r + bearing_meas)
            self.mean[idx] = lm_x
            self.mean[idx + 1] = lm_y
            self.landmarks_seen[landmark_id] = True
        else:
            # EKF更新已知路标点
            idx = 3 + 2 * landmark_id
            x_r, y_r, theta_r = self.mean[:3]
            lm_x, lm_y = self.mean[idx], self.mean[idx + 1]
            
            # 预测观测
            dx = lm_x - x_r
            dy = lm_y - y_r
            predicted_range = np.sqrt(dx**2 + dy**2)
            predicted_bearing = np.arctan2(dy, dx) - theta_r
            
            # 观测雅可比
            H = np.zeros((2, self.state_dim))
            H[0, 0] = -dx / predicted_range
            H[0, 1] = -dy / predicted_range
            H[0, idx] = dx / predicted_range
            H[0, idx + 1] = dy / predicted_range
            H[1, 0] = dy / (dx**2 + dy**2)
            H[1, 1] = -dx / (dx**2 + dy**2)
            H[1, 2] = -1
            H[1, idx] = -dy / (dx**2 + dy**2)
            H[1, idx + 1] = dx / (dx**2 + dy**2)
            
            # EKF更新
            innovation = np.array([z[0] - predicted_range, 
                                    z[1] - predicted_bearing])
            innovation[1] = np.arctan2(np.sin(innovation[1]), 
                                        np.cos(innovation[1]))
            
            S = H @ self.cov @ H.T + np.diag(sensor_noise)
            K = self.cov @ H.T @ np.linalg.inv(S)
            self.mean = self.mean + K @ innovation
            self.cov = (np.eye(self.state_dim) - K @ H) @ self.cov

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan, Imu, Image
from nav_msgs.msg import Odometry
 
class SensorMonitor(Node):
    """多传感器数据监控节点"""
    def __init__(self):
        super().__init__('sensor_monitor')
        
        # 订阅激光雷达
        self.scan_sub = self.create_subscription(
            LaserScan, '/scan', self.scan_callback, 10)
            
        # 订阅IMU
        self.imu_sub = self.create_subscription(
            Imu, '/imu', self.imu_callback, 10)
            
        # 订阅里程计
        self.odom_sub = self.create_subscription(
            Odometry, '/odom', self.odom_callback, 10)
            
        self.scan_count = 0
        self.imu_count = 0
        self.odom_count = 0
        
    def scan_callback(self, msg: LaserScan):
        self.scan_count += 1
        angles = len(msg.ranges)
        valid = sum(1 for r in msg.ranges if 0 < r < msg.range_max)
        self.get_logger().info(
            f'激光: {angles}条射线, {valid}个有效点, '
            f'范围[{msg.range_min:.2f}, {msg.range_max:.2f}]',
            throttle_duration_sec=2.0
        )
        
    def imu_callback(self, msg: Imu):
        self.imu_count += 1
        acc = msg.linear_acceleration
        gyro = msg.angular_velocity
        self.get_logger().info(
            f'IMU: acc=({acc.x:.2f},{acc.y:.2f},{acc.z:.2f}) '
            f'gyro=({gyro.x:.2f},{gyro.y:.2f},{gyro.z:.2f})',
            throttle_duration_sec=2.0
        )
        
    def odom_callback(self, msg: Odometry):
        self.odom_count += 1
        pose = msg.pose.pose
        twist = msg.twist.twist
        self.get_logger().info(
            f'里程计: pos=({pose.position.x:.2f},{pose.position.y:.2f}) '
            f'vel=({twist.linear.x:.2f}m/s)',
            throttle_duration_sec=2.0
        )
 
def main():
    rclpy.init()
    node = SensorMonitor()
    rclpy.spin(node)
    rclpy.shutdown()

import rclpy
from rclpy.node import Node
import tf2_ros
import tf2_geometry_msgs
from geometry_msgs.msg import TransformStamped, PointStamped
 
class TFMonitor(Node):
    """TF变换监控与查询节点"""
    def __init__(self):
        super().__init__('tf_monitor')
        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
        self.timer = self.create_timer(1.0, self.query_tf)
        
    def query_tf(self):
        try:
            # 查询 map → base_link 变换
            trans: TransformStamped = self.tf_buffer.lookup_transform(
                'map', 'base_link', rclpy.time.Time()
            )
            x = trans.transform.translation.x
            y = trans.transform.translation.y
            self.get_logger().info(f'map→base_link: ({x:.3f}, {y:.3f})')
            
            # 坐标变换：将激光点转换到map坐标系
            laser_point = PointStamped()
            laser_point.header.frame_id = 'laser'
            laser_point.point.x = 1.0
            laser_point.point.y = 0.0
            laser_point.point.z = 0.0
            
            map_point = self.tf_buffer.transform(laser_point, 'map')
            self.get_logger().info(
                f'激光点(1,0,0)在map下: '
                f'({map_point.point.x:.3f}, {map_point.point.y:.3f})'
            )
        except Exception as e:
            self.get_logger().warn(f'TF查询失败: {e}')
            
    def can_transform_check(self, source: str, target: str) -> bool:
        """检查TF变换是否可用"""
        try:
            self.tf_buffer.can_transform(target, source, 
                                          rclpy.time.Time(), timeout=rclpy.duration.Duration(seconds=0.1))
            return True
        except:
            return False

# 检查传感器话题频率
ros2 topic hz /scan
ros2 topic hz /odom
ros2 topic hz /imu
 
# 检查传感器时间戳
ros2 topic echo /scan --once | grep stamp
ros2 topic echo /odom --once | grep stamp
 
# 查看TF树
ros2 run tf2_tools view_frames.py
 
# 查看TF广播关系
ros2 run tf2_ros tf2_monitor

import numpy as np
 
class GraphSLAM:
    """GraphSLAM图优化SLAM框架"""
    def __init__(self):
        self.poses = []        # 机器人位姿序列
        self.landmarks = {}    # 路标点字典 {id: (x, y)}
        self.constraints = []  # 约束列表
        self.landmark_id_counter = 0
        
    def add_odometry_constraint(self, pose_i: int, pose_j: int, 
                                  transform: np.ndarray, information: np.ndarray):
        """添加里程计约束"""
        self.constraints.append({
            'type': 'odometry',
            'i': pose_i,
            'j': pose_j,
            'measurement': transform,
            'information': information,
        })
        
    def add_observation_constraint(self, pose_id: int, landmark_id: int,
                                     observation: np.ndarray, information: np.ndarray):
        """添加观测约束"""
        self.constraints.append({
            'type': 'observation',
            'pose': pose_id,
            'landmark': landmark_id,
            'measurement': observation,
            'information': information,
        })
        
    def add_new_landmark(self, position: np.ndarray) -> int:
        """添加新路标点"""
        idx = self.landmark_id_counter
        self.landmarks[idx] = position
        self.landmark_id_counter += 1
        return idx
        
    def build_linear_system(self):
        """构建线性系统 H·Δx = b"""
        n = 3 * len(self.poses) + 2 * len(self.landmarks)
        H = np.zeros((n, n))
        b = np.zeros(n)
        
        for constraint in self.constraints:
            if constraint['type'] == 'odometry':
                i = constraint['i']
                j = constraint['j']
                meas = constraint['measurement']
                info = constraint['information']
                
                # 误差计算和线性化
                xi = self.poses[i] if i < len(self.poses) else np.zeros(3)
                xj = self.poses[j] if j < len(self.poses) else np.zeros(3)
                
                # 残差
                error = self.compute_odometry_error(xi, xj, meas)
                
                # 雅可比矩阵
                Ji = np.eye(3)
                Jj = -np.eye(3)
                
                # 添加到Hessian矩阵
                idx_i = 3 * i
                idx_j = 3 * j
                H[idx_i:idx_i+3, idx_i:idx_i+3] += Ji.T @ info @ Ji
                H[idx_i:idx_i+3, idx_j:idx_j+3] += Ji.T @ info @ Jj
                H[idx_j:idx_j+3, idx_i:idx_i+3] += Jj.T @ info @ Ji
                H[idx_j:idx_j+3, idx_j:idx_j+3] += Jj.T @ info @ Jj
                b[idx_i:idx_i+3] += Ji.T @ info @ error
                b[idx_j:idx_j+3] += Jj.T @ info @ error
                
        return H, b
    
    def compute_odometry_error(self, xi, xj, meas):
        """计算里程计误差"""
        return meas - (xj - xi)
    
    def optimize(self):
        """求解图优化"""
        H, b = self.build_linear_system()
        # 固定第一个位姿（固定map坐标系）
        H[:3, :] = 0
        H[:, :3] = 0
        H[:3, :3] = np.eye(3)
        b[:3] = 0
        
        # 求解线性系统
        delta = np.linalg.solve(H + 1e-6 * np.eye(len(H)), b)
        
        # 更新位姿
        for i in range(len(self.poses)):
            self.poses[i] += delta[3*i:3*i+3]

def suggest_slam_algorithm(
    environment: str = 'indoor',
    has_odom: bool = True,
    has_imu: bool = False,
    sensor_type: str = 'lidar2d',
    precision_requirement: str = 'high',
    realtime: bool = True
) -> str:
    """根据场景推荐SLAM算法"""
    if sensor_type == 'lidar2d':
        if environment == 'indoor' and not has_odom:
            return 'Hector-SLAM (不依赖里程计)'
        elif environment == 'indoor' and has_odom:
            return 'slam_toolbox (ROS2推荐) / gmapping'
        elif environment == 'large_scale':
            return 'Cartographer (大场景图优化)'
    elif sensor_type == 'lidar3d':
        return 'Cartographer 3D / A-LOAM'
    elif sensor_type == 'camera':
        if has_imu:
            return 'VINS-Fusion / ORB-SLAM3'
        else:
            return 'ORB-SLAM2 / DSO'
    elif sensor_type == 'mixed':
        return 'LVI-SAM / R3LIVE'
    return 'slam_toolbox (默认推荐)'

import numpy as np
from scipy import sparse
 
class InformationFormSLAM:
    """信息形式的SLAM滤波器"""
    def __init__(self, num_landmarks: int = 100):
        self.dim = 3 + 2 * num_landmarks
        # 信息矩阵 (协方差的逆)
        self.Omega = sparse.eye(self.dim, format='csr') * 0.01
        # 信息向量
        self.xi = np.zeros(self.dim)
        self.num_landmarks = num_landmarks
        
    def predict(self, motion_info: np.ndarray):
        """信息形式的预测"""
        # 信息形式的预测相当于添加先验约束
        pass
        
    def update_with_measurement(self, z: np.ndarray, 
                                  landmark_id: int,
                                  R: np.ndarray):
        """信息形式的更新"""
        idx = 3 + 2 * landmark_id
        
        # 构建观测约束的信息贡献
        H = self._compute_jacobian(landmark_id)
        H_R = H.T @ np.linalg.inv(R) @ H
        
        # 更新信息矩阵
        self.Omega += H_R
        
        # 更新信息向量
        innovation = z - self._predict_observation(landmark_id)
        self.xi += H.T @ np.linalg.inv(R) @ innovation
        
    def get_mean(self) -> np.ndarray:
        """从信息形式恢复均值"""
        return sparse.linalg.spsolve(self.Omega, self.xi)
    
    def get_covariance(self) -> np.ndarray:
        """从信息形式恢复协方差"""
        return sparse.linalg.inv(self.Omega).toarray()
    
    def _compute_jacobian(self, landmark_id: int):
        """计算观测雅可比"""
        H = np.zeros((2, self.dim))
        # 简化实现
        idx = 3 + 2 * landmark_id
        H[0, 0] = -1.0
        H[0, idx] = 1.0
        H[1, 1] = -1.0
        H[1, idx + 1] = 1.0
        return H
    
    def _predict_observation(self, landmark_id: int) -> np.ndarray:
        """预测观测值"""
        mean = self.get_mean()
        robot_pose = mean[:3]
        landmark_pos = mean[3 + 2*landmark_id:3 + 2*landmark_id + 2]
        return landmark_pos - robot_pose[:2]

# slam_toolbox 在线异步建图
ros2 launch slam_toolbox online_async_launch.py \
  slam_params_file:=./config/slam_toolbox_params.yaml \
  use_sim_time:=true
 
# 课程 Cartographer 2D 建图
ros2 launch slam_lab cartographer_mapping.launch.py \
  configuration_directory:=src/lab_code/ch10_lab/slam_lab/config/cartographer \
  configuration_basename:=xbot_2d.lua \
  use_sim_time:=true
 
# Nav2 AMCL定位
ros2 launch nav2_bringup localization_launch.py \
  map:=./maps/my_map.yaml \
  use_sim_time:=true

# 录制传感器数据
ros2 bag record -o slam_data /scan /tf /tf_static /odom /imu
 
# 离线建图（使用rosbag）
ros2 launch slam_toolbox offline_launch.py \
  bag_filename:=slam_data use_sim_time:=true
 
# 评估建图结果
ros2 run nav2_map_server map_saver_cli -f final_map

source /opt/ros/jazzy/setup.bash
source install/setup.bash
 
# 终端 1：启动仿真并让机器人运动
ros2 launch robot_sim_demo gazebo2.launch.py \
  gui:=false rviz:=false drive:=true

# 终端 2：采样观测 z、控制/运动结果和 TF
source install/setup.bash
ros2 topic echo /scan --once
ros2 topic echo /odom --once
ros2 topic echo /tf --once
ros2 topic info /scan