本次閱讀的源碼爲 release-1.0 版本的代碼

https://github.com/googlecartographer/cartographer_ros/tree/release-1.0

https://github.com/googlecartographer/cartographer/tree/release-1.0

也可以下載我上傳的全套工作空間的代碼，包括 protobuf, cartographer, cartographer_ros, ceres,

https://download.csdn.net/download/tiancailx/11224156，現在上傳資源自己不能選下載需要的積分。。。完全靠系統。

本篇文章只簡要分析　cartographer_ros 下的代碼，cartogrpaher 下的代碼請見另一篇文章

cartographer_ros/cartographer_ros/node_main.cc

main()函數調用Run(), 在Run() 首先進行了參數的讀取，包括node_options 和 trajectory_options，然後初始化了 MapBuilder 和 Node，然後有條件的分別調用 Node 的如下方法。

  if (!FLAGS_load_state_filename.empty()) {
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }

  if (FLAGS_start_trajectory_with_default_topics) {
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }

  ::ros::spin();

  node.FinishAllTrajectories();
  node.RunFinalOptimization();

  if (!FLAGS_save_state_filename.empty()) {
    node.SerializeState(FLAGS_save_state_filename);
  }

在main()函數中有一個 ros::start(); ，我沒找到這個的定義。。。

cartographer_ros/cartographer_ros/node.cc

// Wires up ROS topics to SLAM. 將ROS主題連接到SLAM。

HandleStartTrajectory() --> AddTrajectory()

首先在Node的構造函數中將 HandleStartTrajectory() 註冊到服務中，然後通過服務進行方法的調用。

HandleStartTrajectory() 函數首先檢查trajectory options，然後檢查topics，都通過了之後調用AddTrajectory()。

int Node::AddTrajectory(const TrajectoryOptions& options,
                        const cartographer_ros_msgs::SensorTopics& topics) {
  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options, topics);
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
  AddExtrapolator(trajectory_id, options);
  AddSensorSamplers(trajectory_id, options);
  LaunchSubscribers(options, topics, trajectory_id);
  is_active_trajectory_[trajectory_id] = true;
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }
  return trajectory_id;
}

AddTrajectory() 方法根據配置文檔對 extrapolator 和 SensorSample 進行參數配置。然後通過 LaunchSubscribers() 方法將幾種傳感器的數據處理函數通過boost進行回調函數的註冊，即每次相應類型的消息到來，都會進行 HandleImuMessage()等這幾個數據處理函數的調用。

void Node::HandleImuMessage(const int trajectory_id,
                            const std::string& sensor_id,
                            const sensor_msgs::Imu::ConstPtr& msg) {
  carto::common::MutexLocker lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
    return;
  }
  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
  auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
  if (imu_data_ptr != nullptr) {
    extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
  }
  sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}

可以看到，如果存在IMU數據，就將IMU數據輸入到 extrapolator 中，如果有 Odom 數據，也將 odom 數據輸入到 extrapolator 中。

IMU數據類型的處理是通過 map_builder_bridge_.sensor_bridge(trajectory_id)->HandleImuMessage(sensor_id, msg); 進行的。也就是轉到了 map_builder_bridge.h ，與 sensor_bridge.h , map_build 是進行地圖構建的過程，sensor_bridge 是轉換數據類型以及座標變換。

cartographer_ros/cartographer_ros/sensor_bridge.h

Converts ROS messages into SensorData in tracking frame for the MapBuilder.

ROS消息轉換爲 MapBuilder 的 tracking frame 下的 SensorData

// 將 里程計數據 傳入 carto::sensor::OdometryData, 返回指向 carto::sensor::OdometryData 的智能指針
std::unique_ptr<carto::sensor::OdometryData> SensorBridge::ToOdometryData(
    const nav_msgs::Odometry::ConstPtr& msg) {
  const carto::common::Time time = FromRos(msg->header.stamp);
  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
      time, CheckNoLeadingSlash(msg->child_frame_id));
  if (sensor_to_tracking == nullptr) {
    return nullptr;
  }
  return carto::common::make_unique<carto::sensor::OdometryData>(
      carto::sensor::OdometryData{
          time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()});
}

通過 ToOdometryData() ToImuData() 將 ROS 下的消息類型轉換爲 Cartographer 的數據類型。並將 odom 的數據通過乘以一個座標變化矩陣轉換到 tracking frame 座標系下。

