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

source ~/.config/ros2-course-com260-ch09-ch10/env.bash

class BayesianFilter {
public:
  Vector3d belief_mean = Vector3d::Zero();
  Eigen::Matrix3d belief_cov = Eigen::Matrix3d::Identity() * 0.1;
  void predict(const Vector2d & control, const Vector3d & motion_noise) {
    const double theta = belief_mean.z();
    belief_mean += Vector3d(control.x() * std::cos(theta) * 0.1,
      control.x() * std::sin(theta) * 0.1, control.y() * 0.1);
    belief_cov += motion_noise.asDiagonal();
  }
  void update(const Vector3d & observation, const Vector3d & obs_noise) {
    Eigen::Matrix3d S = belief_cov + obs_noise.asDiagonal().toDenseMatrix();
    Eigen::Matrix3d K = belief_cov * S.inverse();
    belief_mean += K * (observation - belief_mean);
    belief_cov = (Eigen::Matrix3d::Identity() - K) * belief_cov;
  }
};

class SLAMBayesianFilter {
public:
  VectorXd mean;
  MatrixXd cov;
  std::vector<bool> landmarks_seen;
  explicit SLAMBayesianFilter(int num_landmarks = 100)
  : mean(VectorXd::Zero(3 + 2 * num_landmarks)),
    cov(MatrixXd::Identity(mean.size(), mean.size()) * 0.1),
    landmarks_seen(num_landmarks, false) {}
  void predict(const Vector2d & odom, const Vector3d & noise) {
    const double v = odom.x(), theta = mean[2], dt = 0.1;
    mean.head<3>() += Vector3d(v * std::cos(theta) * dt,
      v * std::sin(theta) * dt, odom.y() * dt);
    MatrixXd G = MatrixXd::Identity(mean.size(), mean.size());
    G(0, 2) = -v * std::sin(theta) * dt;
    G(1, 2) = v * std::cos(theta) * dt;
    cov = G * cov * G.transpose();
    cov.topLeftCorner<3, 3>() += noise.asDiagonal();
  }
  void update_with_landmark(const Vector2d & z, int id, const Vector2d & noise) {
    const int idx = 3 + 2 * id;
    if (!landmarks_seen.at(id)) {
      mean.segment<2>(idx) = mean.head<2>() + z.x() *
        Vector2d(std::cos(mean[2] + z.y()), std::sin(mean[2] + z.y()));
      landmarks_seen[id] = true;
      return;
    }
    Vector2d d = mean.segment<2>(idx) - mean.head<2>();
    const double q = d.squaredNorm(), r = std::sqrt(q);
    if (q == 0.0) { throw std::invalid_argument("路标与机器人重合，观测雅可比未定义"); }
    MatrixXd H = MatrixXd::Zero(2, mean.size());
    H(0, 0) = -d.x() / r; H(0, 1) = -d.y() / r;
    H(0, idx) = d.x() / r; H(0, idx + 1) = d.y() / r;
    H(1, 0) = d.y() / q; H(1, 1) = -d.x() / q; H(1, 2) = -1.0;
    H(1, idx) = -d.y() / q; H(1, idx + 1) = d.x() / q;
    Vector2d innovation(z.x() - r, wrap(z.y() - std::atan2(d.y(), d.x()) + mean[2]));
    Eigen::Matrix2d S = H * cov * H.transpose() + noise.asDiagonal().toDenseMatrix();
    MatrixXd K = cov * H.transpose() * S.inverse();
    mean += K * innovation;
    cov = (MatrixXd::Identity(mean.size(), mean.size()) - K * H) * cov;
  }
};

#include <algorithm>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <nav_msgs/msg/odometry.hpp>
 
class SensorMonitor : public rclcpp::Node {
public:
  SensorMonitor() : Node("sensor_monitor") {
    scan_ = create_subscription<sensor_msgs::msg::LaserScan>("/scan", 10,
      [this](sensor_msgs::msg::LaserScan::ConstSharedPtr msg) {
        ++scan_count_;
        const auto valid = std::count_if(msg->ranges.begin(), msg->ranges.end(),
          [msg](float r) { return r > 0 && r < msg->range_max; });
        RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 2000,
          "激光: %zu条射线, %td个有效点, 范围[%.2f, %.2f]",
          msg->ranges.size(), valid, msg->range_min, msg->range_max);
      });
    imu_ = create_subscription<sensor_msgs::msg::Imu>("/imu", 10,
      [this](sensor_msgs::msg::Imu::ConstSharedPtr msg) {
        ++imu_count_;
        const auto & a = msg->linear_acceleration; const auto & g = msg->angular_velocity;
        RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 2000,
          "IMU: acc=(%.2f,%.2f,%.2f) gyro=(%.2f,%.2f,%.2f)", a.x, a.y, a.z, g.x, g.y, g.z);
      });
    odom_ = create_subscription<nav_msgs::msg::Odometry>("/odom", 10,
      [this](nav_msgs::msg::Odometry::ConstSharedPtr msg) {
        ++odom_count_;
        RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 2000,
          "里程计: pos=(%.2f,%.2f) vel=(%.2fm/s)",
          msg->pose.pose.position.x, msg->pose.pose.position.y, msg->twist.twist.linear.x);
      });
  }
