landmark的后端处理

landmark的后端处理 | 沉默杀手

加入landmark后，Pose Graph有了不同的node和edge，也就是landmark也作为node，landmark-pose之间的 edge是新的一种edge，即在pose对landmark的观测

struct LandmarkNode {
    struct LandmarkObservation {
      int trajectory_id;
      common::Time time;
      transform::Rigid3d landmark_to_tracking_transform;
      double translation_weight;
      double rotation_weight;
    };
    // 同一时刻，可能观测到多个landmark数据
    std::vector<LandmarkObservation> landmark_observations;
    absl::optional<transform::Rigid3d> global_landmark_pose;
    bool frozen = false;
  };

landmark是应用于后端，不是实时的，应当调大权重让carto更相信它的结果

添加数据部分

void PoseGraph2D::AddLandmarkData(int trajectory_id,
                                  const sensor::LandmarkData& landmark_data)
{
  AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    absl::MutexLock locker(&mutex_);
    if (CanAddWorkItemModifying(trajectory_id)) {
      for (const auto& observation : landmark_data.landmark_observations)
      {
      	          			// initial landmark_nodes
          // const transform::Rigid3d global_pose =
          // observation.landmark_to_map_transform;
      #if 0
      data_.landmark_nodes的类型是 std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
      #endif
        data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
            PoseGraphInterface::LandmarkNode::LandmarkObservation{
                trajectory_id, landmark_data.time,
                observation.landmark_to_tracking_transform,
                observation.translation_weight, observation.rotation_weight} );

// data_.landmark_nodes[observation.id].global_landmark_pose = global_pose;
      }
    }
    // optimization_problem_->new_landmark_add_ = true;
    return WorkItem::Result::kDoNotRunOptimization;
  });
}

这里就是把观测到的landmark信息都保存到data_.landmark_nodes，最后返回的标识也是不优化

后端优化的流程和其他传感器数据一样:
PoseGraph2D::HandleWorkQueue —— PoseGraph2D::RunOptimization —— OptimizationProblem2D::Solve(data_.constraints, GetTrajectoryStates(), data_.landmark_nodes) —— AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks, &problem, options_.huber_scale() )

OptimizationProblem2D::Solve部分：

  std::set<int> frozen_trajectories;
  for (const auto& it : trajectories_state)
  {
    if (it.second == PoseGraphInterface::TrajectoryState::FROZEN)
    {
      frozen_trajectories.insert(it.first);
    }
  }

ceres::Problem::Options problem_options;
  ceres::Problem problem(problem_options);

// Set the starting point.  TODO(hrapp): Move ceres data into SubmapSpec.
  // ceres需要的是double指针，std::array 能转成原始指针的形式
  MapById<SubmapId, std::array<double, 3>> C_submaps;
  MapById<NodeId, std::array<double, 3>> C_nodes;
  std::map<std::string, CeresPose> C_landmarks;
  bool first_submap = true;


  // 将需要优化的  子图位姿  设置为优化参数
  for (const auto& submap_id_data : submap_data_) {
    // submap_id的轨迹是否是冻结的
    const bool frozen =
        frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
        // 将子图的global_pose放入 C_submaps
    C_submaps.Insert(submap_id_data.id,
                     FromPose(submap_id_data.data.global_pose));
    // c++11中，std::array::data()返回指向数组对象第一个元素的指针
    problem.AddParameterBlock(C_submaps.at(submap_id_data.id).data(), 3);

// 第一个子图或冻结，不优化子图位姿。 也就是不优化初值
    if (first_submap || frozen) {
      first_submap = false;
      // Fix the pose of the first submap or all submaps of a frozen
      // trajectory.
      problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());
    }
  }

// 需要优化的  节点位姿  设置为优化参数，与上面的子图优化大致相同
  for (const auto& node_id_data : node_data_)
  {
    const bool frozen =
        frozen_trajectories.count(node_id_data.id.trajectory_id) != 0;
    C_nodes.Insert(node_id_data.id, FromPose(node_id_data.data.global_pose_2d));
    problem.AddParameterBlock(C_nodes.at(node_id_data.id).data(), 3);
    if (frozen)
    {
      // 这里的第一个节点也要参与优化，跟子图的不同了
      problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).data());
    }
  }
				/* ...... */
