1. 基礎

1.1 軟件架構

1.2 座標系及相互關係

參考定義見：backpack_3d.lua
map_frame = “map”：cartographer中使用的全局座標系，最好保持默認，否則ROS的Rviz不認識其它的定義，導致無法可視化建圖過程。
tracking_frame=”base_link”：機器人中心座標系，其它傳感器數據都是以這個爲基礎進行插入的，它是整個SLAM系統的核心座標系；cartographer_ros裏面有個tf_bridge的類就是專門用來查詢其它座標系到此座標系的轉換關係。
published_frame = “base_link”
odom_frame = “odom” ：published_frame與odom_frame配合使用，如果參數provide_odom_frame = true 那麼最後可視化時，發佈的轉換消息是從 published_frame->odom_frame->map_frame, 也即cartographer內部計算出了未經迴環檢測的局部圖座標到優化後的全局圖座標之間的轉換關係併發布了出來。在跑官網的二維揹包例子時，在map座標周圍一直跳動的odom就是這個玩意。

1.3 調試環境

安裝 ROS Kinetic
http://wiki.ros.org/kinetic/Installation/Ubuntu
Build and install proto3.
https://google-cartographer.readthedocs.io/en/latest/ (安裝完之後重啓系統)
或
https://github.com/google/protobuf/blob/master/src/README.md
Cartographer ROS for TurtleBots (cartographer, cartographer_ros, cartographer_turtlebot)
https://google-cartographer-ros-for-turtlebots.readthedocs.io/en/latest/
下載TurtleBot3
cd ~/burger_ws/src/
git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
git clone https://github.com/ROBOTIS-GIT/turtlebot3.git
cd ~/catkin_ws && catkin_make
Public Data
http://google-cartographer-ros.readthedocs.io/en/latest/data.html#public-data
Resource
- Real-time indoor SLAM with glass detection
  https://github.com/uts-magic-lab/slam_glass
- Cartographer Public Data
  https://google-cartographer-ros.readthedocs.io/en/latest/data.html
- cartographer_hector_tracker
  It is an example of 3D SLAM with a tilting 2D lidar on a mobile robot
  https://github.com/tu-darmstadt-ros-pkg/cartographer_hector_tracker
- Bag validation tool
  $ rosrun cartographer_ros cartographer_rosbag_validate -bag_filename ~/Downloads/2017-10-17-08-59-28.bag
- VeloView performs real-time visualization of live captured 3D LiDAR data from Velodyne’s HDL sensors (HDL-64E, HDL-32E, and VLP-16)
  http://www.paraview.org/VeloView/
  https://github.com/Kitware/VeloView

2. Cartographer主要貢獻

主要目的：減小計算閉環檢測（Loop Closure）的資源消耗，以達到實時應用的目的，不追求高精度（可以達到r=5cm級別的精度）
主要思想: 通過閉環檢測來消除構圖過程中產生的累積誤差，用於閉環檢測的基本單元是Submap。
重點：
- 融合多傳感器數據以創建局部Submap
- 用於閉環檢測的Scan Matching策略的實現

3. 掃描匹配(Scan-Matching）

ICP：Iterative Closest Point
ICL：Iterative Closest Line

3.1 Scan-to-Submap

代碼實現：

std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
    const common::Time time, const transform::Rigid2d& pose_prediction,
    const sensor::RangeData& gravity_aligned_range_data) {
  std::shared_ptr<const Submap2D> matching_submap =
      active_submaps_.submaps().front();
  // The online correlative scan matcher will refine the initial estimate for
  // the Ceres scan matcher.
  transform::Rigid2d initial_ceres_pose = pose_prediction;
  sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
      options_.adaptive_voxel_filter_options());
  // voxel filter
  const sensor::PointCloud filtered_gravity_aligned_point_cloud =
      adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
  if (filtered_gravity_aligned_point_cloud.empty()) {
    return nullptr;
  }
  // RTCSM scan matching refine the pose_prediction that is from
  // IMU/Odom/Kinetics for Ceres scan matcher
  if (options_.use_online_correlative_scan_matching()) {
    CHECK_EQ(options_.submaps_options().grid_options_2d().grid_type(),
             proto::GridOptions2D_GridType_PROBABILITY_GRID);
    double score = real_time_correlative_scan_matcher_.Match(
        pose_prediction, filtered_gravity_aligned_point_cloud,
        *static_cast<const ProbabilityGrid*>(matching_submap->grid()),
        &initial_ceres_pose);
    kFastCorrelativeScanMatcherScoreMetric->Observe(score);
  }
  auto pose_observation = common::make_unique<transform::Rigid2d>();
  ceres::Solver::Summary summary;
  // Ceres scan matcher (Non-linear Least Squares)
  ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
                            filtered_gravity_aligned_point_cloud,
                            *matching_submap->grid(), pose_observation.get(),
                            &summary);
  if (pose_observation) {
    kCeresScanMatcherCostMetric->Observe(summary.final_cost);
    double residual_distance =
        (pose_observation->translation() - pose_prediction.translation())
            .norm();
    kScanMatcherResidualDistanceMetric->Observe(residual_distance);
    double residual_angle = std::abs(pose_observation->rotation().angle() -
                                     pose_prediction.rotation().angle());
    kScanMatcherResidualAngleMetric->Observe(residual_angle);
  }
  return pose_observation;
}

