一、添加landmark代价中涉及到的类型

void AddLandmarkCostFunctions(
    const std::map<std::string, LandmarkNode>& landmark_nodes,
    bool freeze_landmarks, const MapById<NodeId, NodeSpec2D>& node_data,
    MapById<NodeId, std::array<double, 3>>* C_nodes,
    std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem,
    double huber_scale)

1. std::map<std::string, LandmarkNode>

2.LandmarkNode  

using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;

3.LandmarkNode struct

  struct LandmarkNode {
    struct LandmarkObservation {
      int trajectory_id;
      common::Time time;
      transform::Rigid3d landmark_to_tracking_transform;
      double translation_weight;
      double rotation_weight;
    };
    std::vector<LandmarkObservation> landmark_observations;
    absl::optional<transform::Rigid3d> global_landmark_pose;
  };

2、具体形式调用

1.pose_graph_2d.cc   void PoseGraph2D::RunOptimization()函数中调用：

  optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
                               data_.landmark_nodes);

中调用：

  // Add cost functions for landmarks.
  AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_,
                           &C_nodes, &C_landmarks, &problem,
                           options_.huber_scale());

参数：：landmark_nodes    ==data_.landmark_nodes  landmark的所有数据

freeze_landmarks = !frozen_trajectories.empty();   根据轨迹是否冻结判断

node_data_   MapById<NodeId, NodeSpec2D>   激光数据相对应的机器人位子

struct NodeId {
  NodeId(int trajectory_id, int node_index)
      : trajectory_id(trajectory_id), node_index(node_index) {}
 
  int trajectory_id;
  int node_index;
  ...  == != < ToProto
};
struct NodeSpec2D {
  common::Time time;
  transform::Rigid2d local_pose_2d;
  transform::Rigid2d global_pose_2d;
  Eigen::Quaterniond gravity_alignment;
};

&C_nodes, &C_landmarks,  是  node 局部位姿是输入，可得的全局位子。   landmark 优化后得到新位姿

MapById<SubmapId, std::array<double, 3>> C_submaps;
MapById<NodeId, std::array<double, 3>> C_nodes;
std::map<std::string, CeresPose> C_landmarks;
class CeresPose {
 public:
  CeresPose(
      const transform::Rigid3d& rigid,
      std::unique_ptr<ceres::LocalParameterization> translation_parametrization,
      std::unique_ptr<ceres::LocalParameterization> rotation_parametrization,
      ceres::Problem* problem);
  const transform::Rigid3d ToRigid() const;
  double* translation() { return data_->translation.data(); }
  const double* translation() const { return data_->translation.data(); }
  double* rotation() { return data_->rotation.data(); }
  const double* rotation() const { return data_->rotation.data(); }
  struct Data {
    std::array<double, 3> translation;
    // Rotation quaternion as (w, x, y, z).
    std::array<double, 4> rotation;
  };
  Data& data() { return *data_; }
 private:
  std::shared_ptr<Data> data_;
};

3.AddLandmarkCostFunctions

void AddLandmarkCostFunctions(
    const std::map<std::string, LandmarkNode>& landmark_nodes,
    bool freeze_landmarks, const MapById<NodeId, NodeSpec2D>& node_data,
    MapById<NodeId, std::array<double, 3>>* C_nodes,
    std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem,
    double huber_scale)

1.遍历landmark

for (const auto& landmark_node : landmark_nodes)

2. for (const auto& observation : landmark_node.second.landmark_observations)

3.  在landmark观测之前和之后找到轨迹节点。  //next 为最后一个node_data的迭代器

auto next =node_data.lower_bound(observation.trajectory_id, observation.time); 

4.landmark观察已经建立，但是轨迹没有添加时  

if (next == node_data.EndOfTrajectory(observation.trajectory_id))  continue

5.if (next == begin_of_trajectory)  next = std::next(next);

6.auto prev = std::prev(next);

