本节主要介绍2D激光SLAM中cartographer前端的ceres_scan_mather_2d,这里也仅仅给出优化部分的简单分析，作为抛砖引玉，下一讲，将会具体介绍一个与其近似的完整方案，用以让大家理解概率如何对位姿求倒，此处cartographer并非原始ros版，而是非ros版，个人将ros去除，并且也利用pangolin来作为可视化工具，废话不多说，来看看接下来的代码,完整代码如下
https://gitee.com/zl_vslam/slam_optimizer/tree/master/2d_optimize/ch15一. CeresScanMatcher2D 匹配
/**
* @brief 基于Ceres的扫描匹配
*
* @param[in] target_translation 预测出来的先验位置, 只有xy
* @param[in] initial_pose_estimate (校正后的)先验位姿, 有xy与theta
* @param[in] point_cloud 用于匹配的点云 点云的原点位于local坐标系原点
* @param[in] grid 用于匹配的栅格地图
* @param[out] pose_estimate 优化之后的位姿
* @param[out] summary
*/
void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const Grid2D& grid,
transform::Rigid2d* const pose_estimate,
ceres::Solver::Summary* const summary) const {
double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y(),
initial_pose_estimate.rotation().angle()};
ceres::Problem problem;
double occupied_space_weight = 1.0;
double translation_weight = 10.0;
double rotation_weight = 40.0;
// 地图部分的残差
CHECK_GT(occupied_space_weight, 0.0);
switch (grid.GetGridType()) {
case GridType::PROBABILITY_GRID:
problem.AddResidualBlock(
CreateOccupiedSpaceCostFunction2D(
occupied_space_weight /
std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, grid),
nullptr /* loss function */, ceres_pose_estimate);
break;
case GridType::TSDF:
problem.AddResidualBlock(
CreateTSDFMatchCostFunction2D(
occupied_space_weight /
std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, static_cast<const TSDF2D&>(grid)),
nullptr /* loss function */, ceres_pose_estimate);
break;
}
// 平移的残差
CHECK_GT(translation_weight, 0.);
problem.AddResidualBlock(
TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
translation_weight, target_translation), // 平移的目标值, 没有使用校准后的平移
nullptr /* loss function */, ceres_pose_estimate); // 平移的初值
// 旋转的残差, 固定了角度不变
CHECK_GT(rotation_weight, 0.0);
problem.AddResidualBlock(
RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
rotation_weight, ceres_pose_estimate[2]), // 角度的目标值
nullptr /* loss function */, ceres_pose_estimate); // 角度的初值
// 根据配置进行求解
ceres::Solve(ceres_solver_options_, &problem, summary);
*pose_estimate = transform::Rigid2d(
{ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);
LOG(INFO) << summary->BriefReport() << std::endl;
// LOG(INFO) << summary->FullReport() << std::endl;
}如上述代码所示，其优化部分由三个残差项组成；
残差项一： 地图部分的概率残存，较为复杂；
残差项二： 平移误差，这一项，较为简单，就是优化后的平移跟优化前的平移作差；
残差项三： 角度误差，这一项，较为简单，就是优化后的角度跟优化前的角度作差；
上述平移跟角度误差项，实际上就是前边章节讲解的固定零空间
二. OccupiedSpaceCostFunction2D残差
template <typename T>
bool operator()(const T* const pose, T* residual) const {
Eigen::Matrix<T, 2, 1> translation(pose[0], pose[1]);
Eigen::Rotation2D<T> rotation(pose[2]);
Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
Eigen::Matrix<T, 3, 3> transform;
transform << rotation_matrix, translation, T(0.), T(0.), T(1.);
const GridArrayAdapter adapter(grid_);
ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
const MapLimits& limits = grid_.limits();
for (size_t i = 0; i < point_cloud_.size(); ++i) {
// Note that this is a 2D point. The third component is a scaling factor.
const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].position.x())),
(T(point_cloud_[i].position.y())),
T(1.));
// 根据预测位姿对单个点进行坐标变换
const Eigen::Matrix<T, 3, 1> world = transform * point;
// 获取三次插值之后的栅格free值, Evaluate函数内部调用了GetValue函数
interpolator.Evaluate(
(limits.max().x() - world[0]) / limits.resolution() - 0.5 +
static_cast<double>(kPadding),
(limits.max().y() - world[1]) / limits.resolution() - 0.5 +
static_cast<double>(kPadding),
&residual[i]);
// free值越小, 表示占用的概率越大
residual[i] = scaling_factor_ * residual[i];
}
return true;
}这一项就是地图的概率残差，scaling_factor_实际上就是信息矩阵开方；interpolator.Evaluate这个是三次样条插值吧，主要目的就是联系离散空间根连续空间的桥梁，下一将会讲解类似方案
三. TranslationDeltaCostFunctor2D残差
// 平移量残差的计算, (pose[0] - x_)的平方ceres会自动加上
template <typename T>
bool operator()(const T* const pose, T* residual) const {
residual[0] = scaling_factor_ * (pose[0] - x_);
residual[1] = scaling_factor_ * (pose[1] - y_);
return true;
}这个太简单了，也不用介绍了吧
四. RotationDeltaCostFunctor2D残差
// 旋转量残差的计算
template <typename T>
bool operator()(const T* const pose, T* residual) const {
residual[0] = scaling_factor_ * (pose[2] - angle_);
return true;
}这个太简单了，也不用介绍了吧
五. 效果展示
如果所示，黄色为轮式里程计的轨迹，白色的为激光匹配的轨迹，回环都没有加入，随着时间推移，两者间的差异逐渐变大，数据利用的是nclt数据集，并且我也提供了数据转化的脚本，目前因为时间限制，只是粗略的调整，效果也还行
六. 总结
本节主要是工程简介，跟效果展示，至于理论讲解将放入下一讲，上班族时间太少了，有一段时间没更新了，主要还是数据不太好找，找到了还得研究，并且不依赖ros的，全靠自己板砖，还只能利用空余时间写一下，因此速度较为缓慢，且做的过程中，也会经常自我怀疑，干这个到底有啥意义，尤其遇到困难的时候，经常想放弃，所幸每次的坚持都将有收获，完成了一阶段的目标后，又会产生新动力，废话说多了，哈哈，就到这里吧，下一讲理论篇，各位看官不要错过精彩哦