里程计残差，GPS残差
Kong Liangqian Lv6
 2021-12-15 18:38:29  

优化里程计与节点的残差

节点与节点间在global坐标系下的相对坐标变换 与 通过里程计数据插值出的相对坐标变换 的差值作为残差项

  • 第一种坐标变换: 相邻2个节间在global坐标系下的坐标变换
  • 第二种坐标变换: 再分别计算这2个节点的时间在里程计数据队列中插值得到的2个里程计位姿, 计算这2个里程计位姿间的坐标变换

实现

遍历所有的轨迹节点，这个数据是相当于从第一条轨迹的开头到最后一条轨迹的末尾

1
2
3
4
5
6
// 遍历多个轨迹, 添加里程计与local结果的残差
  for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
    // 获取每个节点的轨迹id
    const int trajectory_id = node_it->id.trajectory_id;
    // 获取这条轨迹的最后一个位置的迭代器
    const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);

对于每一个节点，获得他的轨迹ID和此轨迹最后的位置，如果轨迹是冻结的就跳过。记录此节点为prev_node_it

1
2
3
4
5
6
7
// 如果轨迹是frozen的, 则无需处理直接跳过
if (frozen_trajectories.count(trajectory_id) != 0) {
  node_it = trajectory_end;
  continue;
}

auto prev_node_it = node_it;

拿到轨迹对轨迹的节点进行遍历，首先对node_it进行了++，这已经是下一个节点了

分别对上一个节点和此节点的数据和id进行记录。

把prev_node_it向后平移一个

