landmark的添加残差块

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

  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

  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

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中获得

ScaleError一看就是残差乘以权重

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
  }};
}