1 理論基礎

從傳感器出發，laser的數據經兩個濾波器後，進行Scan Matching，用以構建子圖submap，新的laser scan 進來後，插入到已經維護着的子圖的適當位置。
如何去找到這個最優位姿，即通過Ceres Scan Matching實現。估計出來的laserscan的最有位姿，然後與odometry和IMU數據融合，用以估計下一時刻的位姿。

Cartographer主要理論是通過閉環檢測來消除構圖過程中產生的累積誤差。用於閉環檢測的基本單元是submap。一個submap是由一定數量的laser scan構成。將一個laser scan插入其對應的submap時，會基於submap已有的laser scan及其它傳感器數據估計其在該submap中的最佳位置。submap的創建在短時間內的誤差累積被認爲是足夠小的。然而隨着時間推移，越來越多的submap被創建後，submap間的誤差累積則會越來越大。因此需要通過閉環檢測適當的優化這些submap的位姿進而消除這些累積誤差，這就將問題轉化成一個位姿優化問題。當一個submap的構建完成時，也就是不會再有新的laser scan插入到該submap時，該submap就會加入到閉環檢測中。閉環檢測會考慮所有的已完成創建的submap。當一個新的laser scan加入到地圖中時，如果該laser scan的估計位姿與地圖中某個submap的某個laser scan的位姿比較接近的話，那麼通過某種 scan match策略就會找到該閉環。Cartographer中的scan match策略通過在新加入地圖的laser scan的估計位姿附近取一個窗口，進而在該窗口內尋找該laser scan的一個可能的匹配，如果找到了一個足夠好的匹配，則會將該匹配的閉環約束加入到位姿優化問題中。
Cartographer的重點內容:
1)融合多傳感器數據的局部submap創建
2)用於閉環檢測的scan match策略的實現

2 代碼邏輯

Google開源的代碼包含兩個部分：
**cartographer:**底層實現，cartographer主要負責處理來自Lidar、IMU、odometry的數據並基於這些數據進行地圖的構建，是cartographer理論的底層實現。
主要分爲兩個部分：
Local SLAM:也稱爲前端，該部分主要任務是建立維護submaps，但是該部分見圖誤差會隨時間累積。部分參數定義在：
/src/cartographer/configuration_files/trajectory_builder_2d.lua
/src/cartographer/configuration_files/trajectory_builder_3d.lua

Global SLAM:也稱爲後端，該部分主要任務是進行迴環檢測，閉環檢測本質是一個優化問題。

總而言之，
Local SLAM:也稱爲前端 生成較好的子圖
Global SLAM:也稱爲後端 全局優化，將不同子圖用最好的位姿貼合在一起。

**cartographer_ros:**上層應用，cartographer_ros則基於ros的通信機制獲取傳感器的數據並將它們轉換成cartographer中定義的格式傳遞給cartographer處理，與此同時也將cartographer的處理結果發佈用於顯示或保存，是基於cartographer的上層應用。

3 代碼結構

common：定義了基本數據結構以及一些工具的使用接口。

sensor：定義了雷達數據及點雲等相關的數據結構。

transform：定義了位姿的數據結構及其相關的轉換。

kalman_filter: 主要通過kalman濾波器完成對IMU、里程計及基於雷達數據的估計位姿的融合，進而估計新進的laser scan的位姿。

mapping：定義了上層應用的調用接口以及局部submap構建和基於閉環檢測的位姿優化等的接口。

mapping_2d和mapping_3d：對mapping接口的不同實現

4 mapping_2d代碼邏輯

4.1 mapping_2d:: GlobalTrajectoryBuilder

cartographer::mapping_2d::GlobalTrajectoryBuilder類主要實現了接收處理上層應用傳遞的傳感器數據的主要接口：
（1）AddImuData 用於接收處理上層應用傳遞的IMU數據。
（2）AddOdometerPose 用於接收處理上層應用傳遞的里程計數據
（3）AddHorizontalLaserFan 用於接收處理上層應用傳遞的雷達數據
其中包含重要的對象成員：