laserscan, MultiEchoLaserScanMessage 的數據處理是調用 HandleLaserScan() --> HandleRangefinder()，PointCloud2Message 的數據處理是直接調用 HandleRangefinder(), 代碼如下

void SensorBridge::HandleLaserScan() {...}

void SensorBridge::HandleRangefinder(
    const std::string& sensor_id, const carto::common::Time time,
    const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
  const auto sensor_to_tracking =
      tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
  if (sensor_to_tracking != nullptr) {
    trajectory_builder_->AddSensorData(
        sensor_id, carto::sensor::TimedPointCloudData{
                       time, sensor_to_tracking->translation().cast<float>(),
                       carto::sensor::TransformTimedPointCloud(
                           ranges, sensor_to_tracking->cast<float>())});
  }
}

所有的傳感器數據包括 odom imu nav_fix landmark laser multiEchoLaser pointcloud2 的處理都是先通過 cartographer_ros/msg_conversion.h 先將ROS 下的消息類型轉換成 Carto 的消息類型，PointCloud2 的消息類型是直接應用 PCL 的　pcl::fromROSMsg() 轉換的，在傳入 caro 的數據類型裏。

然後在將數據傳入到 cartographer::mapping::TrajectoryBuilderInterface 的 AddSensorData() 的重載函數中。

cartographer/mapping/trajectory_builder_interface.h　

// This interface is used for both 2D and 3D SLAM. Implementations wire up a
// global SLAM stack, i.e. local SLAM for initial pose estimates, scan matching
// to detect loop closure, and a sparse pose graph optimization to compute
// optimized pose estimates.

用於2D和3D SLAM。實現了整體的SLAM過程，即用於　初始姿勢估計的local SLAM，用於檢測循環閉合的掃描匹配，以及用於計算優化姿勢估計的稀疏姿勢圖優化。

cartographer_ros/cartographer_ros/map_builder_bridge.h

node.cc 調用瞭如下方法，也就是進入到了 sensor_bridge.h 文件。

SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
  return sensor_bridges_.at(trajectory_id).get();
}

首先，在頭文件中定義了 TrajectoryState 結構體。內置結構體　LocalSlamData　包含了匹配之後的座標變換 local_pose，以及估計 local_pose 的時刻，以及 RangeData 數據。之後　TrajectoryState 結構體　還包含　local - map 的　座標變換，local - tracking 的座標變換（爲什麼用指針？？？）, 以及　trajectory_options.

總結，TrajectoryState　就是進行 SLAM 之後得到的　位姿座標，以及相對於 map 和 tracking frame 的座標變換。

  struct TrajectoryState {
    // Contains the trajectory state data received from local SLAM, after
    // it had processed accumulated 'range_data_in_local' and estimated
    // current 'local_pose' at 'time'.
    struct LocalSlamData {
      ::cartographer::common::Time time;
      ::cartographer::transform::Rigid3d local_pose;
      ::cartographer::sensor::RangeData range_data_in_local;
    };
    std::shared_ptr<const LocalSlamData> local_slam_data;
    cartographer::transform::Rigid3d local_to_map;
    std::unique_ptr<cartographer::transform::Rigid3d> published_to_tracking;
    TrajectoryOptions trajectory_options;
  };

RangeData 包含了一幀雷達數據的　起點座標，hit障礙物的點　returns，以及沒有掃到障礙物的點　misses ，並將 misses 用一個配置的距離代替。

// cartographer/sensor/range_data.h

// Rays begin at 'origin'. 'returns' are the points where obstructions were
// detected. 'misses' are points in the direction of rays for which no return
// was detected, and were inserted at a configured distance. It is assumed that
// between the 'origin' and 'misses' is free space.
struct RangeData {
  Eigen::Vector3f origin;
  PointCloud returns;
  PointCloud misses;
};

node.cc 的 AddTrajectory() --> map_build_bridge.h 的 AddTrajectory() --> map_builder_interface.h 的　AddTrajectoryBuilder() , 並且將 OnLocalSlamResult() 方法通過回調函數的形式註冊進去。

並且對sensor_bridges_ 進行了第0個 trajectory_id 的初始化，並將　TrajectoryBuilder　傳進去，再調用　 cartographer::mapping::TrajectoryBuilderInterface 的 AddSensorData()　方法進行傳感器數據的處理。