private:
  std::size_t scan_count_ = 0, imu_count_ = 0, odom_count_ = 0;
  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_;
  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_;
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_;
};
int main(int argc, char ** argv) {
  rclcpp::init(argc, argv); rclcpp::spin(std::make_shared<SensorMonitor>()); rclcpp::shutdown();
}

#include <chrono>
#include <memory>
#include <string>
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
class TFMonitor : public rclcpp::Node {
public:
  TFMonitor() : Node("tf_monitor"), buffer_(get_clock()), listener_(buffer_) {
    timer_ = create_wall_timer(std::chrono::seconds(1), [this]() { query_tf(); });
  }
  bool can_transform_check(const std::string & source, const std::string & target) {
    return buffer_.canTransform(target, source, tf2::TimePointZero, tf2::durationFromSec(0.1));
  }
private:
  void query_tf() {
    try {
      const auto trans = buffer_.lookupTransform("map", "base_link", tf2::TimePointZero);
      RCLCPP_INFO(get_logger(), "map→base_link: (%.3f, %.3f)",
        trans.transform.translation.x, trans.transform.translation.y);
      geometry_msgs::msg::PointStamped point;
      point.header.frame_id = "laser_link"; point.point.x = 1.0;
      const auto mapped = buffer_.transform(point, "map");
      RCLCPP_INFO(get_logger(), "激光点(1,0,0)在map下: (%.3f, %.3f)", mapped.point.x, mapped.point.y);
    } catch (const tf2::TransformException & e) {
      RCLCPP_WARN(get_logger(), "TF查询失败: %s", e.what());
    }
  }
  tf2_ros::Buffer buffer_;
  tf2_ros::TransformListener listener_;
  rclcpp::TimerBase::SharedPtr timer_;
};
int main(int argc, char ** argv) {
  rclcpp::init(argc, argv); rclcpp::spin(std::make_shared<TFMonitor>()); rclcpp::shutdown();
}

# 检查传感器话题频率
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
 
# 查看TF广播关系
ros2 run tf2_ros tf2_monitor

class GraphSLAM {
public:
  struct Constraint { int i, j; Vector3d measurement; Eigen::Matrix3d information; };
  std::vector<Vector3d> poses;
  std::vector<Constraint> constraints;
  void add_odometry_constraint(int i, int j, const Vector3d & measurement,
    const Eigen::Matrix3d & information) { constraints.push_back({i, j, measurement, information}); }
  void optimize() {
    if (poses.empty()) { return; }
    const int n = static_cast<int>(3 * poses.size());
    MatrixXd H = MatrixXd::Zero(n, n);
    VectorXd b = VectorXd::Zero(n);
    for (const auto & edge : constraints) {
      // 简化的全局坐标加性约束，不是完整 SE(2) 相对位姿模型。
      const Vector3d error = edge.measurement - (poses.at(edge.j) - poses.at(edge.i));
      const int i = 3 * edge.i, j = 3 * edge.j;
      H.block<3, 3>(i, i) += edge.information;
      H.block<3, 3>(j, j) += edge.information;
      H.block<3, 3>(i, j) -= edge.information;
      H.block<3, 3>(j, i) -= edge.information;
      b.segment<3>(i) += edge.information * error;
      b.segment<3>(j) -= edge.information * error;
    }
    H.topRows(3).setZero(); H.leftCols(3).setZero();
    H.topLeftCorner<3, 3>().setIdentity(); b.head<3>().setZero();
    VectorXd delta = (H + 1e-6 * MatrixXd::Identity(n, n)).ldlt().solve(-b);
    for (std::size_t i = 0; i < poses.size(); ++i) { poses[i] += delta.segment<3>(3 * i); }
  }
};