3.1.1 RTCSM: Real-Time Correlative Scan Matching (by Olson)

方法：把求兩次位姿間的剛體變換轉換爲一個概率問題：找到一個剛體變換（即機器人的新位姿）使用觀測數據出現的概率最大。
方法貢獻：提供了一種高效計算 $p (z | x_{i}, m)$ 的方法。
目標：基於機器人的當前Pose, 求後驗概率分佈

$p (x_{i} | x_{i - 1}, u, m, z ｝$
- $x_{i - 1}$ ：前一個已求的Pose
- $u$ ：機器人的運動參數
- $m$ : 環境模型（environment model）
- $x$ ：激光掃描點(laser scan observation）
目標簡化：應用Bayes規則並刪除不相關的條件

$p (x_{i} | x_{i - 1}, u, m, z ｝ \propto p (z | x_{i}, m) p (x_{i} | x_{i - 1}, u)$
- $p (z | x_{i}, m)$ ：觀測模型（observation model），表示環境模型和機器人的Pose已知的情況下，我們有多大概率可以看到這些數據，其計算是複雜的，且有多個極值
- $p (x_{i} | x_{i - 1}, u)$ ：運動模型（motion model），根據IMU/Odom或控制輸入獲得，是一個多變量高斯分佈（multivariate Guassian Distribution）
方法輸出：
- 更健壯的最大似然估計(robust maximum likelihood estimation）
- 不確定性估計(principled uncertainty estimate）
計算觀測模型（假設Lidar 採樣點是相互獨立的）

$p (z | x_{i}, m) = \prod_{j} p (z_{j} | x_{i}, m)$
- 本方法中，觀測模型 $m$ 定義上一幀Laser scan（Reference scan），而在Cartographer中則定義爲Submap(由多個Laser scans組成)
- $p (z_{j} | x_{i}, m)$ ：近似定義爲 $z_{j}$ 到觀測模型(map $m$ ）任何表面的距離
- 柵格化概率格子
加速計算 ：對於許多不同的候選 ,計算，從而找出最優的
- 多分辨率方法（Multi-Level Resolution Implementation）
  - 第一步：創建兩個map, 一個低分辨率（如30cm），一個高分辨率（如3cm）
  - 第二步：在低分辨率的map中找到概率最大的區域
  - 第三步：在高分辨率的map中的上面確定的區域內尋找最大值，這樣確保是全局最大值，而不是局部極大值
- GPU加速計算
計算協方差：評估Scan Matcher的不確定性
- 當 $x_{i}$ 的最優值被估計之後，可以用多變量高斯分佈來擬合這些數據，設 $x_{i}^{(j)}$ 是 $x_{i}$ 的第 $j$ 次估計
  $K = \sum_{j} x_{i}^{(j)} x_{i}^{(j)^{T}} p (x_{i}^{(j)} | x_{i - 1}, u, m, z)$
  $u = \sum_{j} x_{i}^{(j)} p (x_{i}^{(j)} | x_{i - 1}, u, m, z)$
  $s = \sum_{j} p (x_{i}^{(j)} | x_{i - 1}, u, m, z)$
  $Σ_{x_{i}} = \frac{1}{s} K - \frac{1}{s^{2}} u u^{T}$
- 根據估計Scan Matcher的不確定性主要考慮兩個不確定性因素：
  - 傳感器本身的噪聲
  - 哪些查詢點與map的哪部分相關的不確定性
- 不確定性圖形示例

4. 關鍵流程

4.1 IMU消息處理流程

Node::HandleImuMessage->
  SensorBridge::HandleImuMessage->
    CollatedTrajectoryBuilder::AddSensorData->
      CollatedTrajectoryBuilder::AddData->
        TrajectoryCollator::AddSensorData->
          //把傳感器數據放在隊列中
          //TrajectoryCollator(std::unordered_map<int, OrderedMultiQueue> trajectory_to_queue_)
          //trajectory_to_queue_.at(trajectory_id).Add(std::move(queue_key), std::move(data))      
           OrderedMultiQueue::Add(const QueueKey& queue_key, std::unique_ptr<Data> data)->
             OrderedMultiQueue::Dispatch()->
               //callback爲CollatedTrajectoryBuilder::HandleCollatedSensorData 
               //見CollatedTrajectoryBuilder::CollatedTrajectoryBuilder
               callback(..) ->
                 Dispatchable::AddToTrajectoryBuilder（TrajectoryBuilderInterface*）->
                   GlobalTracjectoryBuilder::addSensorData(sensor_id, Sensordata)

4.2 Laser Scan(2D)消息處理流程

包括Scan Matching、Insert Submap, 關鍵函數 LocalTrajectoryBuilder2D::AddAccumulatedRangeData。

GlobalTrajectoryBuilder::AddSensorData(const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data)
  std::unique_ptr<MatchingResult> LocalTrajectoryBuilder2D::AddRangeData(const std::string& sensor_id,const sensor::TimedPointCloudData& range_data)
     TimedPointCloudOriginData RangeDataCollator::AddRangeData(string& sensor_id,TimedPointCloudData& timed_point_cloud_data)
     //把一個點雲數據加入到vector中，以蒐集一幀數據
     RangeData LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter
       sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.returns)
     MatchingResult LocalTrajectoryBuilder2D::AddAccumulatedRangeData(Time,RangeData&, Rigid3d& gravity_alignment) (***)
       -Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) (得用IMU進行旋轉、Odom進行平移預測,從而獲得新的Pose)
       -Rigid2d LocalTrajectoryBuilder2D::ScanMatch(Time time, Rigid2d& pose_prediction,RangeData& gravity_aligned_range_data) (****)
         PointCloud AdaptiveVoxelFilter::Filter(const PointCloud& point_cloud) (體素濾波)
           // Aligns 'point_cloud' within the 'grid' given an initial_pose_estimate'.  
           // Scan-to-Submap匹配 (RealTimeCorrelativeScanMatcher2D) 
         double RealTimeCorrelativeScanMatcher2D::Match(Rigid2d& initial_pose_estimate, //輸入  @函數返回分數
                                                        PointCloud& point_cloud,   //點雲數據
                                                        ProbabilityGrid& probability_grid, //submap grid
                                                        Rigid2d* pose_estimate) //輸出新的Pose
              // Generates a collection of rotated scans.
           vector<PointCloud> GenerateRotatedScans(PointCloud& point_cloud,SearchParameters& search_parameters)
              // Translates and discretizes the rotated scans into a vector of integer indices.
           vector<DiscreteScan2D> DiscretizeScans(MapLimits& map_limits,vector<sensor::PointCloud>& scans,Translation2f& initial_translation)
              // 在搜索範圍內均勻生成候選者
           vector<Candidate2D> RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates(SearchParameters& search_parameters) 
              //爲每個候選者打分(Computes the score for each Candidate2D in a collection. The cost is computed as the sum of probabilities,)
           void RealTimeCorrelativeScanMatcher2D::ScoreCandidates(ProbabilityGrid& probability_grid,
                                                                  vector<DiscreteScan2D>& discrete_scans,
                                                                  SearchParameters& search_parameters,
                                                                  vector<Candidate2D>* const candidates)
           選一個分數最高的候選者Pose返回
         // 創建三個ResidualBlock<空間佔用、平移、旋轉>然後利用ceres求解最優pose_estimate
         // 根據Odom預測值和RTCSM估計值的殘差進行優化求解
         void CeresScanMatcher2D::Match(Eigen::Vector2d& target_translation,       //通過IMU, Odom運動預測的值
                                        transform::Rigid2d& initial_pose_estimate, //RTCSM所估計的最佳值 (scan-to-map)
                                        sensor::PointCloud& point_cloud,
                                        Grid2D& grid,
                                        transform::Rigid2d* const pose_estimate,   //優化的輸出Pose結果
                                        ceres::Solver::Summary* const summary)  
             //基於一個pose,計算從'point_cloud'匹配到'grid'的cost， 殘差個數與點雲個數一致
           CostFunction* OccupiedSpaceCostFunction2D::CreateAutoDiffCostFunction 
             //計算 the cost of translating 'pose' to 'target_translation', 兩個殘差(x,y)
           CostFunction* TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction
             //計算 the cost of rotating 'pose' to 'target_angle'，一個殘差（偏航角之差）
           CostFunction* RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction
       -void PoseExtrapolator::AddPose(const common::Time time,const transform::Rigid3d& pose)
       -std::unique_ptr<InsertionResult>LocalTrajectoryBuilder2D::InsertIntoSubmap(  //***把Laser Scan插入submap
                                      const common::Time time, const sensor::RangeData& range_data_in_local,
                                      const sensor::RangeData& gravity_aligned_range_data,
                                      const transform::Rigid3d& pose_estimate,
                                      const Eigen::Quaterniond& gravity_alignment)
         void ActiveSubmaps2D::InsertRangeData(const sensor::RangeData& range_data)
           void Submap2D::InsertRangeData(RangeData& range_data,RangeDataInserterInterface* range_data_inserter)           
             void ProbabilityGridRangeDataInserter2D::Insert(RangeData& range_data, GridInterface* const grid) // grid is ProbabilityGrid
               void CastRays(RangeData& range_data,vector<uint16>& hit_table,vector<uint16>& miss_table,
                             insert_free_space,ProbabilityGrid* const probability_grid)

4.3 Laser Scan(3D)消息處理流程

GlobalTrajectoryBuilder::AddSensorData(  string& sensor_id,sensor::TimedPointCloudData& timed_point_cloud_data)->
  *LocalTrajectoryBuilder3D::AddRangeData(string& sensor_id,sensor::TimedPointCloudData& unsynchronized_data)->
    TimedPointCloudOriginData RangeDataCollator::AddRangeData(string& sensor_id,sensor::TimedPointCloudData& timed_point_cloud_data)
    LocalTrajectoryBuilder3D::AddAccumulatedRangeData(Time time, sensor::RangeData& filtered_range_data_in_tracking)-> (***)
      Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time)
      float RealTimeCorrelativeScanMatcher3D::Match(const transform::Rigid3d& initial_pose_estimate,const sensor::PointCloud& point_cloud,
                                                    const HybridGrid& hybrid_grid,transform::Rigid3d* pose_estimate) // 暴力匹配
      void CeresScanMatcher3D::Match(const Eigen::Vector3d& target_translation,  const transform::Rigid3d& initial_pose_estimate,
                                     const std::vector<PointCloudAndHybridGridPointers>&  point_clouds_and_hybrid_grids,
                                     transform::Rigid3d* const pose_estimate, ceres::Solver::Summary* const summary)
                                     // 每個點雲數據有兩個殘差：高低精度（點雲->地圖），根據概率進行計算
                                     // 點雲殘差=scaling_factor * (1-probability)
                                     // 平移3個殘差，旋轉3個殘差
      void PoseExtrapolator::AddPose(const common::Time time,const transform::Rigid3d& pose) // 把新的Pose加入雙向隊列
      std::unique_ptr<LocalTrajectoryBuilder3D::InsertionResult> LocalTrajectoryBuilder3D::InsertIntoSubmap(
                                         const common::Time time,
                                         const sensor::RangeData& filtered_range_data_in_local,
                                         const sensor::RangeData& filtered_range_data_in_tracking,
                                         const sensor::PointCloud& high_resolution_point_cloud_in_tracking,
                                         const sensor::PointCloud& low_resolution_point_cloud_in_tracking,
                                         const transform::Rigid3d& pose_estimate,
                                         const Eigen::Quaterniond& gravity_alignment)->
        void ActiveSubmaps3D::InsertRangeData(sensor::RangeData& range_data,Eigen::Quaterniond& gravity_alignment)->
          void Submap3D::InsertRangeData(const sensor::RangeData& range_data,RangeDataInserter3D& range_data_inserter,
                                    const int high_resolution_max_range)->
            void RangeDataInserter3D::Insert(const sensor::RangeData& range_data,HybridGrid* hybrid_grid)->
                                         // 調用兩次，分別把RangeData插入高/低精度HybridGrid 
               bool ApplyLookupTable(const Eigen::Array3i& index, const std::vector<uint16>& table)
                                         // 通過查表方法更新Cell的概率值[1,32767],與概率對應的值（即歸一化值）
  *NodeId PoseGraph3D::AddNode(std::shared_ptr<const TrajectoryNode::Data> constant_data, const int trajectory_id,
                               std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps)->
    PoseGraphData.Append   //1) 爲此Trajectory增加TrajectoryNode
    PoseGraphData.Append   //2) 如果對應的Submap3D沒有被增加到軌跡中，則增加到軌跡中
   *PoseGraph3D::ComputeConstraintsForNode(NodeId& node_id,vector<shared_ptr<const Submap3D>> insertion_submaps,
                              bool newly_finished_submap) //3) 爲新增加的節點計算約束   
      vector<SubmapId> PoseGraph3D::InitializeGlobalSubmapPoses（...）-> // 獲取新插入的兩個Submap的SubMapId
        OptimizationProblem3D::AddSubmap(int trajectory_id, transform::Rigid3d& global_submap_pose)  //把Submap的全局位姿增加到優化器中
      根據新節點的局部位姿計算其全局位姿 
      OptimizationProblem3D::AddTrajectoryNode(int trajectory_id,NodeSpec3D& node_data) // 把新Node的局部和全局位姿增加到優化器中  
      PoseGraphData.constraints.push_back(..) //計算新節點與每個插入子圖（2個）間的約束變換，然後增加到約束列表中
      PoseGraph3D::ComputeConstraint(NodeId& node_id,SubmapId& submap_id) //計算新節點與以前每個Submap的約束變換
        ConstraintBuilder3D::MaybeAddConstraint(...)
          ConstraintBuilder3D::ComputeConstraint(...) //計算(submap i <- node j) 的約束變換
            unique_ptr<FastCorrelativeScanMatcher3D::Result>FastCorrelativeScanMatcher3D::Match(...)
            或
            unique_ptr<FastCorrelativeScanMatcher3D::Result> FastCorrelativeScanMatcher3D::Match(...)->
              unique_ptr<FastCorrelativeScanMatcher3D::Result>FastCorrelativeScanMatcher3D::MatchWithSearchParameters(...)->
                Candidate3D FastCorrelativeScanMatcher3D::BranchAndBound(...)
            CeresScanMatcher3D::Match(...) 
        或
        ConstraintBuilder3D::MaybeAddGlobalConstraint(...)
      PoseGraph3D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) //計算以前加入的Nodes與新加入的Submap間的約束
      PoseGraph3D::DispatchOptimization() // 閉環之後，當節點增加到90 （optimize_every_n_nodes in pose_graph.lua），

4.3.1 FastCorrelativeScanMatcher3D::MatchFullSubmap

輸入：
- Node(LaserScan)在global frame中的旋轉
- Submap在global frame中的旋轉
- Node的點雲數據
輸出：匹配結果

  struct Result {
    float score;
    transform::Rigid3d pose_estimate;
    float rotational_score;
    float low_resolution_score;
  };

功能：在指定的立方體內搜索得分最高的位姿(存放在Result.pose_estimate)中
搜索立方體的邊長：設定邊長的一半 + 點雲中的最遠點的距離
每個體素的搜索角度：180度

  struct SearchParameters {
    const int linear_xy_window_size;     // voxels
    const int linear_z_window_size;      // voxels
    const double angular_search_window;  // radians
    const MatchingFunction* const low_resolution_matcher;
  };
  const int linear_window_size =
      (width_in_voxels_ + 1) / 2 +
      common::RoundToInt(max_point_distance / resolution_ + 0.5f);
  const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
      low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud);
  const SearchParameters search_parameters{
      linear_window_size, linear_window_size, M_PI, &low_resolution_matcher};

執行搜索：FastCorrelativeScanMatcher3D::MatchWithSearchParameters

4.3.2 FastCorrelativeScanMatcher3D::MatchWithSearchParameters

輸入：
- 搜索空間
- Node(LaserScan)在global frame中的旋轉
- Submap在global frame中的旋轉
- Node的點雲數據
- 統計直方圖
- 重力方向
- 最小分
輸出：匹配結果
功能：
- 生成離散的3D掃描點:GenerateDiscreteScans

struct DiscreteScan3D {
  transform::Rigid3f pose;  // 此點雲對應Node的位姿
  // Contains a vector of discretized scans for each 'depth'.
  std::vector<std::vector<Eigen::Array3i>> cell_indices_per_depth;
  float rotational_score; //此角度旋轉匹配得分,通過匹配統計直方圖而得（向量點乘）
};

生成Candidate3D: ComputeLowestResolutionCandidates

struct Candidate3D {
  Candidate3D(const int scan_index, const Eigen::Array3i& offset)
      : scan_index(scan_index), offset(offset) {}

  static Candidate3D Unsuccessful() {
    return Candidate3D(0, Eigen::Array3i::Zero());
  }

  // Index into the discrete scans vectors.
  // 對應搜索角度
  int scan_index;

  // Linear offset from the initial pose in cell indices. For lower resolution
  // candidates this is the lowest offset of the 2^depth x 2^depth x 2^depth
  // block of possibilities.
  Eigen::Array3i offset; // 對應空間搜索位置索引

  // Score, higher is better.
  float score = -std::numeric_limits<float>::infinity();

  // Score of the low resolution matcher.
  float low_resolution_score = 0.f;

  bool operator<(const Candidate3D& other) const { return score < other.score; }
  bool operator>(const Candidate3D& other) const { return score > other.score; }
};

4.3.2 FastCorrelativeScanMatcher3D::GenerateDiscreteScans

輸出：std::vector< DiscreteScan3D>
功能：
- 根據點雲中最遠點的距離及搜索角度窗口，計算每個需要嘗試匹配的角度
- 把節點的全局座標轉換成submap的局部座標
- 把Node(LaserScan)直方圖與Submap直方圖進行匹配，每個角度都進行一次匹配並得到一個分數

   // 計算每個搜索角度的匹配得分
   std::vector<float> RotationalScanMatcher::Match(Eigen::VectorXf& histogram,float initial_angle,std::vector<float>& angles) ->
     // 按角度旋轉直方圖 
     Eigen::VectorXf RotateHistogram(const Eigen::VectorXf& histogram,const float angle) 
     // 通過點乘計算兩個直方圖的相似性，越相似，分數越高
     float MatchHistograms(Eigen::VectorXf& submap_histogram, Eigen::VectorXf& scan_histogram)

根據每個搜索角度計算點雲的新Pose, 再加上搜索參數、點雲、此角度的匹配分數，生成一個DiscreteScan3D (每上搜索角度一個DiscreteScan3D)，實現函數：DiscreteScan3D FastCorrelativeScanMatcher3D::DiscretizeScan，從而輸出std::vector
爲每個搜索角度生成一個DiscreteScan3D

4.3.3 DiscreteScan3D FastCorrelativeScanMatcher3D::DiscretizeScan

輸出：DiscreteScan3D
功能：
- PrecomputationGrid3D是HybridGrid的8位值版本，PrecomputationGrid3D使用8位表示概率值，而HybridGrid使用16位值表示概率
- 根據全分辨率各低分辨率的數量(深度數)，爲每一層生成所有激光點在PrecomputationGrid3D中的位置索引
- 最後把點雲的位姿、所有層的激光點索引、在此角度的旋轉得分生成一個DiscreteScan3D

DiscreteScan3D{pose, cell_indices_per_depth, rotational_score};

4.3.4 Eigen::VectorXf RotationalScanMatcher::ComputeHistogram

Eigen::VectorXf RotationalScanMatcher::ComputeHistogram(
    const sensor::PointCloud& point_cloud, const int histogram_size)

功能：計算一個點雲的直方圖
直方圖定義：
- bucket index：當前激光點到最後一個參考激光點的連線的斜率對應的角度的離散值
- bucket value：激光點的值的累加，此值定義爲：“當前點與參考點的連線”與“當前點與質心點的連線”的同向程度，同向爲0，垂直最大
直方圖計算方法：
- 把激光點按其 $z$ 值範圍（離散化）放入不同的slice中（每個Slice中的點位於一個圓上）
- 對於每個Slice, 計算當前點與參考點的連線所形成的斜率，然後計算出斜率對應的角度 $θ$ ，此角度用於對應到一個bucket，當“當前點與參考點的距離大於閾值”，則當前點設置爲參考點
- 計算“當前點與參考點的連線”與“當前點與質心點的連線”的同向程度(向量 $a 与 b$ 的點乘)，同向爲0，垂直最大，此值增加到直方圖的bucket value值
- 參考示意如下圖所示：
- 代碼實現

void AddPointCloudSliceToHistogram(const sensor::PointCloud& slice,
                                   Eigen::VectorXf* const histogram) {
  if (slice.empty()) {
    return;
  }
  // We compute the angle of the ray from a point to the centroid of the whole
  // point cloud. If it is orthogonal to the angle we compute between points, we
  // will add the angle between points to the histogram with the maximum weight.
  // This is to reject, e.g., the angles observed on the ceiling and floor.
  const Eigen::Vector3f centroid = ComputeCentroid(slice);

  Eigen::Vector3f last_point = slice.front();
  for (const Eigen::Vector3f& point : slice) {
    const Eigen::Vector2f delta = (point - last_point).head<2>();
    const Eigen::Vector2f direction = (point - centroid).head<2>();
    const float distance = delta.norm();
    if (distance < kMinDistance || direction.norm() < kMinDistance) {
      continue;
    }
    if (distance > kMaxDistance) {
      last_point = point;
      continue;
    }
    const float angle = common::atan2(delta);
    const float value = std::max(
        0.f, 1.f - std::abs(delta.normalized().dot(direction.normalized())));
    AddValueToHistogram(angle, value, histogram);
  }
}

4.3.5 FastCorrelativeScanMatcher3D::ComputeLowestResolutionCandidates

輸出：std::vector< Candidate3D>
a
a

5. 基礎工具

5.1 Protobuf

Protocol Buffers是Google出品並開源的語言和平臺均中立的數據序列化和反序列化工具，使用步驟：
定義.proto文件
生成Protobuf編譯器protoc編譯.proto文件生成.pb.h和.pb.cc
寫應用程序幷包含*.pb.h，然後進行編譯即可
示例.proto文件

syntax = "proto3";
package testx;   #對應namespace
message Person { #對應class name
    string name = 1;
    int32 id = 2;
    string email = 3;
}

示例CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(test)
find_package(protobuf CONFIG REQUIRED)
set(CMAKE_INCLUDE_CURRENT_DIR TRUE)
#find_package(Ceres REQUIRED)
include_directories(${PROTOBUF_INCLUDE_DIRS})
# test
add_executable(test test.cc test.pb.cc)
target_link_libraries(test  protobuf::libprotobuf)

5. TurtleBot3-Burger + Cartographer

5.1 解決IMU和Odom Timestamp不一致的問題

使用系統當前時間替換IMU和Odom的Timestamp
time_conversion.h

// cartographer_ros/cartographer_ros/time_conversion.h
::cartographer::common::Time FromRosNow();

time_conversion.cc

// To improve Odom, IMU time inconsistent
// cartographer_ros/cartographer_ros/time_conversion.cc
::cartographer::common::Time FromRosNow(){
  const std::chrono::nanoseconds now =
             std::chrono::duration_cast<std::chrono::nanoseconds>(
                  std::chrono::system_clock::now().time_since_epoch());
  ::ros::Time rosTime;
  rosTime.sec = now.count()/1000000000;
  rosTime.nsec = now.count()%1000000000;
  return FromRos(rosTime);
}

代碼修改

// cartographer_ros/cartographer_ros/msg_conversion.cc
// Func: LaserScanToPointCloudWithIntensities(const LaserMessageType& msg)
  // ::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
  ::cartographer::common::Time timestamp = FromRosNow();

// cartographer_ros/cartographer_ros/sensor_bridge.cc
// Func: SensorBridge::SensorBridge
  // const carto::common::Time time = FromRos(msg->header.stamp);
  // const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
  //     time, CheckNoLeadingSlash(msg->child_frame_id));
  const carto::common::Time time = FromRosNow();
  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
      time, CheckNoLeadingSlash(msg->header.frame_id));