7.std::array<double, 3>* prev_node_pose = &C_nodes->at(prev->id);   //观察到landmark时的前一帧node_data

 std::array<double, 3>* next_node_pose = &C_nodes->at(next->id);   //观察到landmark时的后一帧 node_data

8. const transform::Rigid3d starting_point  = 观察到的landmarkPose  或者 选择与landmark观测最接近的轨迹节点位姿

9.C_landmarks->emplace( landmark_id，starting_point（局部位姿）

10.如果landmark冻结则 该landmark 平移旋转固定

11.创建不同的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());

12.上述中 CreateAutoDiffCostFunction函数

  static ceres::CostFunction* CreateAutoDiffCostFunction(
      const LandmarkObservation& observation, const NodeSpec2D& prev_node,
      const NodeSpec2D& next_node) {
    return new ceres::AutoDiffCostFunction<
        LandmarkCostFunction2D, 6 /* residuals */,
        3 /* previous node pose variables */, 3 /* next node pose variables */,
        4 /* landmark rotation variables */,
        3 /* landmark translation variables */>(
        new LandmarkCostFunction2D(observation, prev_node, next_node));
  }

LandmarkCostFunction2D 函数

  LandmarkCostFunction2D(const LandmarkObservation& observation,
                         const NodeSpec2D& prev_node,
                         const NodeSpec2D& next_node)
      : landmark_to_tracking_transform_(
            observation.landmark_to_tracking_transform),
        prev_node_gravity_alignment_(prev_node.gravity_alignment),
        next_node_gravity_alignment_(next_node.gravity_alignment),
        translation_weight_(observation.translation_weight),
        rotation_weight_(observation.rotation_weight),
        interpolation_parameter_(
            common::ToSeconds(observation.time - prev_node.time) /
            common::ToSeconds(next_node.time - prev_node.time)) {}

该类中

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

该类中 操作服()的定义中：

1>InterpolateNodes2D函数  计算两个插值的中间位姿

template <typename T>
std::tuple<std::array<T, 4> /* rotation */, std::array<T, 3> /* translation */>
InterpolateNodes2D(const T* const prev_node_pose,
                   const Eigen::Quaterniond& prev_node_gravity_alignment,
                   const T* const next_node_pose,
                   const Eigen::Quaterniond& next_node_gravity_alignment,
                   const double interpolation_parameter)

首先介绍InterpolateNodes2D函数 返回   旋转 4 ，平移 3维

 // The following is equivalent to (Embed3D(prev_node_pose) * Rigid3d::Rotation(prev_node_gravity_alignment)).rotation().

  const Eigen::Quaternion<T> prev_quaternion(
      (Eigen::AngleAxis<T>(prev_node_pose[2], Eigen::Matrix<T, 3, 1>::UnitZ()) *
       prev_node_gravity_alignment.cast<T>())
          .normalized());
  const std::array<T, 4> prev_node_rotation = {
      {prev_quaternion.w(), prev_quaternion.x(), prev_quaternion.y(),
       prev_quaternion.z()}};

同理  next_quaternion    next_node_rotation

返回角度：   SlerpQuaternions(prev_node_rotation.data(), next_node_rotation.data(),  interpolation_parameter),

 就是  prev_scale * start[0] + next_scale * end[0]    T prev_scale(1. - factor);T next_scale(factor); 四元素的按比例插值

平移：    3维      {prev_node_pose[0] + interpolation_parameter * (next_node_pose[0] - prev_node_pose[0]),

   prev_node_pose[1] 同理   T(0)    相当于x y 前一个值+后一个值的比例

2>ComputeUnscaledError函数  计算參差

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)

首先求起始点旋转的逆R_i_inverse，然后求两个平移向量之差delta，求平移的最终差值 R_i_inverse * delta（相对与起始坐标），求旋转的逆，求角度的差值，求解最终的差值

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

2>ScaleError函数  加上权重计算   

平移和旋转分别乘以其权重

将观察到的 landmark 计算所有的參差  最小二乘跟其他一起进行优化