int MapBuilderBridge::AddTrajectory(
    const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
        expected_sensor_ids,
    const TrajectoryOptions& trajectory_options) {
  const int trajectory_id = map_builder_->AddTrajectoryBuilder(
      expected_sensor_ids, trajectory_options.trajectory_builder_options,
      ::std::bind(&MapBuilderBridge::OnLocalSlamResult, this,
                  ::std::placeholders::_1, ::std::placeholders::_2,
                  ::std::placeholders::_3, ::std::placeholders::_4,
                  ::std::placeholders::_5));
  LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";

  // Make sure there is no trajectory with 'trajectory_id' yet.
  CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
  sensor_bridges_[trajectory_id] =
      cartographer::common::make_unique<SensorBridge>(
          trajectory_options.num_subdivisions_per_laser_scan,
          trajectory_options.tracking_frame,
          node_options_.lookup_transform_timeout_sec, tf_buffer_,
          map_builder_->GetTrajectoryBuilder(trajectory_id));
  auto emplace_result =
      trajectory_options_.emplace(trajectory_id, trajectory_options);
  CHECK(emplace_result.second == true);
  return trajectory_id;
}

OnLocalSlamResult()

void MapBuilderBridge::OnLocalSlamResult(
    const int trajectory_id, const ::cartographer::common::Time time,
    const Rigid3d local_pose,
    ::cartographer::sensor::RangeData range_data_in_local,
    const std::unique_ptr<const ::cartographer::mapping::
                              TrajectoryBuilderInterface::InsertionResult>) {
  std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data =
      std::make_shared<TrajectoryState::LocalSlamData>(
          TrajectoryState::LocalSlamData{time, local_pose,
                                         std::move(range_data_in_local)});
  cartographer::common::MutexLocker lock(&mutex_);
  trajectory_state_data_[trajectory_id] = std::move(local_slam_data);
}

OnLocalSlamResult() 這個方法大致就是確定當前的　localSlamData ，然後傳入到　TrajectoryState 集合中。

OnLocalSlamResult() 的使用如下所示：( 具體的處理函數還沒找到)

// cartographer/cartographer/mapping/trajectory_builder_interface.h
  #include<functional>
  using LocalSlamResultCallback =
      std::function<void(int /* trajectory ID */, common::Time,
                         transform::Rigid3d /* local pose estimate */,
                         sensor::RangeData /* in local frame */,
                         std::unique_ptr<const InsertionResult>)>;

// cartographer/cartographer/mapping/map_builder_interface.h
  using LocalSlamResultCallback =
      TrajectoryBuilderInterface::LocalSlamResultCallback;

  // Creates a new trajectory builder and returns its index.
  virtual int AddTrajectoryBuilder(
      const std::set<SensorId>& expected_sensor_ids,
      const proto::TrajectoryBuilderOptions& trajectory_options,
      LocalSlamResultCallback local_slam_result_callback) = 0;

// cartographer/cartographer/mapping/map_builder.h
  int AddTrajectoryBuilder(
      const std::set<SensorId> &expected_sensor_ids,
      const proto::TrajectoryBuilderOptions &trajectory_options,
      LocalSlamResultCallback local_slam_result_callback) override;

// cartographer/cartographer/mapping/map_builder.cc
int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback local_slam_result_callback) {
    // ...
    trajectory_builders_.push_back(
        common::make_unique<CollatedTrajectoryBuilder>(
            sensor_collator_.get(), trajectory_id, expected_sensor_ids,
            CreateGlobalTrajectoryBuilder3D(
                std::move(local_trajectory_builder), trajectory_id,
                static_cast<PoseGraph3D*>(pose_graph_.get()),
                local_slam_result_callback)));
}

下邊３個方法讀取　map_builder_->pose_graph() 進行 rviz 的可視化。

visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {

visualization_msgs::MarkerArray MapBuilderBridge::GetLandmarkPosesList() {

visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {

剩下的方法都是調用　map_build.h中的方法，起到個　橋樑的作用。

cartographer探祕第四章之代碼解析（七）--- Cartographer_ros

cartographer_ros/cartographer_ros/node_main.cc

cartographer_ros/cartographer_ros/node.cc

cartographer_ros/cartographer_ros/sensor_bridge.h

cartographer_ros/cartographer_ros/map_builder_bridge.h

OnLocalSlamResult()

cartographer探祕第五章之 Cartographer 所用到的 C++11 新特性

karto探祕之open_karto 第四章 --- 迴環檢測與後端優化

cartographer探祕第四章之代碼解析（七）--- Cartographer_ros

cartographer探祕第四章之代碼解析（二） --- 傳感器數據處理過程

cartographer探祕第四章之代碼解析（一） --- SLAM處理過程 --- 文章索引

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結