Ceres Solver:从入门到使用_非晚非晚的博客-CSDN博客_ceres核函数
基本原理公式:
LocalTrajectoryBuilder2D::ScanMatch --》 ceres_scan_matcher_.Match
/**
* @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.);
switch (grid.GetGridType()) {
case GridType::PROBABILITY_GRID: //使用的使概率栅格地图
problem.AddResidualBlock(
CreateOccupiedSpaceCostFunction2D(
options_.occupied_space_weight() /
std::sqrt(static_cast(point_cloud.size())),
point_cloud, grid),
nullptr /* loss function */, ceres_pose_estimate);
break;
case GridType::TSDF:
problem.AddResidualBlock(
CreateTSDFMatchCostFunction2D(
options_.occupied_space_weight() /
std::sqrt(static_cast(point_cloud.size())),
point_cloud, static_cast(grid)),
nullptr /* loss function */, ceres_pose_estimate);
break;
}
// 平移的残差
CHECK_GT(options_.translation_weight(), 0.);
problem.AddResidualBlock(
TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
options_.translation_weight(), target_translation), // 平移的目标值, 没有使用校准后的平移
nullptr /* loss function */, ceres_pose_estimate); // 平移的初值
// 旋转的残差, 固定了角度不变
CHECK_GT(options_.rotation_weight(), 0.);
problem.AddResidualBlock(
RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
options_.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]);
}
CreateOccupiedSpaceCostFunction2D
// 工厂函数, 返回地图的CostFunction
ceres::CostFunction* CreateOccupiedSpaceCostFunction2D(
const double scaling_factor, const sensor::PointCloud& point_cloud,
const Grid2D& grid) {
return new ceres::AutoDiffCostFunction (
new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud, grid),
point_cloud.size()); // 比固定残差维度的 多了一个参数
}
操作函数
bool operator()(const T* const pose, T* residual) const {
Eigen::Matrix translation(pose[0], pose[1]);
Eigen::Rotation2D rotation(pose[2]);
Eigen::Matrix rotation_matrix = rotation.toRotationMatrix();
Eigen::Matrix transform;
transform << rotation_matrix, translation, T(0.), T(0.), T(1.);
const GridArrayAdapter adapter(grid_);
ceres::BiCubicInterpolator 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 point((T(point_cloud_[i].position.x())),
(T(point_cloud_[i].position.y())),
T(1.));
// 根据预测位姿对单个点进行坐标变换
const Eigen::Matrix world = transform * point;
// 获取三次插值之后的栅格free值, Evaluate函数内部调用了GetValue函数
interpolator.Evaluate(
(limits.max().x() - world[0]) / limits.resolution() - 0.5 +
static_cast(kPadding),
(limits.max().y() - world[1]) / limits.resolution() - 0.5 +
static_cast(kPadding),
&residual[i]);
// free值越小, 表示占用的概率越大
residual[i] = scaling_factor_ * residual[i];
}
return true;
}
平移矩阵:
const Eigen::Matrix