CeresScanMatcher 匹配的使用步骤与实例解析_ceres做扫描匹配-优快云博客

本文详细介绍了CeresScanMatcher的使用步骤，包括声明CostFunctor、创建AutoDiffCostFunction、添加地图、平移和旋转残差，以及优化位姿估计的过程。重点讲解了OccupiedSpaceCostFunction2D、TranslationDeltaCostFunctor2D和RotationDeltaCostFunctor2D在匹配中的应用。

CeresScanMatcher 匹配的使用步骤与实例解析

一、使用步骤
二、cere 使用实例
1.地图部分的残差
2.平移的残差
3.旋转的残差
三、总结

一、使用步骤

第一步声明CostFunctor

class MyScalarCostFunctor {
   
   
	MyScalarCostFunctor(double k): k_(k) {
   
   }
	template <typename T>
	bool operator()(const T* const x , const T* const y, T* e) const {
   
   
		// (k - x^T * y)^2, 平 方 由 ceres自 动 添 加
		e[0] = k_ - x[0] * y[0] - x[1] * y[1];
		return true;
	}
	private:
		double k_;
};

第二步调用AutoDiffCostFunction函数

在这里插入图片描述
第三步添加残差块

ceres::Problem problem;
problem.AddResidualBlock(cost_function);

第四步进行求解

ceres::Solver::Summary summary;
ceres::Solve(ceres_solver_options_, &problem, summary);
std::cout << summary.BriefReport() << std::endl; // summary.FullReport()

二、cere 使用实例

/**
 * @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;
  CHECK_GT(options_.occupied_space_weight(), 0.);
  // 地图部分的残差
  problem.AddResidualBlock(
      OccupiedSpaceCostFunction2D::CreateAutoDiffCostFunction(
          options_.occupied_space_weight() /
              std::sqrt(static_cast<double>(point_cloud.size())),
          point_cloud, grid),
      nullptr /* loss function */, ceres_pose_estimate);
  CHECK_GT(options_.translation_weight(), 0.);
  // 平移的残差
  problem.AddResidualBlock(
      TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
          options_.translation_weight(