inline std::string suggest_slam_algorithm(const std::string & environment = "indoor",
  bool has_odom = true, bool has_imu = false, const std::string & sensor_type = "lidar2d",
  const std::string & precision_requirement = "high", bool realtime = true) {
  (void)precision_requirement; (void)realtime;  // 原示例未使用这两个选型因素。
  if (sensor_type == "lidar2d") {
    if (environment == "indoor" && !has_odom) { return "Hector-SLAM (不依赖里程计)"; }
    if (environment == "indoor" && has_odom) { return "slam_toolbox (ROS2推荐) / gmapping"; }
    if (environment == "large_scale") { return "Cartographer (大场景图优化)"; }
  }
  if (sensor_type == "lidar3d") { return "Cartographer 3D / A-LOAM"; }
  if (sensor_type == "camera") { return has_imu ? "VINS-Fusion / ORB-SLAM3" : "ORB-SLAM2 / DSO"; }
  if (sensor_type == "mixed") { return "LVI-SAM / R3LIVE"; }
  return "slam_toolbox (默认推荐)";
}

class InformationFormSLAM {
public:
  MatrixXd Omega;
  VectorXd xi;
  explicit InformationFormSLAM(int num_landmarks = 100)
  : Omega(MatrixXd::Identity(3 + 2 * num_landmarks, 3 + 2 * num_landmarks) * 0.01),
    xi(VectorXd::Zero(3 + 2 * num_landmarks)) {}
  // 原例 predict 是 pass；本例只演示测量的信息贡献，不提供运动预测。
  void update_with_measurement(const Vector2d & z, int id, const Eigen::Matrix2d & R) {
    const int idx = 3 + 2 * id;
    MatrixXd H = MatrixXd::Zero(2, xi.size());
    H(0, 0) = -1.0; H(0, idx) = 1.0; H(1, 1) = -1.0; H(1, idx + 1) = 1.0;
    // 线性观测 z=H*x 的信息更新；信息向量累加 H^T R^-1 z。
    Omega += H.transpose() * R.inverse() * H;
    xi += H.transpose() * R.inverse() * z;
  }
  VectorXd get_mean() const { return Omega.ldlt().solve(xi); }
  MatrixXd get_covariance() const { return Omega.inverse(); }
};

# x86课程容器已加载 /workspace/install/setup.bash
# slam_toolbox 在线异步建图
ros2 launch slam_toolbox online_async_launch.py \
  slam_params_file:="$(ros2 pkg prefix slam_lab)/share/slam_lab/config/mapper_params_online_async.yaml" \
  use_sim_time:=true
 
# 课程 Cartographer 2D 建图
ros2 launch slam_lab cartographer_mapping.launch.py \
  configuration_directory:="$(ros2 pkg prefix slam_lab)/share/slam_lab/config/cartographer" \
  configuration_basename:=burger_2d.lua \
  use_sim_time:=true
 
# Nav2 AMCL定位
ros2 launch slam_lab amcl_localization.launch.py \
  map:=/workspace/output/maps/my_map.yaml use_sim_time:=true

# x86课程容器
source /workspace/install/setup.bash
# 录制传感器数据
ros2 bag record --use-sim-time -o slam_data /scan /tf /tf_static /odom /imu
 
# 离线建图（使用rosbag）
# x86 容器终端 1；先停止实时 Gazebo 和原建图节点
ros2 launch slam_toolbox online_sync_launch.py \
  slam_params_file:="$(ros2 pkg prefix slam_lab)/share/slam_lab/config/mapper_params_online_async.yaml" \
  use_sim_time:=true
 
# 另一终端播放同一轮录制数据；回放时不同时接入实时 /clock
ros2 bag play slam_data --clock --rate 0.5
 
# 评估建图结果
ros2 run nav2_map_server map_saver_cli -f final_map

RUN_ID=teaching-slam
bash course_support/k3_com260_kit/scripts/x86-ch09-ch10.bash start "$RUN_ID"
bash course_support/k3_com260_kit/scripts/x86-ch09-ch10.bash exec "$RUN_ID" bash

# x86 课程容器终端 1：启动仿真
source /workspace/install/setup.bash
ros2 launch sim_demo sim_bringup.launch.py \
  world:="$(ros2 pkg prefix sim_demo)/share/sim_demo/worlds/museum.sdf" \
  sensor_profile:=teacher gui:=true rviz:=true

# 终端 2：采样观测 z、控制/运动结果和 TF
source ~/.config/ros2-course-com260-ch09-ch10/env.bash
ros2 topic echo /scan --once
ros2 topic echo /odom --once
ros2 topic echo /tf --once
ros2 topic info /scan

source ~/.config/ros2-course-com260-ch09-ch10/env.bash
ros2 run param_demo_cpp patrol_driver --ros-args -p linear_speed:=0.12 -p angular_speed:=0.45