（1）cartographer::mapping_2d::LocalTrajectoryBuilder類的對象local_trajectory_builder_用於完成局部submap的構建。
（2）cartographer::mapping_2d::SparsePoseGraph類的對象sparse_pose_graph_用於完成閉環檢測及全局位姿優化。

在AddImuData和AddOdometerPose函數的實現中會將接收的相應傳感器數據傳遞給local_trajectory_builder_對象處理。在AddHorizontalLaserFan函數的實現中則將新進的laser fan傳遞給local_trajector_builder_對象用於局部submap構建，如果該laser fan被成功插入到某個submap，那麼該laser fan被插入後的相關信息則被傳遞給sparse_pose_graph_對象用於基於閉環檢測的全局位姿優化。

4.2 mapping_2d::LocalTrajectoryBuilder

cartographer::mapping_2d::LocalTrajectoryBuilder類主要完成局部submap的構建。其提供了接收處理傳感器數據的public函數：

（1） AddImuData用於處理IMU數據。

（2） AddOdometerPose用於處理里程計數據。

（3） AddHorizontalLaserFan用於處理雷達數據。

以及包含了一些重要的private成員：

（1） ScanMatch成員函數基於submap已有的laser fan估計當前laser fan在submap中的位置。

（2） cartographer::kalman_filter::PoseTracker類的對象
pose_tracker_用於融合基於雷達數據的laser fan的局部估計位姿、IMU數據以及里程計數據，進而估計出較優的laser fan的位姿。

在AddImuData和AddOdometerPose函數中會將IMU數據和里程計數據傳遞給pose_tracker_進行處理。pose_tracker通過UKF不斷融合IMU和里程計數據進而更新當前位姿，因此通過pose_tracker可以獲取當前laser fan的估計位姿的一個較好的初始化值。進一步的，在AddHorizontalLaserFan函數中會調用ScanMatch，ScanMatch函數中通過在submap中局部匹配得到的當前laser fan的估計位姿被pose_tracker_用來調整該laser fan的初始化值。這樣pose_tracker_通過融合多傳感器數據，進而能夠估計出較優的laser fan的位姿。

4.3 mapping_2d::SparsePoseGraph

artographer::mapping_2d::SparsePoseGraph類主要完成基於閉環檢測的全局位姿優化。其提供了接收處理新進被插入到submap的laser fan相關信息的public函數：

（1） AddScan 對新進的laser fan進行閉環檢測及在適當的時候進行全局優化。

以及一些重要的私有成員：

（1） ComputeConstraintsForScan對新近laser fan信息進行處理並啓動閉環檢測scan match以及計算其約束，進而將約束添加到位姿優化目標中。

（2） AddWorkItem將laser fan與ComputeConstraintsForScan綁定，並將任務加入到隊列中。

（3） HandleScanQueue依此調度隊列中的任務。

（4） sparse_pose_graph::ConstraintBuilder constraint_builder_ 用於完成laser fan的scan match以及約束計算。

（5） RunOptimization優化目標。

在AddScan函數中會將laser fan相關信息與ComputeConstraintsForScan函數綁定，並將綁定好的任務通過AddWorkItem函數加入到隊列中。HandleScanQueue函數則依次調度隊列中的任務。第一次調用AddWorkItem時會直接啓動ComputeConstraintsForScan任務，且在第一次ComputeConstraintsForScan任務時啓動HandleScanQueue調度。在ComputeConstrainsScan中，通過constraint_builder_對象完成閉環檢測的scan match以及約束計算。當所有約束計算完成時，則會進行RunOptimization優化目標。

4.4 Scan Match

LocalTrajectoryBuilder中的scan match策略與SparsePoseGraph中的scan match策略是不同的。前者使用scan_matching::RealTimeCorrelativeScanMatcher，後者則使用scan_matching::FastCorrelativeScanMatcher。二者的目標優化均是由scan_matching::CeresScanMatcher完成

5.代碼跳轉

第一部分：cartographer訂閱的各種話題的流程是什麼？
從cartographer的框架圖中，可以看出裏面有三種類型的數據，Lidar數據、Odom數據及IMU數據。
cartographer分別訂閱他們各自發出來的topic。
第一步：在ros運行時，入口函數的main（路徑: /src/cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc），其中在main()中，主要運行的函數：

