Cartographer 构建约束优化问题

计算优化的思路，需要两种坐标变换，用两种坐标变化的残差求解。

第一种残差

将节点与子图原点在global坐标系下的相对位姿与约束的差值作为残差项

第一种坐标变换：节点与子图原点在global坐标系下的坐标变换

第二种坐标变换：子图内约束子图间约束

回顾一下约束的数据来源

子图类约束

      data_.constraints.push_back(
          Constraint{submap_id,
                     node_id,
                     {transform::Embed3D(constraint_transform),
                      options_.matcher_translation_weight(),
                      options_.matcher_rotation_weight()},
                     Constraint::INTRA_SUBMAP}); // 子图内约束

子图间约束

在handleWorkQueue里面进行添加

// 将计算完的约束结果进行保存, 并执行优化

void PoseGraph2D::HandleWorkQueue(
    const constraints::ConstraintBuilder2D::Result& result) {
  {
    absl::MutexLock locker(&mutex_);
    // Step: 把新计算出的约束信息添加到data_.constraints向量的末尾处
    data_.constraints.insert(data_.constraints.end(), result.begin(),
                             result.end());
  }

// Step: 第一种残差将节点与子图原点在global坐标系下的相对位姿与约束的差值作为残差项

  // Add cost functions for intra- and inter-submap constraints.
  for (const Constraint& constraint : constraints) {
    problem.AddResidualBlock(
        // 根据SPA论文中的公式计算出的残差的CostFunction
        CreateAutoDiffSpaCostFunction(constraint.pose),
        // Loop closure constraints should have a loss function.
        // 为闭环约束提供一个Huber的核函数,用于降低错误的闭环检测对最终的优化结果带来的负面影响
        constraint.tag == Constraint::INTER_SUBMAP // 核函数
            ? new ceres::HuberLoss(options_.huber_scale()) // param: huber_scale
            : nullptr,
        C_submaps.at(constraint.submap_id).data(), // 2个优化变量
        C_nodes.at(constraint.node_id).data());
  }

这里关注三个变量

constraint.pose 约束位姿变换关系

C_submaps.at(constraint.submap_id).data() 子图原点

C_nodes.at(constraint.node_id).data() 节点位姿

通过子图原点与节点位姿得到的位姿变换关系与约束的位姿变换做残差

  const T cos_theta_i = cos(start[2]);
  const T sin_theta_i = sin(start[2]);
  const T delta_x = end[0] - start[0]; // t2 -t1
  const T delta_y = end[1] - start[1];
  const T h[3] = {cos_theta_i * delta_x + sin_theta_i * delta_y, // R.inverse * (t2 -t1)
                  -sin_theta_i * delta_x + cos_theta_i * delta_y,
                  end[2] - start[2]};

CostFunction

// 根据SPA论文中的公式计算出的残差的CostFunction

CreateAutoDiffSpaCostFunction(constraint.pose)

第二种残差

landmark数据与通过2个节点位姿插值出来的相对位姿的差值作为残差项

第一种坐标变换: landmark数据的时间在2个节点位姿中插值出来的位姿

第二种坐标变换: landmark数据中的landmark_to_tracking_transform_

// Add cost functions for landmarks.

// Step: landmark数据与通过2个节点位姿插值出来的相对位姿的差值作为残差项

AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,

&problem, options_.huber_scale());

第三种残差