// 第2种残差：landmark与landmark数据插值出来的节点相对位姿的差值
AddLandmarkCostFunctions(landmark_nodes, node_data_, 
						&C_nodes, &C_landmarks,
                        &problem, options_.huber_scale()  );

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

AddLandmarkCostFunctions

最外层两个循环是遍历landmark节点和遍历每个节点的观测，也就是说可能会同时看到多个landmark。直接看循环里面的内容

const std::string& landmark_id = landmark_node.first;
// 轨迹中第一个 node_data
const auto& begin_of_trajectory =
    node_data.BeginOfTrajectory(observation.trajectory_id);
// 如果 landmark observation was made before the trajectory was created
if (observation.time < begin_of_trajectory->data.time) {
    continue;
}
/* 以下语句Find the trajectory nodes before and after the landmark observation */

// 找到在landmark观测时间后的第一个节点
auto next = 
    node_data.lower_bound(observation.trajectory_id, observation.time);
/*  The landmark observation was made, but the next trajectory node has
    not been added yet. 即next已经是轨迹最后一个节点  */
if (next == node_data.EndOfTrajectory(observation.trajectory_id)  ) {
    continue;
}
// 如果是刚开始的node data
if (next == begin_of_trajectory) {
  next = std::next(next);
}
// 这里的pre配合next是为了获取两个位置，找到landmark观测时间的前一个节点
auto prev = std::prev(next);

// 根据两个索引，获取两个节点位姿
// Add parameter blocks for the landmark ID if they were not added before
std::array<double, 3>* prev_node_pose = &C_nodes->at(prev->id);
std::array<double, 3>* next_node_pose = &C_nodes->at(next->id);

根据landmark观测的时间，找到观测前后的节点，获取位姿

// 如果landmark_id 不存在于 C_landmarks，
if (!C_landmarks->count(landmark_id)){
  // landmark如果已经优化，则 global_landmark_pose 有值
  // 否则根据时间插值计算一个位姿
  const transform::Rigid3d starting_point =
      landmark_node.second.global_landmark_pose.has_value()
          ? landmark_node.second.global_landmark_pose.value()
          : GetInitialLandmarkPose(observation, prev->data, next->data,
                                   *prev_node_pose, *next_node_pose);

开始C_landmarks当然为空，只有在优化完成后，这里才存在，即 RunOptimization 的最后会添加: data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second;，顺着上面的调用路线就能发现

GetInitialLandmarkPose

这里用了一堆数据类型转换，以及四元数、旋转向量、欧拉角之间的转换。还重新实现了slerp函数SlerpQuaternions，因为Eigen的slerp与Ceres的不兼容。

  C_landmarks->emplace(
      landmark_id,
      // 四元数不支持广义加法，使用QuaternionParameterization 
      CeresPose(starting_point, nullptr /* translation_parametrization */,
                absl::make_unique<ceres::QuaternionParameterization>(),
                problem)  );

// Set landmark constant if it is frozen. 不参与优化
  // 这里容易记不清，frozen 定义在  struct LandmarkNode
  if (landmark_node.second.frozen) {
    problem->SetParameterBlockConstant(
        C_landmarks->at(landmark_id).translation());

problem->SetParameterBlockConstant(
        C_landmarks->at(landmark_id).rotation());
  }
}

CeresPose的构造函数里就两行

problem->AddParameterBlock(data_->translation.data(), 3,
                         translation_parametrization.release());
problem->AddParameterBlock(data_->rotation.data(), 4,
                         rotation_parametrization.release());

对于平移，无需ceres::LocalParameterization

添加残差块

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() );

if (!C_landmarks->count(landmark_id))已经结束，添加残差块。

Solve 和 RunOptimization 的最后处理

Solve的最后

for (const auto& C_landmark : C_landmarks) {
	// first 是 landmark_id
	landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();
}

这个C_landmarks还是上面那个，ToRigid()当然是优化后的landmark全局位姿了

在RunOptimization的最后

// 遍历成员变量 landmark_data_，它在 Solve 函数的最后赋值
for (const auto& landmark : optimization_problem_->landmark_data() )
{
	 // first 是 landmark_id
  data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second;
}

optimization_problem_->landmark_data()返回的就是上面的landmark_data_，说白了，这里就是把优化后的landmark数据从OptimizationProblem2D层传回PoseGraph2D层