沉默杀手HomeArchivesTagsCategories
landmark的添加残差块
|激光SLAMCartographer源码解读|
Word count: 488|Reading time: 2 min
1
2
3
4
5
6
7
8
problem->AddResidualBlock(
    LandmarkCostFunction2D::CreateAutoDiffCostFunction(
        observation, prev->data, next->data),

    new ceres::HuberLoss(huber_scale), 
    prev_node_pose->data(),  next_node_pose->data(), 
    C_landmarks->at(landmark_id).rotation(),
    C_landmarks->at(landmark_id).translation() );

CreateAutoDiffCostFunction

1
2
3
4
5
6
7
8
9
10
11
12
13
14
  static ceres::CostFunction* CreateAutoDiffCostFunction(
      const LandmarkObservation& observation, const NodeSpec2D& prev_node,
      const NodeSpec2D& next_node)
{
    return new ceres::AutoDiffCostFunction<
        LandmarkCostFunction2D,
        6 /* residuals，operator() return的是6维向量 */,
                    // 所估计变量的维度
        3 /* previous node pose variables (x,y,theta) */, 
        3 /* next node pose variables  (x,y,theta)  */,
        4 /* landmark rotation variables  */,
        3 /* landmark translation variables */>(
        new LandmarkCostFunction2D(observation, prev_node, next_node));
}

operator

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
  template <typename T>
  bool operator()(const T* const prev_node_pose, const T* const next_node_pose,
                  const T* const landmark_rotation,
                  const T* const landmark_translation,
                  T* const e) const
{
    const std::tuple<std::array<T, 4>, std::array<T, 3>>
        interpolated_rotation_and_translation = InterpolateNodes2D(
            prev_node_pose, prev_node_gravity_alignment_, next_node_pose,
            next_node_gravity_alignment_, interpolation_parameter_);

    const std::array<T, 6> error = ScaleError(
        ComputeUnscaledError(
            landmark_to_tracking_transform_,
            std::get<0>(interpolated_rotation_and_translation).data(),
            std::get<1>(interpolated_rotation_and_translation).data(),
            landmark_rotation, landmark_translation),
        translation_weight_, rotation_weight_);

    std::copy(std::begin(error), std::end(error), e);
    return true;
}

ComputeUnscaledError

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
template <typename T>
static std::array<T, 6> ComputeUnscaledError(
    const transform::Rigid3d&  relative_pose, 
    const T* const start_rotation,  const T* const start_translation, 
    const T* const end_rotation,    const T* const end_translation )
{
	// 四元数共轭，在这里就是逆
  const Eigen::Quaternion<T> R_i_inverse( start_rotation[0],
                                         -start_rotation[1],
                                         -start_rotation[2],
                                         -start_rotation[3] );
    // 平移的差
  const Eigen::Matrix<T, 3, 1> delta(end_translation[0] - start_translation[0],
                                     end_translation[1] - start_translation[1],
                                     end_translation[2] - start_translation[2]);
  const Eigen::Matrix<T, 3, 1> h_translation = R_i_inverse * delta;

  const Eigen::Quaternion<T> h_rotation_inverse =
      Eigen::Quaternion<T>(end_rotation[0], -end_rotation[1], 
                          -end_rotation[2], -end_rotation[3]) *
      Eigen::Quaternion<T>(start_rotation[0], start_rotation[1],
                           start_rotation[2], start_rotation[3]);

  const Eigen::Matrix<T, 3, 1> angle_axis_difference =
      transform::RotationQuaternionToAngleAxisVector(
          h_rotation_inverse * relative_pose.rotation().cast<T>());

  return {  {T(relative_pose.translation().x()) - h_translation[0],
             T(relative_pose.translation().y()) - h_translation[1],
             T(relative_pose.translation().z()) - h_translation[2],
           angle_axis_difference[0], 
           angle_axis_difference[1],
           angle_axis_difference[2]}  };
}

relative_pose 是 landmark_to_tracking_transform，landmark和tracking坐标系的相对位姿，在SensorBridge::HandleLandmarkMessage中获得

landmark作为独立变量进入Pose Graph的稀疏优化，它增加的优化维度是 landmarks个数 * 每个landmark的参数个数，不要使用太多landmark

ScaleError一看就是残差乘以权重

1
2
3
4
5
6
7
8
9
10
11
template <typename T>
std::array<T, 3> ScaleError( const std::array<T, 3>& error,
                             double translation_weight,
                             double rotation_weight )
{
  return {{
      error[0] * translation_weight,
      error[1] * translation_weight,
      error[2] * rotation_weight
  }};
}