Cartographer后端之Ceres优化
看这篇之前，要是一点都没看过 Ceres ，看一下这里 ,都写在注释里，直接看注释
Ceres优化库_羊狗狗一只2022年的博客-CSDN博客
cartographer后端的优化由两部分组成
一、Ceres_scan_matcher_2d.cc中的Match方法
这里主要对激光算出来的概率、平移、旋转做优化，优化的部分主要为推测出来的,其中针对激光数据同时优化，第二部分对计算的x,y和预估的x,y进行优化，第三部分对计算的和预估的进行优化。
1.通过激光数据对优化部分
problem.AddResidualBlock(
CreateOccupiedSpaceCostFunction2D( // CreateOccupiedSpaceCostFunction2D 为代价函数 nullptr 为损失函数 ceres_pose_estimate 为需要优化的参数
options_.occupied_space_weight() /
std::sqrt(static_cast(point_cloud.size())),
point_cloud, grid),
nullptr /* loss function */, ceres_pose_estimate);
这里去优化点云中每个点的free值
原理是根据当前的旋转与平移变换点云，将点云中所有点的坐标映射到地图的整形坐标中，获得点云中每个点的概率值，并转换为代价
costfunction为：
template
bool operator()(const T * const pose, T* residual) const // residual是输出
{
// 第一步，将 pose 转换成一个 3 × 3 的变换矩阵
// 平移矩阵
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.);
// 这里将构造时传入的概率栅格图（local submap）加载到一个双三次插值器中
// GridArrayAdapter 的实现这里省略了，想了解具体实现的可以在 Cartographer 的代码里找到
// 功能主要是在概率栅格图对应的 index 中取出相应的概率值
const GridArrayAdapter adapter(grid_);
ceres::BiCubicInterpolator interpolator(adapter); // 双三次插值器
const MapLimits& limits = grid_.limits();
// 遍历 point_cloud_（当前 scan）中的所有点，用变换矩阵将其变换到 global map 的坐标系中
// 取出每个点对应的栅格地图概率（双三次插值之后的）p
// 我们要求的是这个 p 最大的情况，
// 这个残差的纬度就是 point_cloud_ 中的点数
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), // 行 转换为整形地图中的x和y的位置 也就是栅格坐标
(limits.max().y() - world[1]) / limits.resolution() - 0.5 +
static_cast(kPadding), // 列
&residual[i]); // 残差
// free值越小, 表示占用的概率越大
residual[i] = scaling_factor_ * residual[i];
}
return true;
} 这里 free的值来自概率表 value_to_correspondence_cost_table_
2.对计算的位移与推测的位移优化部分
problem.AddResidualBlock(
TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
options_.translation_weight(), target_translation), // 平移的目标值, 没有使用校准后的平移
nullptr /* loss function */, ceres_pose_estimate); // 平移的初值costfunction为：
template <typename T>
bool operator()(const T * const pose, T* residual) const
{
// 获得一个 2 维的残差，即 x,y 方向上的位移
residual[0] = scaling_factor_ * (pose[0] - x_); // 权重 * dx
residual[1] = scaling_factor_ * (pose[1] - y_);
return true;
}
3.计算角度的优化
problem.AddResidualBlock(
RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
options_.rotation_weight(), ceres_pose_estimate[2]), // 角度的目标值
nullptr /* loss function */, ceres_pose_estimate); // 角度的初值costfunction为：
template
bool operator()(const T * const pose, T* residual) const
{
residual[0] = scaling_factor_ * (pose[2] - angle_);
return true;
} 二、 optimization_problem_2d.cc里的优化
1.对约束的位置三维度求残差
problem.AddResidualBlock(
// 根据SPA论文中的公式计算出的残差的CostFunction
CreateAutoDiffSpaCostFunction(constraint.pose),
// Loop closure constraints should have a loss function.
// 为闭环约束提供一个Huber的核函数,用于降低错误的闭环检测对最终的优化结果带来的负面影响
constraint.tag == Constraint::INTER_SUBMAP // 核函数 intra_submap表示node在submap内 ,inter_submap表示node与submap相关
? new ceres::HuberLoss(options_.huber_scale())
: nullptr,
C_submaps.at(constraint.submap_id).data(), // 2个优化变量
C_nodes.at(constraint.node_id).data());这里会进 spa.cost_function_2d.cc然后进 cost_helpers_impl.h里的 ComputeUnscaledError 和 ScaleError
这两块的说明在注释里写清楚了
/**
* @brief 2d 根据SPA论文里的公式求残差
*
* 计算残差：
* T12 = T1.inverse() * T2
* [R1.inverse * R2, R1.inverse * (t2 -t1)]
* [0 , 1 ]
*
* @param[in] relative_pose
* @param[in] start
* @param[in] end
* @return std::array
* 这里 relative_pose 为论文中的计算值 zij start与end的实际值offset 为h(ci,cj)
*
* 根据论文计算公式是这样的，先把角度theta 转换为2*2的旋转矩阵R [ cos(theta) -sin(theta)
* sin(theta) cos(theta) ]
* 再对R取转置 RT [ cos(theta) sin(theta)
* -sin(theta) cos(theta) ]
* ci,cj的 h(ci,cj) = { RT(tj-ti)
* theta(j)-theta(i)
* }
* 所以RT（tj-ti） = [ cos(theta) sin(theta)
* -sin(theta) cos(theta) ] [ deltax deltay]
*
*下面的h3就是这么算的
*
*
* eij = zij -h(ci,cj)
* 不明白的地方去看 Efficient Sparse Pose Adjustment for 2D Mapping 这个论文
*/
template
static std::array ComputeUnscaledError(
const transform::Rigid2d& relative_pose, const T * const start, const T * const end)
{
const T cos_theta_i = cos(start[2]);
const T sin_theta_i = sin(start[2]);
const T delta_x = end[0] - start[0];
const T delta_y = end[1] - start[1];
const T h[3] = {cos_theta_i* delta_x + sin_theta_i * delta_y,
-sin_theta_i * delta_x + cos_theta_i * delta_y,
end[2] - start[2]};
return {{T(relative_pose.translation().x()) - h[0],
T(relative_pose.translation().y()) - h[1],
common::NormalizeAngleDifference(
T(relative_pose.rotation().angle()) - h[2])}};
}
// 2d 为残差中的xy与theta分别乘上不同的权重
template
std::array ScaleError(const std::array& error, double translation_weight, double rotation_weight)
{
// clang-format off
return {{
error[0] * translation_weight,
error[1] * translation_weight,
error[2] * rotation_weight
}};
// clang-format on
} 2. 对landmark求残差，这里代码赋值太长，残差模型和1一样的，不再丢代码了。
3.对odom三维度求残差
problem.AddResidualBlock(
CreateAutoDiffSpaCostFunction(Constraint::Pose {
*relative_odometry, options_.odometry_translation_weight(),
options_.odometry_rotation_weight()
}),
nullptr /* loss function */, C_nodes.at(first_node_id).data(),
C_nodes.at(second_node_id).data());costfunction和前面一样不讲了。
4.计算两个节点在local坐标系下的残差
problem.AddResidualBlock(
CreateAutoDiffSpaCostFunction(
Constraint::Pose {relative_local_slam_pose,
options_.local_slam_pose_translation_weight(),
options_.local_slam_pose_rotation_weight()}),
nullptr /* loss function */, C_nodes.at(first_node_id).data(),
C_nodes.at(second_node_id).data());costfunction和前面一样不讲了。