沉默杀手
landmark的前端处理
|激光SLAMCartographer源码解读|
Word count: 628|Reading time: 3 min

landmark就是添加了一个观测，让定位更鲁棒。如果观测是空的，应当发布空的 landmark messages to unblock the queue. 对于fixed frame poses (GPS)也适用。

前端的处理

node.cc 层

1
2
3
4
// landmark_poses_list 话题
  landmark_poses_list_publisher_ =
      node_handle_.advertise<::visualization_msgs::MarkerArray>(
          kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);

发布话题：

1
2
3
4
5
6
7
8
void Node::PublishLandmarkPosesList(const ::ros::WallTimerEvent& unused_timer_event)
{
  if (landmark_poses_list_publisher_.getNumSubscribers() > 0)
  {
      absl::MutexLock lock(&mutex_);
      landmark_poses_list_publisher_.publish(map_builder_bridge_.GetLandmarkPosesList() );
  }
}

处理传感器数据

1
2
3
4
5
6
7
8
9
10
11
void Node::HandleLandmarkMessage(
    const int trajectory_id, const std::string& sensor_id,
    const cartographer_ros_msgs::LandmarkList::ConstPtr& msg)
{
  absl::MutexLock lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) 
      return;
  CHECK(msg->landmarks.size() != 0) << "No Landmarks !";

  map_builder_bridge_.sensor_bridge(trajectory_id)->HandleLandmarkMessage(sensor_id, msg);
}

sensor_bridge.cc 层

用到的类型

1
2
3
4
5
6
7
8
9
10
11
12
struct LandmarkData {
  common::Time time;
  std::vector<LandmarkObservation> landmark_observations;
};

struct LandmarkObservation
{
  std::string id;
  transform::Rigid3d landmark_to_tracking_transform;
  double translation_weight;
  double rotation_weight;
};

如果是二维码，就可以将识别到的ID传进去，landmark的观测是相对于 tracking 坐标系的

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
void SensorBridge::HandleLandmarkMessage(
    const std::string& sensor_id,
    const cartographer_ros_msgs::LandmarkList::ConstPtr& msg)
{
  auto landmark_data = ToLandmarkData(*msg);
  auto tracking_from_landmark_sensor = tf_bridge_.LookupToTracking(
      landmark_data.time, CheckNoLeadingSlash(msg->header.frame_id));
  // 把相机对landmark的观测，转换到 tracking 坐标系
  if (tracking_from_landmark_sensor != nullptr)
  {
      for (auto& observation : landmark_data.landmark_observations)
      {
        observation.landmark_to_tracking_transform =
            *tracking_from_landmark_sensor *
            observation.landmark_to_tracking_transform;
      }
  }
  // 进入传感器数据融合阶段
  trajectory_builder_->AddSensorData(sensor_id, landmark_data);
}

简单的类型转换而已

1
2
3
4
5
6
7
8
9
10
11
12
13
14
LandmarkData ToLandmarkData(const LandmarkList& landmark_list)
{
    LandmarkData landmark_data;
    landmark_data.time = FromRos(landmark_list.header.stamp);
    for (const LandmarkEntry& entry : landmark_list.landmark)
    {
        landmark_data.landmark_observations.push_back(
        {entry.id, ToRigid3d(entry.tracking_from_landmark_transform),
         entry.translation_weight, 10, entry.type});
         //LOG(INFO) << "find landmark x: " << entry.tracking_from_landmark_transform.position.x;
         //LOG(INFO) << "find landmark y: " << entry.tracking_from_landmark_transform.position.y;
    }
    return landmark_data;
}

数据融合阶段

CollatedTrajectoryBuilder类

1
2
3
4
5
6
7
8
9
10
11
12
void AddSensorData(const std::string& sensor_id,
                   const sensor::LandmarkData& landmark_data) override
{
    if (collate_landmarks_)
    {
      AddData(sensor::MakeDispatchable(sensor_id, landmark_data));
      return;
    }
    // wrapped_trajectory_builder_ 赋值在 CollatedTrajectoryBuilder 构造函数
    // 应当为map_builder_->GetTrajectoryBuilder(trajectory_id)
    wrapped_trajectory_builder_->AddSensorData(sensor_id, landmark_data);
}

  • collate_landmarks的设置问题

在CollatedTrajectoryBuilder的构造函数里体现，注意设置为on或off，不是true, false

1
2
3
4
5
if (sensor_id.type == SensorId::SensorType::LANDMARK &&
    !collate_landmarks_)
{
    continue;
}

如果提供landmark的观测不低于10 Hz，那么可以设置TRAJECTORY_BUILDER.collate_landmarks = on. Cartographer将deterministically运行(对于给定的bag, 使用offline node每次获得的结果是一样的). 如果collate_landmarks = off， landmark observations将跳过sensor queue (so they do not block it if they are sparse) and are injected directly into the pose graph, which is not deterministic.

但是看到另一种说法，在 #1224更新版本，作者提出如果有landmark，collate_landmarks就要为 false。这个再观察

接下来进入GlobalTrajectoryBuilder

1
2
3
4
5
void AddSensorData(const std::string& sensor_id,
                   const sensor::LandmarkData& landmark_data) override
{
   pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);
}

这里可以看到landmark数据直接进后端，前端没有它的事了