int main(int argc, char** argv) {
    ...
    cartographer_ros::Run(); // 調用了同一個文件中的run函數
    ...
}

在main函數中，除了初始化glog等對象外，就是運行Run函數。Run()函數就是主要啓動的函數。其代碼如下：

void Run() {
    ...
    //Node在/cartographer_ros/cartographer_ros/cartographer/node.h中定義；
    // 在該構造函數中訂閱了很多傳感器的topic。收集傳感器數據
    // node_options 是爲了node配置的一個類
    // map_builder 是 cartographer裏面的一個類
    Node node(node_options, std::move(map_builder), &tf_buffer);
    ..
}

在Run函數中，主要與訂閱相關實例化Node類
ROS程序當中，主要是以節點的形式進行存在。所以工程師們想用一個類來代表這個ROS節點，進行處理ROS節點事務。
node_options，是另外一個類，是用來表示節點的選項的一個類。
map_builder，是另外一個類，是同來表示地圖構造的，雖然看過他的函數，但仍然不知道他具體的東西。
tf_buffer，是tf樹的一個緩衝區。
第二步：Node類節點，在 src/cartographer_ros/cartographer_ros/cartographer_ros/node.h 裏面進行定義的.
該類中有訂閱的相關代碼，一定擁有ros::Subscriber信息。可以看到代碼中有跟訂閱相關的變量和函數：

namespace cartographer_ros {
class Node {    
private:
   struct Subscriber 
   { // Subscriber 構成的新的訂閱pair似的數據結構
       ::ros::Subscriber subscriber; // ros訂閱用法的類
       std::string topic;  // 主題的名字
   }
   ....
   // 因爲調用 AddTrajectory
  bool HandleStartTrajectory( 
      cartographer_ros_msgs::StartTrajectory::Request& request,
      cartographer_ros_msgs::StartTrajectory::Response& response);
  // 從名字看，就知道和訂閱有關。
  void LaunchSubscribers(const TrajectoryOptions& options,
                         const cartographer_ros_msgs::SensorTopics& topics,
                         int trajectory_id);
  // 會調用 LaunchSubscribers
  int Node::AddTrajectory(const TrajectoryOptions& options,
                        const cartographer_ros_msgs::SensorTopics& topics)
  .... 
 // 用服務的形式管理所有第一個 trajectory                      
  std::vector<::ros::ServiceServer> service_servers_;
  // 再根據 Subscriber， 會搜索到這個變量。用一個unordered_map來進行組織
  // int 類型，我看了後面知道這是一個 trajectory_id 
 // 後面是一個vector組成Subscriber
  std::unordered_map<int, std::vector<Subscriber>> subscribers_;
  ...
}    
}

ros::Subscriber 開始，一路順延下去進行挖掘。能夠得出這些代碼都是訂閱相關的。
在後面，會講解每個函數都做了什麼，會解釋每一個函數當中與訂閱相關的代碼。

第三步：初始化函數裏面的東西很多，找到相關的代碼就OK了。但真正的流程是從Node的初始化函數開始的。
主要與訂閱數據相關的代碼如下：

Node::Node(
    const NodeOptions& node_options,    // 選項
    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,    // 地圖匹配
    tf2_ros::Buffer* const tf_buffer)     // tf 樹
    : node_options_(node_options),
      map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) {
    ...
    // node_constants.h 裏面定義了下面的常量
    // constexpr char kStartTrajectoryServiceName[] = "start_trajectory";
    // 將 HandleStartTrajectory 作爲服務的回調函數, 那麼問題來了, 哪裏調用了這個服務? 
   // 這個我還不確切知道. 但以我自己的感覺, 是在Mapbuilder那邊,開始添加trajectory的時候.
    service_servers_.push_back(node_handle_.advertiseService(
      kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this)); 
   ...   
}

從上述代碼可以看出，初始化函數裏面vector service_servers_進行了服務的管理，它壓入一個新增的名爲KStartTrajectoryServiceName的服務。這個服務的回調函數HandleStartTrajectory。因此，當此服務被調用時，就會啓動HandleStartTrajectory這個函數。接下來我們開始去探索這個HandleStarTrajectory做了什麼事情。