// cartographer_ros/cartographer_ros/sensor_bridge.cc
// Func: SensorBridge::ToImuData
  //const carto::common::Time time = FromRos(msg->header.stamp);
  const carto::common::Time time = FromRosNow();

5.2 刪除告警

TfBridge::LookupToTracking

// cartographer_ros/cartographer_ros/tf_bridge.cc
    // return ::cartographer::common::make_unique<
    //     ::cartographer::transform::Rigid3d>(ToRigid3d(buffer_->lookupTransform(
    //     tracking_frame_, frame_id, requested_time, timeout)));
    return ::cartographer::common::make_unique<
        ::cartographer::transform::Rigid3d>(ToRigid3d(buffer_->lookupTransform(
        tracking_frame_, frame_id, ::ros::Time(0.), timeout)));

5.3 把Laser Scan轉換爲PointCloud

LaserScanToPointCloudWithIntensities

// Check laser scan data (msg_conversion.cc)
  LOG(ERROR) << "range_min=" << msg.range_min << ", range_max=" << msg.range_max \
             << ", angle_min=" << msg.angle_min << ", angle_max=" << msg.angle_max \
             << ", angle_increment=" << msg.angle_increment << ", msg.ranges.size=" << msg.ranges.size() \
             << ", frame_id=" << msg.header.frame_id << ", scan_time=" << msg.scan_time \
             << ", time_increment=" << msg.time_increment;

