深入分析 constraint_list 话题

深入分析 constraint_list 话题

这个话题是显示所有的约束，包括同一轨迹和不同轨迹的Inter约束，Intra约束。

以Inter constraints, different trajectories 和 Inter residuals, different trajectories为例进行分析，内容如下：

# 每个点的 z为0.0，省略
header: 
  seq: 0
  stamp: 
    secs: 1673860622
    nsecs: 104007813
  frame_id: "map"
ns: "Inter constraints, different trajectories"
# 12个点，有6个点相同或极为接近
points: 
  - 
    x: 2.93968786067
    y: -1.41820975755
  - 
    x: 3.44855066169
    y: -1.24014780956
  - 
    x: -0.000569545301176
    y: 0.00522600210993
  - 
    x: 3.4816963258
    y: -1.19978045681
  - 
    x: 3.45484823588
    y: -1.19817846586
  - 
    x: 3.78063457495
    y: -1.54584384415
  - 
    x: 3.45484823588
    y: -1.19817846586
  - 
    x: 4.61884003257
    y: -1.49287256215
  - 
    x: 3.45484823588
    y: -1.19817846586
  - 
    x: 4.6545497049
    y: -1.84995755765
  -
    x: 3.45484823588
    y: -1.19817846586
  - 
    x: 4.91335613134
    y: -2.16836009999


- 
header: 
  seq: 0
  stamp: 
    secs: 1673860622
    nsecs: 104007813
  frame_id: "map"
ns: "Inter residuals, different trajectories"
# 12个点，有些点很接近，结果构成的线很短
points: 
  - 
    x: 3.44855066169
    y: -1.24014780956
  - 
    x: 3.47573816704
    y: -1.1999840042
  - 
    x: 3.4816963258
    y: -1.19978045681
  - 
    x: 3.47896481246
    y: -1.20118000282
  - 
    x: 3.78063457495
    y: -1.54584384415
  - 
    x: 3.80065304682
    y: -1.55796489639
  - 
    x: 4.61884003257
    y: -1.49287256215
  - 
    x: 4.65790271882
    y: -1.44107978827
  - 
    x: 4.6545497049
    y: -1.84995755765
  - 
    x: 3.00991092288
    y: -2.53605574275
  - 
    x: 4.91335613134
    y: -2.16836009999
  - 
    x: 3.11523021674
    y: -2.61951559764

源码中可以看到Rviz显示的是LINE_LIST类型的visualization_msgs::Marker，这里要提一下Rviz显示的线类型，如下

LINE_LIST是两两连接而成的线段，在carto这里，点数一定为偶数。把上面消息内容，没两个点的坐标连线，就可以得到Rviz里的显示

黄线有6条，水绿线有2条比较长，仔细还能看到3条很短的，最后一条在坐标系附近，肉眼已经看不到。Inter约束和residual个数相同，有一个共同的端点原因可以看源码。

看MapBuilderBridge::GetConstraintList源码部分有些跳过了

const auto trajectory_node_poses =
    map_builder_->pose_graph()->GetTrajectoryNodePoses();
const auto submap_poses = map_builder_->pose_graph()->GetAllSubmapPoses();
const auto constraints = map_builder_->pose_graph()->constraints();
for (const auto& constraint : constraints)
{
    const auto submap_it = submap_poses.find(constraint.submap_id);
    if (submap_it == submap_poses.end()) {
      continue;
    }
    const auto& submap_pose = submap_it->data.pose;
    const auto node_it = trajectory_node_poses.find(constraint.node_id);
    if (node_it == trajectory_node_poses.end()) {
      continue;
    }
    const auto& trajectory_node_pose = node_it->data.global_pose;
    const Rigid3d constraint_pose = submap_pose * constraint.pose.zbar_ij;

// Inter constraints, different trajectories  线段的两个端点
    constraint_marker->points.push_back(
        ToGeometryMsgPoint(submap_pose.translation()));
    constraint_marker->points.push_back(
        ToGeometryMsgPoint(constraint_pose.translation()));
    // Inter residuals, different trajectories  线段的两个端点
    residual_marker->points.push_back(
        ToGeometryMsgPoint(constraint_pose.translation()));
    residual_marker->points.push_back(
        ToGeometryMsgPoint(trajectory_node_pose.translation()));

从最后可以看出，共同的端点是constraint_pose，map坐标系下。