第四步：HandleStartTrajectory的主要代碼如下，因爲它是ROS規定的回調函數，它有自己的參數格式。
request 表達的是調用這個服務的請求的數據結構
response 就是自定義的回覆的格式

bool Node::HandleStartTrajectory(
    ::cartographer_ros_msgs::StartTrajectory::Request& request,
    ::cartographer_ros_msgs::StartTrajectory::Response& response) {
  carto::common::MutexLocker lock(&mutex_);
  TrajectoryOptions options;
  // 判斷從ROS消息裏面讀取出options, 並且,這個options 是否是有效的
  if (!FromRosMessage(request.options, &options) ||
      !ValidateTrajectoryOptions(options)) {
          // 無效的option
    const std::string error = "Invalid trajectory options.";
    LOG(ERROR) << error;
    response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
    response.status.message = error;
  } else if (!ValidateTopicNames(request.topics, options)) { // 判斷是否無效的topic name
      // 無效的topic name
    const std::string error = "Invalid topics.";
    LOG(ERROR) << error;
    response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
    response.status.message = error;
  } else {
      // 如果都成功, 所以就會添加這個主題
    response.trajectory_id = AddTrajectory(options, request.topics);
    response.status.code = cartographer_ros_msgs::StatusCode::OK;
    response.status.message = "Success.";
  }
  return true;
}

從上述代碼中看到，當判斷當前情況爲正常後，使用AddTrajectory來添加第一個trajector。在調用AddTrajectory的時候，是通過request.topics來定義需要調用哪些話題。接下來進入到AddTrajectory函數。

第五步：AddTrajectory 裏面的代碼如下：

int Node::AddTrajectory(const TrajectoryOptions& options,
                        const cartographer_ros_msgs::SensorTopics& topics) {
  // 根據options, 得出一個SensorID的set
  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options, topics); 
  // 根據sensor id 和 option, 得出 trajectory id.    
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
  // Extrapolator 是一個推算位置的類, 因爲我看過後面, 跟我想要解決的數據流入沒關係
  AddExtrapolator(trajectory_id, options);
  // sensor_samplers_ 同樣也是一個抽樣的類, 跟數據流入沒關係
  AddSensorSamplers(trajectory_id, options);
  // LaunchSubscribers 訂閱所有話題的主要函數
  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;
}

Extrapolator是一個推算位置的類，在概率機器人裏面，提到兩種方法推算機器人位置：採樣法和根據當前的速度推算的方法。本文采用的就是基於速度來推算下一個位置的類

第六步：LaunchSubscribers的代碼如下：

// 在這兒，訂閱了一堆東西。
void Node::LaunchSubscribers(const TrajectoryOptions& options,
                             const cartographer_ros_msgs::SensorTopics& topics,
                             const int trajectory_id) {
  // ComputeRepeatedTopicNames 作用: 確保傳進去的topic,出來之後是唯一的.
  for (const std::string& topic : ComputeRepeatedTopicNames(
           topics.laser_scan_topic, options.num_laser_scans)) {
    // SubscribeWithHandler 的作用: 
    // 使用'node_handle'爲'trajectory_id'訂閱'topic'，並在'node'上調用'handler'來處理消息。 返回訂閱者。          
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::LaserScan>(
             &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
             this),
         topic});
  }
  for (const std::string& topic :
       ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
                                 options.num_multi_echo_laser_scans)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>(
             &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
             &node_handle_, this),
         topic});
  }
  for (const std::string& topic : ComputeRepeatedTopicNames(
           topics.point_cloud2_topic, options.num_point_clouds)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::PointCloud2>(
             &Node::HandlePointCloud2Message, trajectory_id, topic,
             &node_handle_, this),
         topic});
  }

  // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
  // required.
  if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
      (node_options_.map_builder_options.use_trajectory_builder_2d() &&
       options.trajectory_builder_options.trajectory_builder_2d_options()
           .use_imu_data())) {
    std::string topic = topics.imu_topic;
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
                                                trajectory_id, topic,
                                                &node_handle_, this),
         topic});
  }

  if (options.use_odometry) {
    std::string topic = topics.odometry_topic;
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
                                                  trajectory_id, topic,
                                                  &node_handle_, this),
         topic});
  }
  if (options.use_nav_sat) {
    std::string topic = topics.nav_sat_fix_topic;
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::NavSatFix>(
             &Node::HandleNavSatFixMessage, trajectory_id, topic, &node_handle_,
             this),
         topic});
  }
  if (options.use_landmarks) {
    std::string topic = topics.landmark_topic;
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>(
             &Node::HandleLandmarkMessage, trajectory_id, topic, &node_handle_,
             this),
         topic});
  }
}

