landmark的前端处理

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的构造函数里体现,注意设置为onoff不是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数据直接进后端,前端没有它的事了