重新捋一捋cartographer的代码：
一、cartographer的代码分为两部分：ROS中的cartographer_ROS包也是调用了cartographer的算法，cartographer的代码分为两部分：
代码从博客https://www.cnblogs.com/hitcm/p/5939507.html中的github下载：
（1）https://github.com/hitcm/cartographer.git
（2）https://github.com/hitcm/cartographer_ros.git
二、从cartographer_ROS进入cartographer算法：
main()——》Run()——》node.Initialize()——》HandleSensorData()——》AddHorizontalLaserFan(cartographer_ros)——》AddHorizontalLaserFan()进入cartographer算法
三、在cartographer中，AddHorizontalLaserFan()函数（global_trajectory_builder.cc） 分为两大步：
（1）进行激光帧插入 前端匹配
（2） 插入成功则进行回环检测和后端优化 ，就是SPA
void GlobalTrajectoryBuilder::AddHorizontalLaserFan(
const common::Time time, const sensor::LaserFan3D& laser_fan)
{
//1、进行激光帧插入 前端匹配
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
local_trajectory_builder_.AddHorizontalLaserFan(time, laser_fan); //前端匹配——帧节匹配的过程
//2、SPA 插入成功则进行回环检测和后端优化
if (insertion_result != nullptr)
{
sparse_pose_graph_->AddScan(
insertion_result->time, //激光帧的时间
insertion_result->tracking_to_tracking_2d, //把激光数据转换到平面转换矩阵
insertion_result->laser_fan_in_tracking_2d, //平面坐标系中的激光数据
insertion_result->pose_estimate_2d, //滤波器估计出来的机器人最新位姿
kalman_filter::Project2D(insertion_result->covariance_estimate), //滤波器估计出来的机器人位姿的方差
insertion_result->submaps, //所有的submap
insertion_result->matching_submap, //本次用来进行scan-match的submap
insertion_result->insertion_submaps); //插入了激光数据的submap 就是submap(size-1) 和 submap(size-2)
}
}四、先进入前端匹配AddHorizontalLaserFan()
1、得到ukf预测的位姿和协方差, 相当于滤波器中的预测位姿
2、在ukf的预测位姿的基础上，通过scanmatch匹配的位姿叫做观测位姿。
ScanMatch()函数内部会更新滤波器，滤波器更新完毕之后，得到机器人的最新的估计位姿和方差
进入ScanMatch（分为4步）：
void LocalTrajectoryBuilder::ScanMatch(
common::Time time,
const transform::Rigid3d& pose_prediction,
const transform::Rigid3d& tracking_to_tracking_2d,
const sensor::LaserFan& laser_fan_in_tracking_2d,
transform::Rigid3d* pose_observation,
kalman_filter::PoseCovariance* covariance_observation)
{
//帧节匹配的流程分为4个步骤：
//1、得到一个地图。 用来进行scan-match对应的submap的概率栅格地图
const ProbabilityGrid& probability_grid =
submaps_.Get(submaps_.matching_index())->probability_grid;
//2、得到一个预估位姿。 计算出来预测的2d位姿 预测的位姿是3d的，因此必须把它旋转到2d平面
//因为这里是2d-slam所以要把预测的位姿旋转到2d平面
transform::Rigid2d pose_prediction_2d =
transform::Project2D(pose_prediction * tracking_to_tracking_2d.inverse());
// The online correlative scan matcher will refine the initial estimate for
// the Ceres scan matcher.
// csm用滤波器提供的初始值进行优化，然后提供一个更好的初始值给ceres-scan-match
transform::Rigid2d initial_ceres_pose = pose_prediction_2d;
//定义一个滤波器
sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.adaptive_voxel_filter_options());
//对激光雷达数据进行滤波 & 转换成点云数据 这里的点云数据是在平面机器人坐标系中
const sensor::PointCloud2D filtered_point_cloud_in_tracking_2d =
adaptive_voxel_filter.Filter(laser_fan_in_tracking_2d.point_cloud);
//配置文件中是否需要用csm来优化ceres-scan-match的初始解
if (options_.use_online_correlative_scan_matching())
{
//3、进行csm匹配，得到一个初始解：initial_ceres_pose，它又分为4步骤。
//通过csm和滤波器过后的2d平面的 激光雷达数据来进行位姿优化
//传入预测的初始位姿＼激光雷达数据＼栅格地图
//返回一个更好的值initial_ceres_pose
real_time_correlative_scan_matcher_.Match(
pose_prediction_2d, filtered_point_cloud_in_tracking_2d,
probability_grid, &initial_ceres_pose);
}
//4、再调用ceres优化的方法进行一次匹配。
/*最终通过ceres_scan_match来得到最终的位姿*/
/*这里得到的位姿是tracking_2d坐标系到map坐标系的转换*/
transform::Rigid2d tracking_2d_to_map;
kalman_filter::Pose2DCovariance covariance_observation_2d;
ceres::Solver::Summary summary;
ceres_scan_matcher_.Match(
transform::Project2D(scan_matcher_pose_estimate_ *
tracking_to_tracking_2d.inverse()),//表示上一个周期的平面位姿
initial_ceres_pose, //这一次的初始估计位姿
filtered_point_cloud_in_tracking_2d, //对应的2维激光点云
probability_grid, //概率栅格地图
&tracking_2d_to_map, &covariance_observation_2d, &summary);
CHECK(pose_tracker_ != nullptr);
//...结束了 CSM先算出一个初始解，叫做initial_ceres_pose,再把这个解作为基于优化的初始解。
......
......
}调用了real_time_correlative_scan_matcher_.Match()和ceres_scan_matcher_.Match()
3、进入real_time_correlative_scan_matcher_.Match(分为4步)：real_time_correlative_scan_matcher.cc
double RealTimeCorrelativeScanMatcher::Match() const
{
CHECK_NOTNULL(pose_estimate);
//得到机器人初始的位姿
const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
。。。。。
。。。。。
//1、得到整个搜索空间里面的所有的候选解
std::vector<Candidate> candidates =
GenerateExhaustiveSearchCandidates(search_parameters);
//2、打分。计算空间中所有的候选解的得分
ScoreCandidates(probability_grid, discrete_scans, search_parameters,
&candidates);
//3、找到得到最大的候选解candidate
const Candidate& best_candidate =
*std::max_element(candidates.begin(), candidates.end());
//4、候选解的位姿即为优化过后的位姿
*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;
}4、进入ceres_scan_matcher_.Match()：ceres_scan_matcher.cc
void CeresScanMatcher::Match(*) const
{
。。。
//1、构造三个误差函数
//构造残差--栅格的误差函数，进行三次样条插值
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunctor, ceres::DYNAMIC,
3>(
new OccupiedSpaceCostFunctor(
options_.occupied_space_cost_functor_weight() /
std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, probability_grid),
point_cloud.size()),
nullptr, ceres_pose_estimate);
CHECK_GT(options_.previous_pose_translation_delta_cost_functor_weight(), 0.);
//构造残差--平移，实际上是一个里程计乘法
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor, 2, 3>(
new TranslationDeltaCostFunctor(
options_.previous_pose_translation_delta_cost_functor_weight(),
previous_pose)),
nullptr, ceres_pose_estimate);
CHECK_GT(options_.initial_pose_estimate_rotation_delta_cost_functor_weight(),
0.);
//构造残差--旋转的乘法
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor, 1,
3>(new RotationDeltaCostFunctor(
options_.initial_pose_estimate_rotation_delta_cost_functor_weight(),
ceres_pose_estimate[2])),
nullptr, ceres_pose_estimate);
//2、求解器Solve进行求解
ceres::Solve(ceres_solver_options_, &problem, summary);
//优化完毕之后得到的最优位姿
*pose_estimate = transform::Rigid2d(
{ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);
//计算位姿的方差
ceres::Covariance::Options options;
ceres::Covariance covariance_computer(options);
std::vector<std::pair<const double*, const double*>> covariance_blocks;
covariance_blocks.emplace_back(ceres_pose_estimate, ceres_pose_estimate);
CHECK(covariance_computer.Compute(covariance_blocks, &problem));
double ceres_covariance[3 * 3];
covariance_computer.GetCovarianceBlock(ceres_pose_estimate,
ceres_pose_estimate, ceres_covariance);
*covariance = Eigen::Map<kalman_filter::Pose2DCovariance>(ceres_covariance);
*covariance *= options_.covariance_scale();
}
五、再进入后端优化和回环检测AddScan(分为4步)，进入线程：
void SparsePoseGraph::AddScan()
{
//1、所有的位姿投影过来
//得到最优pose， 将trajectory_id下的TrajectoryNode中局部位姿转换到世界坐标系下
const transform::Rigid3d optimized_pose(GetLocalToGlobalTransform(*submaps) *
transform::Embed3D(pose));
.....
//2、新生成一个轨迹节点constant_node_data_
//生成一帧ConstantData 并且存储起来
constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{
time, laser_fan_in_pose,
Compress(sensor::LaserFan3D{Eigen::Vector3f::Zero(), {}, {}, {}}),
submaps, transform::Rigid3d(tracking_to_pose)});
.....
//3、是否有新的submap生成，如果有加进submap_data_里面
//如果submap(size -1)没有分配下标，则为它分配下表
// 如果刚刚被插入的那个submap是我们之前从未看过的，就更新submap_data_的数据
// 也就是说submap_data_里面,包含了目前为止所有的submap
.....
//4、为这帧新激光添加约束并开始在后台进行扫描匹配
AddWorkItem(
std::bind(std::mem_fn(&SparsePoseGraph::ComputeConstraintsForScan), this,
j, submaps, matching_submap, insertion_submaps, finished_submap,
pose, covariance));
}(1)ComputeConstraintsForScan():
void SparsePoseGraph::ComputeConstraintsForScan(*)
{
.....
//1、枚举执行了插入操作的submap，即submap(size-1) & submap(size-2)
//并为当前激光添加观测约束（因为当前激光帧刚刚匹配完成并插入到这两个submap中，所以是观测约束）
//Constraint2D形式的约束都是观测约束
for (const mapping::Submap* submap : insertion_submaps)
{
const int submap_index = GetSubmapIndex(submap);
CHECK(!submap_states_[submap_index].finished);
submap_states_[submap_index].scan_indices.emplace(scan_index);
// Unchanged covariance as (submap <- map) is a translation.
//更新submap_id里面的node
const transform::Rigid2d constraint_transform =
sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose;
//构建2D约束（所有的约束都是指观测值即观测约束）
constraints_.push_back(Constraint2D{
submap_index,
scan_index,
{constraint_transform,
common::ComputeSpdMatrixSqrtInverse(
covariance, options_.constraint_builder_options()
.lower_covariance_eigenvalue_bound())},
Constraint2D::INTRA_SUBMAP});
}
....
//2、接下来进行回环检测
//枚举所有的submap 用当前的scan和所有finished的submap进行匹配来进行回环检测
if（）
{。。。
if (scan_trajectory == submap_trajectory ||
scan_and_submap_trajectories_connected)
{
//每一个submap进行约束计算，这里就是回环检测了
//MaybeAddConstraint 计算一个scan和一个submap之间的位姿
//这里就是回环检测
constraint_builder_.MaybeAddConstraint(
submap_index, submap_states_[submap_index].submap, scan_index,
&trajectory_nodes_[scan_index]
.constant_data->laser_fan.point_cloud,
relative_pose);
}
}
}
（2）进入MaybeAddConstraint() constraint_builder.cc
//计算约束激光和submap之间的约束，被spa调用
void ConstraintBuilder::MaybeAddConstraint(*)
{
...
if (sampler_.Pulse())
{
....
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_index, submap->finished_probability_grid,
[=]() EXCLUDES(mutex_) {
//ComputeConstraint调用的是fast_csm，也就是分支定界的csm。
ComputeConstraint(*);
});
}
}(3)进入ComputeConstraint()函数，调用的是fast_csm，也就是分支定界的csm。
//1、真正的计算约束的函数 这个函数被MaybeAddGlobalConstraint()和MaybeAddConstraint()调用，
//分支定界的CSM，就是fast—csm（fast correlative scan match）
void ConstraintBuilder::ComputeConstraint(*)
{
。。。。
//在整个图上进行搜索 程序自行确实搜索的起始位姿
if (match_full_submap) //调用fast_CSM（就是分支定界）进行约束，然后进行
{
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
filtered_point_cloud, options_.global_localization_min_score(),
&score, &pose_estimate))
{
trajectory_connectivity->Connect(scan_trajectory, submap_trajectory);
}
else
{
return;
}
}
//指定初始位姿
else
{
if (!submap_scan_matcher->fast_correlative_scan_matcher->Match(
initial_pose, filtered_point_cloud, options_.min_score(), &score,
&pose_estimate))
{
return;
}
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
{
common::MutexLocker locker(&mutex_);
score_histogram_.Add(score);
}
}
// Use the CSM estimate as both the initial and previous pose. This has the
// effect that, in the absence of better information, we prefer the original
// CSM estimate.
// 用CSM的解作为初始解，再次用ceres来进行优化
ceres::Solver::Summary unused_summary;
kalman_filter::Pose2DCovariance covariance;
ceres_scan_matcher_.Match(pose_estimate, pose_estimate, filtered_point_cloud,
*submap_scan_matcher->probability_grid,
&pose_estimate, &covariance, &unused_summary);
// 'covariance' is unchanged as (submap <- map) is a translation.
const transform::Rigid2d constraint_transform =
ComputeSubmapPose(*submap).inverse() * pose_estimate;
//只是增加一个约束 ，还没开始优化
//约束和优化是分开的，约束是通过调用Fast_CSM实现的，而Fast_CSM就是分支定界的方法了。
constraint->reset(new OptimizationProblem::Constraint{
submap_index,
scan_index,
{constraint_transform,
common::ComputeSpdMatrixSqrtInverse(
covariance, options_.lower_covariance_eigenvalue_bound())},
OptimizationProblem::Constraint::INTER_SUBMAP});
。。。。。
}（4）进入FastCorrelativeScanMatcher::Match（）：
bool FastCorrelativeScanMatcher::Match(*) const
{
//1、设置搜索参数
const SearchParameters search_parameters(options_.linear_search_window(),
options_.angular_search_window(),
point_cloud, limits_.resolution());
//2、用这个参数进行匹配
return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
point_cloud, min_score, score,
pose_estimate);
}（5）进入MatchWithSearchParameters（）
bool FastCorrelativeScanMatcher::MatchWithSearchParameters(*) const
{
CHECK_NOTNULL(score);
CHECK_NOTNULL(pose_estimate);
//1、先旋转
//把激光数据旋转到世界坐标系中的0度的位置
const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
const sensor::PointCloud2D rotated_point_cloud =
sensor::TransformPointCloud2D(
point_cloud,
transform::Rigid2d::Rotation(initial_rotation).cast<float>());
//生成一系列的rotated scans
const std::vector<sensor::PointCloud2D> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters);
//把上面的rotated scans转换到世界坐标系中 然后转换到地图坐标系中
//这里之后，所有激光点的坐标走在世界坐标系中了 或者说地图坐标系中。
//这里的离散激光点 是在最细的分辨率的地图上面
const std::vector<DiscreteScan> discrete_scans = DiscretizeScans(
limits_, rotated_scans,
Eigen::Translation2f(initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y()));
search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits());
//2、计算最低分辨率（最粗糙）中的所有的候选解，并且进行了打分，得分从大到小排列
//最低分辨率是通过搜索树的层数、地图的分辨率计算出来的。
//对于地图坐标系来说 最低分辨率=1<<h h表示搜索树的总的层数
//这里不但对最低分辨率的所有候选解的得分进行了计算 同时还按照从大到小排列
const std::vector<Candidate> lowest_resolution_candidates =
ComputeLowestResolutionCandidates(discrete_scans, search_parameters);
//3、用分枝定界方法来计算最优的候选解。得到一个最优的值
const Candidate best_candidate = BranchAndBound(
discrete_scans, search_parameters, lowest_resolution_candidates,
precomputation_grid_stack_->max_depth(), min_score);
//如果计算出来的解大于最小的阈值 则认为匹配成功，返回对应的位姿
...
return false;
}(5)进行分支定界计算 BranchAndBound（），分为5步，并进行循环调用BranchAndBound()迭代：
Candidate FastCorrelativeScanMatcher::BranchAndBound(*) const
{
//如果只有一层 那么最低分辨率中最好的就是全局最好的，直接返回
//相当于是叶子节点 这个分数会用来更新父节点的best_score。
//这个在返回之后 会用来更新bestScore
//1、是否是叶子节点
if (candidate_depth == 0)
{
// Return the best candidate.
return *candidates.begin();
}
Candidate best_high_resolution_candidate(0, 0, 0, search_parameters);
best_high_resolution_candidate.score = min_score;
//枚举所有的候选解 从高到低美剧
for (const Candidate& candidate : candidates)
{
//如果某个候选解小于min_score可不需要再进行计算了。这里的min_score相当于当前搜索过的所有解中的最优解的得分
//这里相当于定界 如果当前这颗字数的分数小于最优解的分数 则这颗子树可以直接被减枝
//因为候选解是按照分数从大到小排列的
//在进行迭代的时候，这个min_score是会不断的进行更新的。因为会不断的进行搜索。每次在子节点搜索到更优的解。这个值就会被更新。
//2、先定界。min_score只有最最底层的叶子节点的时候，才会进行更新。
if (candidate.score <= min_score) //剪枝的过程
{
break;
}
//3、开始进行分支
std::vector<Candidate> higher_resolution_candidates;
const int half_width = 1 << (candidate_depth - 1);
//该节点分解为四个子节点 这里就是分枝
for (int x_offset : {0, half_width})
{
//超出范围则不需要进行操作了
if (candidate.x_index_offset + x_offset >
search_parameters.linear_bounds[candidate.scan_index].max_x)
{
break;
}
for (int y_offset : {0, half_width})
{
//超出范围则不需要进行计算了
if (candidate.y_index_offset + y_offset >
search_parameters.linear_bounds[candidate.scan_index].max_y)
{
break;
}
//把这个可能存在更优解的地方放入队列中
higher_resolution_candidates.emplace_back(
candidate.scan_index, candidate.x_index_offset + x_offset,
candidate.y_index_offset + y_offset, search_parameters);
}
}
//4、计算所有的候选解(4个)的得分
ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1),
discrete_scans, search_parameters,
&higher_resolution_candidates);
//进行迭代求解最优值 这里相当于传进去最新的best_score来作为子节点的min_score
//注意这个best_score相当于这颗子树下面的所有叶子节点的best_score
best_high_resolution_candidate = std::max(
best_high_resolution_candidate,
//5、再进行迭代
BranchAndBound(discrete_scans, search_parameters,
higher_resolution_candidates, candidate_depth - 1,
best_high_resolution_candidate.score));
}
return best_high_resolution_candidate;
}