b级英语cartographer源码分析
sherry什么意思重新捋⼀捋cartographer的代码：
⼀、cartographer的代码分为两部分：ROS中的cartographer_ROS包也是调⽤了cartographer的算法，cartographer的代码分为两部分：
shin涉及主要论⽂：
1、 Real-Time Loop Closure in 2D LIDAR SLAM , ICRA 2016
2、Efficient Spar Po Adjustment for 2D Mapping (SPA：Spar Po Adjustment)
3、 Real-Time Correlative Scan Matching (BBS：Branch-and-bound scan matching)
⼆、从cartographer_ROS进⼊cartographer算法：
在中：
main()——》Run()——》node.Initialize()——》HandleSensorData()
——》AddHorizontalLarFan(cartographer_ros)——》AddHorizontalLarFan()进⼊cartographer算法
三、在cartographer中，AddHorizontalLarFan()函数（global_trajectory_builder） 分为两⼤步：
（1）进⾏激光帧插⼊ 前端匹配，调⽤流程：
——》AddHorizontalLarFan()——》ScanMatch()——》real_time_correlative_scan_matcher_.Match（）
——》ceres_scan_matcher_.Match（）
（2） 插⼊成功则进⾏回环检测和后端优化 ，就是SPA，调⽤流程：
——》AddScan()——》ComputeConstraintsForScan()——》MaybeAddConstraint()——》ComputeConstraint（）
——》fast_correlative_scan_matcher->Match——》MatchWithSearchParameters（）——》BranchAndBound()
void GlobalTrajectoryBuilder::AddHorizontalLarFan(
const common::Time time, const nsor::LarFan3D& lar_fan)
{
//1、进⾏激光帧插⼊前端匹配
std::unique_ptr<LocalTrajectoryBuilder::InrtionResult> inrtion_result =
local_trajectory_builder_.AddHorizontalLarFan(time, lar_fan); //前端匹配——帧节匹配的过程
//2、SPA 插⼊成功则进⾏回环检测和后端优化
if (inrtion_result != nullptr)
{
spar_po_graph_->AddScan(
resist temptationinrtion_result->time, //激光帧的时间
inrtion_result->tracking_to_tracking_2d, //把激光数据转换到平⾯转换矩阵
inrtion_result->lar_fan_in_tracking_2d, //平⾯坐标系中的激光数据
inrtion_result->po_estimate_2d, //滤波器估计出来的机器⼈最新位姿
kalman_filter::Project2D(inrtion_result->covariance_estimate), //滤波器估计出来的机器⼈位姿的⽅差
astonished
inrtion_result->submaps, //所有的submap
inrtion_result->matching_submap, //本次⽤来进⾏scan-match的submap
inrtion_result->inrtion_submaps); //插⼊了激光数据的submap 就是submap(size-1) 和 submap(size-2) }
}
fire ball四、先进⼊前端匹配AddHorizontalLarFan()
（1）进⼊ScanMatch（分为4步）：
1、得到ukf预测的位姿和协⽅差, 相当于滤波器中的预测位姿
2、在ukf的预测位姿的基础上，通过scanmatch匹配的位姿叫做观测位姿。
limit
ScanMatch()函数内部会更新滤波器，滤波器更新完毕之后，得到机器⼈的最新的估计位姿和⽅差
void LocalTrajectoryBuilder::ScanMatch(*)
{
//帧节匹配的流程分为4个步骤：
2013年四级//1、得到⼀个地图。⽤来进⾏scan-match对应的submap的概率栅格地图
国庆英文const ProbabilityGrid& probability_grid =
submaps_.Get(submaps_.matching_index())->probability_grid;
//2、得到⼀个预估位姿。计算出来预测的2d位姿 预测的位姿是3d的，因此必须把它旋转到2d平⾯
//因为这⾥是2d-slam所以要把预测的位姿旋转到2d平⾯
transform::Rigid2d po_prediction_2d =
transform::Project2D(po_prediction * tracking_to_tracking_2d.inver());
/
/ The online correlative scan matcher will refine the initial estimate for
// the Ceres scan matcher.
// csm⽤滤波器提供的初始值进⾏优化，然后提供⼀个更好的初始值给ceres-scan-match
transform::Rigid2d initial_ceres_po = po_prediction_2d;
//定义⼀个滤波器
nsor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.adaptive_voxel_filter_options());
//对激光雷达数据进⾏滤波 & 转换成点云数据这⾥的点云数据是在平⾯机器⼈坐标系中
const nsor::PointCloud2D filtered_point_cloud_in_tracking_2d =
adaptive_voxel_filter.Filter(lar_fan_in_tracking_2d.point_cloud);
//配置⽂件中是否需要⽤csm来优化ceres-scan-match的初始解
if (options_.u_online_correlative_scan_matching())
{
//3、进⾏csm匹配，得到⼀个初始解：initial_ceres_po，它⼜分为4步骤。
矫情饰行//通过csm和滤波器过后的2d平⾯的 激光雷达数据来进⾏位姿优化
//传⼊预测的初始位姿＼激光雷达数据＼栅格地图
//返回⼀个更好的值initial_ceres_po
real_time_correlative_scan_matcher_.Match(
po_prediction_2d, filtered_point_cloud_in_tracking_2d,
probability_grid, &initial_ceres_po);
}
//4、再调⽤ceres优化的⽅法进⾏⼀次匹配。
/
*最终通过ceres_scan_match来得到最终的位姿*/
/*这⾥得到的位姿是tracking_2d坐标系到map坐标系的转换*/
transform::Rigid2d tracking_2d_to_map;
kalman_filter::Po2DCovariance covariance_obrvation_2d;
ceres::Solver::Summary summary;
ceres_scan_matcher_.Match(
transform::Project2D(scan_matcher_po_estimate_ *
tracking_to_tracking_2d.inver()),//表⽰上⼀个周期的平⾯位姿
initial_ceres_po, //这⼀次的初始估计位姿
filtered_point_cloud_in_tracking_2d, //对应的2维激光点云
probability_grid, //概率栅格地图
&tracking_2d_to_map, &covariance_obrvation_2d, &summary);
CHECK(po_tracker_ != nullptr);
//...结束了 CSM先算出⼀个初始解，叫做initial_ceres_po，再把这个解作为基于优化的初始解。
......
......
}
调⽤了real_time_correlative_scan_matcher_.Match()和ceres_scan_matcher_.Match()
（2）、进⼊real_time_correlative_scan_matcher_.Match(分为4步)：real_time_correlative_scan_matcher
double RealTimeCorrelativeScanMatcher::Match() const
{
CHECK_NOTNULL(po_estimate);
//得到机器⼈初始的位姿
const Eigen::Rotation2Dd initial_rotation = initial_ation();
。。。。。
。。。。。
//1、得到整个搜索空间⾥⾯的所有的候选解
std::vector<Candidate> candidates =
GenerateExhaustiveSearchCandidates(arch_parameters);
//2、打分。计算空间中所有的候选解的得分
ScoreCandidates(probability_grid, discrete_scans, arch_parameters,
&candidates);
//3、找到得到最⼤的候选解candidate
const Candidate& best_candidate =
*std::max_element(candidates.begin(), d());
//4、候选解的位姿即为优化过后的位姿
*po_estimate = transform::Rigid2d(
{initial_anslation().x() + best_candidate.x,
initial_anslation().y() + best_candidate.y},
initial_rotation * Eigen::Rotation2Dd(ientation));
return best_candidate.score;
}
（3）、进⼊ceres_scan_matcher_.Match()：ceres_scan_matcher
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_po_estimate);
CHECK_GT(options_.previous_po_translation_delta_cost_functor_weight(), 0.);
//构造残差--平移，实际上是⼀个⾥程计乘法
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor, 2, 3>(
new TranslationDeltaCostFunctor(
options_.previous_po_translation_delta_cost_functor_weight(),
previous_po)),
nullptr, ceres_po_estimate);
CHECK_GT(options_.initial_po_estimate_rotation_delta_cost_functor_weight(), 0.);
//构造残差--旋转的乘法
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor, 1,
3>(new RotationDeltaCostFunctor(
options_.initial_po_estimate_rotation_delta_cost_functor_weight(),
ceres_po_estimate[2])),
nullptr, ceres_po_estimate);
//2、求解器Solve进⾏求解
ceres::Solve(ceres_solver_options_, &problem, summary);
//优化完毕之后得到的最优位姿
*po_estimate = transform::Rigid2d(
{ceres_po_estimate[0], ceres_po_estimate[1]}, ceres_po_estimate[2]);
/
/计算位姿的⽅差
ceres::Covariance::Options options;
ceres::Covariance covariance_computer(options);
std::vector<std::pair<const double*, const double*>> covariance_blocks;
place_back(ceres_po_estimate, ceres_po_estimate);
CHECK(covariance_computer.Compute(covariance_blocks, &problem));
double ceres_covariance[3 * 3];
covariance_computer.GetCovarianceBlock(ceres_po_estimate,
ceres_po_estimate, ceres_covariance);
*covariance = Eigen::Map<kalman_filter::Po2DCovariance>(ceres_covariance); *covariance *= options_.covariance_scale();
}
五、再进⼊后端优化和回环检测AddScan(分为4步)，进⼊线程：