根據配置所配置的名稱，來進行訂閱話題。
在這些代碼裏面，C++用法以及不清楚具體的目的，因爲不確定裏面到底輸出的是什麼東西以及輸出東西的含義。有一個相關函數如下：

std::vector<std::string> ComputeRepeatedTopicNames(const std::string& topic,
                                                   const int num_topics) {
  CHECK_GE(num_topics, 0);
  if (num_topics == 1) {
    return {topic};
  }
  std::vector<std::string> topics;
  topics.reserve(num_topics);
  for (int i = 0; i < num_topics; ++i) {
    topics.emplace_back(topic + "_" + std::to_string(i + 1));
  }
  return topics;
}

從函數裏面可以看到，如果輸入的名字一樣，但是有多個雷達，那麼就會加上_這樣的格式。所以，這裏只是生成話題，然後進行了訂閱，然而這些topic怎麼來？當然是有一定的相關的默認配置，應該也是可以通過配置文件進行配置。默認配置選項如下：

cartographer_ros_msgs::SensorTopics DefaultSensorTopics() {
  cartographer_ros_msgs::SensorTopics topics;
  topics.laser_scan_topic = kLaserScanTopic;
  topics.multi_echo_laser_scan_topic = kMultiEchoLaserScanTopic;
  topics.point_cloud2_topic = kPointCloud2Topic;
  topics.imu_topic = kImuTopic;
  topics.odometry_topic = kOdometryTopic;
  topics.nav_sat_fix_topic = kNavSatFixTopic;
  topics.landmark_topic = kLandmarkTopic;
  return topics;
}

KLaserScanTopic這些名字，定義在node_constants.h裏面。

第七步：與主線關係不大，訂閱的在上面已經講清楚，這兒是我在上面有個函數每看明白，所以列出來了。
函數是 SubscribeWithHandle 這個函數：

template <typename MessageType>
::ros::Subscriber SubscribeWithHandler(
    void (Node::*handler)(int, const std::string&,
                          const typename MessageType::ConstPtr&),
    const int trajectory_id, const std::string& topic,
    ::ros::NodeHandle* const node_handle, Node* const node) {
  return node_handle->subscribe<MessageType>(
      topic, kInfiniteSubscriberQueueSize,
      boost::function<void(const typename MessageType::ConstPtr&)>(
          [node, handler, trajectory_id,
           topic](const typename MessageType::ConstPtr& msg) {
            (node->*handler)(trajectory_id, topic, msg);
          }));
}

2D SLAM_Cartographer（2）_Google Cartographer 之思想解讀與分析

1 理論基礎

2 代碼邏輯

3 代碼結構

4 mapping_2d代碼邏輯

4.1 mapping_2d:: GlobalTrajectoryBuilder

4.2 mapping_2d::LocalTrajectoryBuilder

4.3 mapping_2d::SparsePoseGraph

4.4 Scan Match

5.代碼跳轉

2D SLAM_Gmapping（3）_從Gmapping建圖到Amcl定位中地圖的變換過程

Linux知識點（8）_ROS下如何利用CMakeLists.txt進行多目錄工程管理？

數據預處理（17）_座標轉換，tf::StampedTransform =」 Eigen::Matrix4f

C++知識點（13）_返回對象用“引用“還是“值“?

C++知識點（7）_istringstream文件流

Mac下配置sublime實現LaTeX

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

linux以太網驅動總結