从网上搜索大量的文献来解决这一部分难题，进度上更慢一些，理解可能会有偏差，有错误之处请多多指出
数据处理+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中对激光雷达运动畸变的处理方法分析