1
2
3
4
5
6
7
8
// 遍历一个轨迹的所有节点, 添加里程计与local结果的残差
for (++node_it; node_it != trajectory_end; ++node_it) {
  const NodeId first_node_id = prev_node_it->id;
  const NodeSpec2D& first_node_data = prev_node_it->data;
  prev_node_it = node_it;

  const NodeId second_node_id = node_it->id;
  const NodeSpec2D& second_node_data = node_it->data;

根据里程计数据算出两个节点间的相对坐标变换，疑问（这个local和global的相对的坐标一样么？）

1
2
3
4
5
6
7
8
9
10
11
// 如果节点的索引不连续, 跳过
if (second_node_id.node_index != first_node_id.node_index + 1) {
  continue;
}

// Add a relative pose constraint based on the odometry (if available).
// 根据里程计数据进行插值得到的2个节点间的坐标变换
// 根据里程计数据算出两个节点间的相对坐标变换
std::unique_ptr<transform::Rigid3d> relative_odometry =
    CalculateOdometryBetweenNodes(trajectory_id, first_node_data,
                                  second_node_data);

两节点对应时间的里程计相对位姿

疑问：还要计算里程计的相对位姿，里程计表示不准么？

根据里程计数据算出两个节点间的相对坐标变换

总体流程：

  • 获取两个数据相应时间的odom位姿信息
  • 通过位姿变换求得这两个odom的相对位姿，是second在first下的位姿

注意的是，位姿*gravity_alignment.inverse() 就是2d的了

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
35
36
/**
 * @brief 根据里程计数据算出两个节点间的相对坐标变换
 * 
 * @param[in] trajectory_id 轨迹的id
 * @param[in] first_node_data 前一个节点数据
 * @param[in] second_node_data 后一个节点数据
 * @return std::unique_ptr<transform::Rigid3d> 两个节点的坐标变换
 */
std::unique_ptr<transform::Rigid3d>
OptimizationProblem2D::CalculateOdometryBetweenNodes(
    const int trajectory_id, const NodeSpec2D& first_node_data,
    const NodeSpec2D& second_node_data) const {

  if (odometry_data_.HasTrajectory(trajectory_id)) {
    // 插值得到time时刻的里程计数据
    const std::unique_ptr<transform::Rigid3d> first_node_odometry =
        InterpolateOdometry(trajectory_id, first_node_data.time);
    const std::unique_ptr<transform::Rigid3d> second_node_odometry =
        InterpolateOdometry(trajectory_id, second_node_data.time);

    if (first_node_odometry != nullptr && second_node_odometry != nullptr) {
      // 根据里程计数据算出的相对坐标变换
      // 需要注意的是, 实际上在optimization_problem中, node的位姿都是2d平面上的
      // 而odometry的pose是带姿态的, 因此要将轮速计插值出来的位姿转到平面上
      transform::Rigid3d relative_odometry =
          transform::Rigid3d::Rotation(first_node_data.gravity_alignment) *
          first_node_odometry->inverse() * (*second_node_odometry) *
          transform::Rigid3d::Rotation(
              second_node_data.gravity_alignment.inverse());

      return absl::make_unique<transform::Rigid3d>(relative_odometry);
    }
  }

  return nullptr;
}

添加里程计残差块

注意这里没有loss函数，疑问：为什么？残差块添加的方式和第一次添加的残差块的时候一样

1
2
3
4
5
6
7
8
9
10
11
// Step: 第三种残差 节点与节点间在global坐标系下的相对坐标变换 与 通过里程计数据插值出的相对坐标变换 的差值作为残差项
// 如果存在里程计则可增加一个残差
if (relative_odometry != nullptr) {
  problem.AddResidualBlock(
      CreateAutoDiffSpaCostFunction(Constraint::Pose{
          *relative_odometry, options_.odometry_translation_weight(),
          options_.odometry_rotation_weight()}),
      nullptr /* loss function */, 
      C_nodes.at(first_node_id).data(),
      C_nodes.at(second_node_id).data());
}

优化local和global下的节点相对位姿残差

节点与节点间在global坐标系下的相对坐标变换 与 相邻2个节点在local坐标系下的相对坐标变换的差值作为残差项

  • 第一种坐标变换: 相邻2个节间在global坐标系下的坐标变换
  • 第二种坐标变换: 相邻2个节点在local坐标系下的坐标变换

注意，他是节点间的坐标变换，第一种残差是节点和子图间的坐标变换

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
    // Add a relative pose constraint based on consecutive local SLAM poses.
    // 计算相邻2个节点在local坐标系下的坐标变换
    const transform::Rigid3d relative_local_slam_pose =
        transform::Embed3D(first_node_data.local_pose_2d.inverse() *
                           second_node_data.local_pose_2d);
    // Step: 第四种残差 节点与节点间在global坐标系下的相对坐标变换 与 相邻2个节点在local坐标系下的相对坐标变换 的差值作为残差项
    problem.AddResidualBlock(
        CreateAutoDiffSpaCostFunction(
            Constraint::Pose{relative_local_slam_pose,
                             options_.local_slam_pose_translation_weight(),
                             options_.local_slam_pose_rotation_weight()}),
        nullptr /* loss function */, 
        C_nodes.at(first_node_id).data(),
        C_nodes.at(second_node_id).data());
  }
}

到此，这个node的大循环就结束了

添加GPS的残差

暂时略过

优化变量总结

残差块全部添加完毕，最后开始优化

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
 // Solve. 进行求解
  ceres::Solver::Summary summary;
  ceres::Solve(
      common::CreateCeresSolverOptions(options_.ceres_solver_options()),
      &problem, &summary);

  // 如果开启了优化的log输出, 就输出ceres的报告
  if (options_.log_solver_summary()) {
    LOG(INFO) << summary.FullReport();
  }

  // 将优化后的所有数据进行更新 Store the result.
  for (const auto& C_submap_id_data : C_submaps) {
    submap_data_.at(C_submap_id_data.id).global_pose =
        ToPose(C_submap_id_data.data);
  }
  for (const auto& C_node_id_data : C_nodes) {
    node_data_.at(C_node_id_data.id).global_pose_2d =
        ToPose(C_node_id_data.data);
  }
  for (const auto& C_fixed_frame : C_fixed_frames) {
    trajectory_data_.at(C_fixed_frame.first).fixed_frame_origin_in_map =
        transform::Embed3D(ToPose(C_fixed_frame.second));
  }
  for (const auto& C_landmark : C_landmarks) {
    landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();
  }
}

优化的过程一共有四种变量会变化

  • submapdata 的global_pose
  • nodedata 的 global_pose
  • C_fix_frame: 和GPS相关
  • landmark的pose
 Comments