当前位置：首页 > news >正文

SLAM中的非线性优化-2D图优化之激光SLAM cartographer前端匹配（十七）

news 2025/7/14 8:01:56

本节主要介绍2D激光SLAM中cartographer前端的ceres_scan_mather_2d,这里也仅仅给出优化部分的简单分析，作为抛砖引玉，下一讲，将会具体介绍一个与其近似的完整方案，用以让大家理解概率如何对位姿求倒，此处cartographer并非原始ros版，而是非ros版，个人将ros去除，并且也利用pangolin来作为可视化工具，废话不多说，来看看接下来的代码,完整代码如下

slam_optimizer: 个人CSDN博客《SLAM中非线性优化的从古至今》对应的源码，该系列博客地址：https://blog.csdn.net/zl_vslam/category_12872677.html - Gitee.comhttps://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的，全靠自己板砖，还只能利用空余时间写一下，因此速度较为缓慢，且做的过程中，也会经常自我怀疑，干这个到底有啥意义，尤其遇到困难的时候，经常想放弃，所幸每次的坚持都将有收获，完成了一阶段的目标后，又会产生新动力，废话说多了，哈哈，就到这里吧，下一讲理论篇，各位看官不要错过精彩哦

查看全文

http://www.dtcms.com/a/277571.html