從網上搜尋大量的文獻來解決這一部分難題，進度上更慢一些，了解可能會有偏差，有錯誤之處請多多指出
資料處理+Local SLAM
輸入：
1.雷射雷達的資料經過Voxel Filter、Adaptive Voxel Filter(調用體素濾波，如果體素濾波後點數大于門檻值，則傳回，如果小于門檻值，則接着使用二分法進行體素濾波)後的點雲資料
2.經過裡程計、慣導及點雲比對後的位姿态預估下一時刻的位姿
輸出：
1.Submaps
Local SLAM主要是運用了ScanMatching的方法，當獲得一次掃描資料後，掃描資料與目前最近的submap比對，獲得最優的位置姿态（？？？？）後，将這一幀點雲加入到submap中，當submap裡的幀數達到一定數目（關鍵幀插入方法？？？）後就不在更新這個子地圖，接下來會建立下一個submap繼續以上工作。
流程：
- 多雷射傳感器資料基于時間戳同步融合
- 開啟位置估計器
- 根據位置估計器，針對每個點雲point的時間戳進行預測位置進行畸變校準
- 校準後的點雲轉換成scanmath和map 更新使用的range格式，并将miss和hit分類
- 擷取校準後的origin pos
- 擷取預測的重力加速度方向
- 根據重力加速度方向投影點雲
- 降采樣濾波，并抛棄設定高度範圍外的所有點雲
- 然後調用比對方法
- 插入并更新submap
- 擷取比對後的結果内容，包括時間，軌迹節點位置，節點位置對應點雲，submap
Local SLAM之相關比對（correlative scan matcher）
原理
目前尋找的最好的資料：Real-Time Correlative Scan Matching完全解析（CSM幀比對算法）
通過Real-time correlative scan matching 論文算法分析、[翻譯]Real-Time Correlative Scan Matching對算法有了大緻的了解，Cartographer是采取的雙搜尋的方式進行的， 先用一次real-time correlative scan matcher（三維視窗周遊尋優），再建構優化等式，利用ceres優化求解。（栅格機率， T的偏差，R的偏差）
數學模型
模型為利用最大似然估計計算在t時刻最有可能的位姿，可以了解為比對的越好時候的位姿
，上面公式的機率越大，我們的目的就是求解這個，其中
、
表示t時刻的位姿資料和雷射雷達的觀測資料；
在此處了解為距離最近的submap；
，
表示為
時刻的位姿和控制量資訊，控制資訊可以了解為從裡程計和IMU推導出來的資料，有地方公式将
也表達為
；
為觀測模型，了解為在
位姿，
地圖下，觀測資料為
的機率，詳細可以參照卡爾曼濾波相關知識；
為運動模型，了解為在
，
下
時刻最可能的位姿。
公式細化
雖然有公式了，但是還不夠詳細，雷射雷達來一幀資料，是實實在在的資料，得踏踏實實的處理這些資料
1.運動模型
cartographer部分利用imu建構預測模型，采取UKF進行運動預測。
具體運動模型推測見：cartographer 處理IMU（雷射，裡程計等）流程
2.觀測模型
單線雷射雷達的資料一般是一個距離和一個角度的極坐标資訊，通過求解可以得出x，y；多線雷射雷達直接輸出x,y,z
一幀資料寫到每個點上：
其中
為雷射雷達每一個點的機率值，将資料進行栅格化後再計算
栅格地圖的知識可以參考占據栅格地圖（Occupancy Grid Map）
每一幀資料都可進行栅格化，如下圖所示，
用來表示submap，
為目前幀的資料所形成的栅格地圖
cartographer 是一種暴力搜尋的模式，主要的算法流程：
1.在
時刻設定一個search window（這個search window用來和目前的雷射雷達的幀的資料進行比對）
2.在search window區域生成目前的位姿的
可能值，作為一個子集
3.周遊所有的子集，計算每一個子集上
所有的機率和，機率最高的那個位姿就是我們要找的目前時刻的位姿（機率和？目前幀資料的栅格和submap裡的機率和）
算法流程（未完成）
1.構造粗分辨率和細分辨率兩個似然場
2.在粗分辨率似然場上進行搜尋，擷取最優位姿
3.粗分辨率最優位姿對應的栅格進行細分辨率劃分
4.進行細分辨率搜尋，擷取最優位姿
5.計算位姿比對方差
代碼解析
double RealTimeCorrelativeScanMatcher2D::Match(
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, const Grid2D& grid,
transform::Rigid2d* pose_estimate) const {
CHECK(pose_estimate != nullptr);
//擷取初始位置的角度
const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
//将目前點雲轉換到初始角度坐标系下
const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
point_cloud,
transform::Rigid3f::Rotation(Eigen::AngleAxisf(
initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
//定義搜尋空間，包括平移的視窗和角度視窗
//option_參數由上層調用傳入？？？？？？
const SearchParameters search_parameters(
options_.linear_search_window(), options_.angular_search_window(),
rotated_point_cloud, grid.limits().resolution());
//在一定的角度搜尋空間中（一定的視窗内，一定分辨率，對目前點雲放入vector中）
const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters);
//将點雲進行坐标變換轉換到世界坐标系下，根據目前的位置（裡程計的位置）進行轉換
const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
grid.limits(), rotated_scans,
Eigen::Translation2f(initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y()));
//初始化xy平移空間内偏移量，即視窗大小和偏移量
std::vector<Candidate2D> candidates =
GenerateExhaustiveSearchCandidates(search_parameters);
//計算目前幀在角度、x、y三層視窗中每個狀态在grid中的比對評分
ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);
//采用标準庫，疊代擷取最大置信度元素
const Candidate2D& best_candidate =
*std::max_element(candidates.begin(), candidates.end());
//坐标轉換為全局坐标
*pose_estimate = transform::Rigid2d(
{initial_pose_estimate.translation().x() + best_candidate.x,
initial_pose_estimate.translation().y() + best_candidate.y},
initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
return best_candidate.score;
}
基礎太差了，全都細細的分析，步履維艱，腦補c++
1.double RealTimeCorrelativeScanMatcher2D::Match(const transform::Rigid2d& initial_pose_estimate,const sensor::PointCloud& point_cloud, const Grid2D& grid,transform::Rigid2d* pose_estimate)
1.1 transform::Rigid2d& initial_pose_estimate（定義在catkin_ws/src_carto/cartographer/cartographer/transform/transform.h）//初始的位姿資訊
template <typename FloatType> class Rigid2{}；
using Rigid2d = Rigid2<double>;
1.2 const sensor::PointCloud& point_cloud;點雲的類型
1.3 transform::Rigid2d* pose_estimate //估計的位姿
2.const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
3.const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(point_cloud,transform::Rigid3f::Rotation(Eigen::AngleAxisf(initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
3.1const sensor::PointCloud
定義在（/catkin_ws/src_carto/cartographer/cartographer/sensor/point_cloud.h)
using PointCloud = std::vector<RangefinderPoint>;
struct RangefinderPoint {
Eigen::Vector3f position;
};
3.2 sensor::TransformPointCloud函數（ vector::reserve 、c++11 之emplace_back 與 push_back的差別）
PointCloud TransformPointCloud(const PointCloud& point_cloud,
const transform::Rigid3f& transform) {
PointCloud result;
result.reserve(point_cloud.size());
for (const RangefinderPoint& point : point_cloud) {
result.emplace_back(transform * point);
//c++開發中我們會經常用到插入操作對stl的各種容器進行操作,常使用push_back()向容器中加入一個右值元
//素(臨時對象)時，首先會調用構造函數構造這個臨時對象，然後需要調用拷貝構造函數将這個臨時對象放入容
//器中。原來的臨時變量釋放。這樣造成的問題就是臨時變量申請資源的浪費。
//引入了右值引用，轉移構造函數後，push_back()右值時就會調用構造函數和轉移構造函數,如果可以在插入
//的時候直接構造，就隻需要構造一次即可。這就是c++11 新加的emplace_back。
}
return result;
}
for實作對vector的周遊
假設實作一個vector周遊可以用以下方法
vector<
int
> line={1,2,3,4,5,6,7,8,9};
（1）
for (vector<int>::const_iterator iter = line.cbegin();iter != line.cend(); iter++) {
cout << (*iter) << endl;
}
（2）
for (auto iter = line.cbegin(); iter != line.cend(); iter++) {
cout << (*iter) << endl;
}
（3）
for (auto lin : line) {
cout << lin;
}
4.const SearchParameters search_parameters(options_.linear_search_window(), options_.angular_search_window(),rotated_point_cloud, grid.limits().resolution());
4.1 SearchParameters
定義在/catkin_ws/src_carto/cartographer/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h
struct SearchParameters {
// Linear search window in pixel offsets; bounds are inclusive.
struct LinearBounds {
int min_x;
int max_x;
int min_y;
int max_y;
};
SearchParameters(double linear_search_window, double angular_search_window,
const sensor::PointCloud& point_cloud, double resolution);
SearchParameters::SearchParameters(const double linear_search_window,
const double angular_search_window,
const sensor::PointCloud& point_cloud,
const double resolution)
: resolution(resolution) {
// We set this value to something on the order of resolution to make sure that
// the std::acos() below is defined.
float max_scan_range = 3.f * resolution;
for (const sensor::RangefinderPoint& point : point_cloud) {
const float range = point.position.head<2>().norm();
max_scan_range = std::max(range, max_scan_range);
}
const double kSafetyMargin = 1. - 1e-3;
angular_perturbation_step_size =
kSafetyMargin * std::acos(1. - common::Pow2(resolution) /
(2. * common::Pow2(max_scan_range)));
num_angular_perturbations =
std::ceil(angular_search_window / angular_perturbation_step_size);
num_scans = 2 * num_angular_perturbations + 1;
const int num_linear_perturbations =
std::ceil(linear_search_window / resolution);
linear_bounds.reserve(num_scans);
for (int i = 0; i != num_scans; ++i) {
linear_bounds.push_back(
LinearBounds{-num_linear_perturbations, num_linear_perturbations,
-num_linear_perturbations, num_linear_perturbations});
}
}
5. const std::vector<sensor::PointCloud> rotated_scans = GenerateRotatedScans(rotated_point_cloud, search_parameters);
std::vector<sensor::PointCloud> GenerateRotatedScans(
const sensor::PointCloud& point_cloud,
const SearchParameters& search_parameters) {
std::vector<sensor::PointCloud> rotated_scans;//定義一個參數存儲旋轉後的資料
rotated_scans.reserve(search_parameters.num_scans);//預設大小
double delta_theta = -search_parameters.num_angular_perturbations *
search_parameters.angular_perturbation_step_size;//計算一個最大的角度變換量
for (int scan_index = 0; scan_index < search_parameters.num_scans;
++scan_index,delta_theta += search_parameters.angular_perturbation_step_size){
rotated_scans.push_back(sensor::TransformPointCloud(
point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
delta_theta, Eigen::Vector3f::UnitZ()))));//每一個點計算旋轉之後的坐标
}
return rotated_scans;
}
6. const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(grid.limits(), rotated_scans,Eigen::Translation2f(initial_pose_estimate.translation().x(),initial_pose_estimate.translation().y()));
std::vector<DiscreteScan2D> DiscretizeScans(
const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
const Eigen::Translation2f& initial_translation) {
std::vector<DiscreteScan2D> discrete_scans;
discrete_scans.reserve(scans.size());
for (const sensor::PointCloud& scan : scans) {
discrete_scans.emplace_back();
discrete_scans.back().reserve(scan.size());
for (const sensor::RangefinderPoint& point : scan) {
const Eigen::Vector2f translated_point =
Eigen::Affine2f(initial_translation) * point.position.head<2>();
discrete_scans.back().push_back(
map_limits.GetCellIndex(translated_point));
}
}
return discrete_scans;
}
7.std::vector<Candidate2D> candidates = GenerateExhaustiveSearchCandidates(search_parameters);///初始化x,y平移空間内偏移量
8.ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);//計算得分
9.const Candidate2D& best_candidate = *std::max_element(candidates.begin(), candidates.end());//擷取最大的置信度
10.*pose_estimate = transform::Rigid2d({initial_pose_estimate.translation().x() + best_candidate.x,//轉換到全局坐标系
initial_pose_estimate.translation().y() + best_candidate.y},
initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
11.return best_candidate.score;//傳回得分
--------------------------------------
參考文獻
1.cartographer 代碼思想解讀（1）- 相關比對
2.Cartographe純雷達計算位姿的前後端總結
3.Real-time correlative scan matching 論文算法分析
4.Cartographer中對雷射雷達運動畸變的處理方法分析