5.4 3D截取匹配的數據

std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
LocalTrajectoryBuilder3D::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& unsynchronized_data)

5.5 2D截取匹配的數據

template <typename LaserMessageType>
std::tuple<PointCloudWithIntensities, ::cartographer::common::Time>
LaserScanToPointCloudWithIntensities(const LaserMessageType& msg)

6. 關鍵元素

6.1 體素濾波器(Voxel Filter）

adaptive_voxel_filter (自適應體素濾波器)
- 用於爲Scan Matching計算稀疏點雲（sparser point cloud）
- 爲了找到滿足要求的點雲數據，其體素（Voxel）的邊長可變。
- 最大邊長：因爲是自適應濾波，首先用最大的體素邊長max_length嘗試，濾波之後的點數是否大於min_num_points, 如果大於返回結果；否則邊長減半並進行濾波，直至找到大於min_num_points爲止。(注：一般有1萬多個點雲數據，經過體素濾波之後只有200多個點雲數據用於匹配）
- 代碼參見：sensor/internal/voxel_filter.cc
- 自適應體素濾波器的參數：

    max_length = 2., // 最大邊長
    min_num_points = 150, // 點雲需要保留的最小點數
    max_range = 15., // 點雲的最大距離，距離（l2範數）大於此值的點雲數據直接丟棄

loop_closure_adaptive_voxel_filter（閉環自適應體素濾波器）
- 用於爲閉環檢測計算稀疏點雲（sparser point cloud）
a
a
a
a
a
a

cartographer代碼及論文分析

1. 基礎

1.1 軟件架構

1.2 座標系及相互關係

1.3 調試環境

2. Cartographer主要貢獻

3. 掃描匹配(Scan-Matching）

3.1 Scan-to-Submap

3.1.1 RTCSM: Real-Time Correlative Scan Matching (by Olson)

4. 關鍵流程

4.1 IMU消息處理流程

4.2 Laser Scan(2D)消息處理流程

4.3 Laser Scan(3D)消息處理流程

4.3.1 FastCorrelativeScanMatcher3D::MatchFullSubmap

4.3.2 FastCorrelativeScanMatcher3D::MatchWithSearchParameters

4.3.2 FastCorrelativeScanMatcher3D::GenerateDiscreteScans

4.3.3 DiscreteScan3D FastCorrelativeScanMatcher3D::DiscretizeScan

4.3.4 Eigen::VectorXf RotationalScanMatcher::ComputeHistogram

4.3.5 FastCorrelativeScanMatcher3D::ComputeLowestResolutionCandidates

5. 基礎工具

5.1 Protobuf

5. TurtleBot3-Burger + Cartographer

5.1 解決IMU和Odom Timestamp不一致的問題

5.2 刪除告警

5.3 把Laser Scan轉換爲PointCloud

5.4 3D截取匹配的數據

5.5 2D截取匹配的數據

6. 關鍵元素

6.1 體素濾波器(Voxel Filter）

物理機開關機

計算機視覺-數據集

ToF基礎知識

決策樹和隨機森林（Decision Trees and Random Forests）

OpenCV4 常用技巧

UDT Java 封裝

Mac下配置sublime實現LaTeX

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

linux以太網驅動總結