Cartographer-2D数据流动-数据分发
承接上一篇:
轨迹添加
Node
// 接收轮速计数据
void Node::HandleOdometryMessage(const int trajectory_id,
const std::string& sensor_id,
const nav_msgs::Odometry::ConstPtr& msg) {
carto::common::MutexLocker lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) {
return;
}
// 获取该条轨迹的SensorBridge
auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
// 转换格式
auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg);
if (odometry_data_ptr != nullptr) {
// Node位姿外推器加入数据,推算位姿
extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr);
}
// 传入SensorBridge
sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
}
// 接收IMU数据
void Node::HandleImuMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg) {
carto::common::MutexLocker lock(&mutex_);
// 计数器
if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
return;
}
// 获取该条轨迹的SensorBridge
auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
// 转换格式
auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
if (imu_data_ptr != nullptr) {
// Node位姿外推器加入数据,推算位姿
extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
}
// 传入SensorBridge
sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}
void Node::HandleLaserScanMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::LaserScan::ConstPtr& msg) {
carto::common::MutexLocker lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
// 传入SensorBridge
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLaserScanMessage(sensor_id, msg);
}
在Node.cc中,将数据通过trajecotry_id传进相应的SensorBridge中进行处理。
关于计数器原理:
假如历史上一共收到了N帧数据,之前进入前端和后端的数据有n帧,当前帧来之后,判断:
n
/
(
N
+
1
)
n/(N+1)
n/(N+1)是否小于阈值(0-1),如果小于阈值,则该帧数据进入前端和后端,如果大于阈值,则丢弃该帧。N变为N+1。
SensorBridge
class SensorBridge {
public:
explicit SensorBridge(
int num_subdivisions_per_laser_scan, const std::string& tracking_frame,
double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer,
::cartographer::mapping::TrajectoryBuilderInterface* trajectory_builder);
SensorBridge(const SensorBridge&) = delete;
SensorBridge& operator=(const SensorBridge&) = delete;
// 转换数据类型
std::unique_ptr<::cartographer::sensor::OdometryData> ToOdometryData(
const nav_msgs::Odometry::ConstPtr& msg);
void HandleOdometryMessage(const std::string& sensor_id,
const nav_msgs::Odometry::ConstPtr& msg);
void HandleNavSatFixMessage(const std::string& sensor_id,
const sensor_msgs::NavSatFix::ConstPtr& msg);
void HandleLandmarkMessage(
const std::string& sensor_id,
const cartographer_ros_msgs::LandmarkList::ConstPtr& msg);
// 转换数据类型
std::unique_ptr<::cartographer::sensor::ImuData> ToImuData(
const sensor_msgs::Imu::ConstPtr& msg);
void HandleImuMessage(const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg);
void HandleLaserScanMessage(const std::string& sensor_id,
const sensor_msgs::LaserScan::ConstPtr& msg);
void HandleMultiEchoLaserScanMessage(
const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
void HandlePointCloud2Message(const std::string& sensor_id,
const sensor_msgs::PointCloud2::ConstPtr& msg);
const TfBridge& tf_bridge() const;
private:
void HandleLaserScan(
const std::string& sensor_id, ::cartographer::common::Time start_time,
const std::string& frame_id,
const ::cartographer::sensor::PointCloudWithIntensities& points);
void HandleRangefinder(const std::string& sensor_id,
::cartographer::common::Time time,
const std::string& frame_id,
const ::cartographer::sensor::TimedPointCloud& ranges);
const int num_subdivisions_per_laser_scan_;
std::map<std::string, cartographer::common::Time>
sensor_to_previous_subdivision_time_;
const TfBridge tf_bridge_;
::cartographer::mapping::TrajectoryBuilderInterface* const
trajectory_builder_;
::cartographer::common::optional<::cartographer::transform::Rigid3d>
ecef_to_local_frame_;
};
重点关注
(1)激光数据处理:
void SensorBridge::HandleLaserScanMessage(
const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
carto::sensor::PointCloudWithIntensities point_cloud;
carto::common::Time time;
// 转换数据类型
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}
void SensorBridge::HandleMultiEchoLaserScanMessage(
const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
carto::sensor::PointCloudWithIntensities point_cloud;
carto::common::Time time;
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg) {
return LaserScanToPointCloudWithIntensities(msg);
}
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg) {
return LaserScanToPointCloudWithIntensities(msg);
}
// 将激光数据转换为点云,点云中的每个点为xyz和time,该time都是负值,即接收到该点时的时间戳,减去最后一个点的时间戳。这样处理是为了在前端做畸变校正,去除运动带来的畸变
template <typename LaserMessageType>
std::tuple<PointCloudWithIntensities, ::cartographer::common::Time>
LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
CHECK_GE(msg.range_min, 0.f);
CHECK_GE(msg.range_max, msg.range_min);
if (msg.angle_increment > 0.f) {
CHECK_GT(msg.angle_max, msg.angle_min);
} else {
CHECK_GT(msg.angle_min, msg.angle_max);
}
PointCloudWithIntensities point_cloud;
float angle = msg.angle_min;
for (size_t i = 0; i < msg.ranges.size(); ++i) {
const auto& echoes = msg.ranges[i];
if (HasEcho(echoes)) {
const float first_echo = GetFirstEcho(echoes);
if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
Eigen::Vector4f point;
point << rotation * (first_echo * Eigen::Vector3f::UnitX()),
i * msg.time_increment;
point_cloud.points.push_back(point);
if (msg.intensities.size() > 0) {
CHECK_EQ(msg.intensities.size(), msg.ranges.size());
const auto& echo_intensities = msg.intensities[i];
CHECK(HasEcho(echo_intensities));
point_cloud.intensities.push_back(GetFirstEcho(echo_intensities));
} else {
point_cloud.intensities.push_back(0.f);
}
}
}
angle += msg.angle_increment;
}
::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
if (!point_cloud.points.empty()) {
const double duration = point_cloud.points.back()[3];
timestamp += cartographer::common::FromSeconds(duration);
for (Eigen::Vector4f& point : point_cloud.points) {
point[3] -= duration;
}
}
return std::make_tuple(point_cloud, timestamp);
}
// 激光数据分割成几个部分,并保证每个部分的最后一个点的时间戳为0
void SensorBridge::HandleLaserScan(
const std::string& sensor_id, const carto::common::Time time,
const std::string& frame_id,
const carto::sensor::PointCloudWithIntensities& points) {
if (points.points.empty()) {
return;
}
CHECK_LE(points.points.back()[3], 0);
// TODO(gaschler): Use per-point time instead of subdivisions
for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
const size_t start_index =
points.points.size() * i / num_subdivisions_per_laser_scan_;
const size_t end_index =
points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
carto::sensor::TimedPointCloud subdivision(
points.points.begin() + start_index, points.points.begin() + end_index);
if (start_index == end_index) {
continue;
}
const double time_to_subdivision_end = subdivision.back()[3];
// `subdivision_time` is the end of the measurement so sensor::Collator will
// send all other sensor data first.
const carto::common::Time subdivision_time =
time + carto::common::FromSeconds(time_to_subdivision_end);
auto it = sensor_to_previous_subdivision_time_.find(sensor_id);
if (it != sensor_to_previous_subdivision_time_.end() &&
it->second >= subdivision_time) {
LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "
<< sensor_id << " because previous subdivision time "
<< it->second << " is not before current subdivision time "
<< subdivision_time;
continue;
}
sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
for (Eigen::Vector4f& point : subdivision) {
point[3] -= time_to_subdivision_end;
}
CHECK_EQ(subdivision.back()[3], 0);
HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
}
}
// 将数据从激光雷达坐标系转换到tracking坐标系,并将转换后的数据传给SensorBridge的trajectory_builder_,该trajectory_builder_是MapBuiler中的CollectorTrajectoryBuilder
void SensorBridge::HandleRangefinder(
const std::string& sensor_id, const carto::common::Time time,
const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
const auto sensor_to_tracking =
tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
if (sensor_to_tracking != nullptr) {
trajectory_builder_->AddSensorData(
sensor_id, carto::sensor::TimedPointCloudData{
time, sensor_to_tracking->translation().cast<float>(),
carto::sensor::TransformTimedPointCloud(
ranges, sensor_to_tracking->cast<float>())});
}
}
(2)轮速计数据处理:
void SensorBridge::HandleOdometryMessage(
const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
std::unique_ptr<carto::sensor::OdometryData> odometry_data =
ToOdometryData(msg);
if (odometry_data != nullptr) {
// 将转换后的数据传给SensorBridge的trajectory_builder_,该trajectory_builder_是MapBuiler中的CollectorTrajectoryBuilder
trajectory_builder_->AddSensorData(
sensor_id,
carto::sensor::OdometryData{odometry_data->time, odometry_data->pose});
}
}
// 转换到tracking坐标系下,并转换数据格式
std::unique_ptr<carto::sensor::OdometryData> SensorBridge::ToOdometryData(
const nav_msgs::Odometry::ConstPtr& msg) {
const carto::common::Time time = FromRos(msg->header.stamp);
const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
time, CheckNoLeadingSlash(msg->child_frame_id));
if (sensor_to_tracking == nullptr) {
return nullptr;
}
return carto::common::make_unique<carto::sensor::OdometryData>(
carto::sensor::OdometryData{
time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()});
}
(3)IMU数据
void SensorBridge::HandleImuMessage(const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg) {
std::unique_ptr<carto::sensor::ImuData> imu_data = ToImuData(msg);
if (imu_data != nullptr) {
// 将数据从激光雷达坐标系转换到tracking坐标系,并将转换后的数据传给SensorBridge的trajectory_builder_,该trajectory_builder_是MapBuiler中的CollectorTrajectoryBuilder
trajectory_builder_->AddSensorData(
sensor_id,
carto::sensor::ImuData{imu_data->time, imu_data->linear_acceleration,
imu_data->angular_velocity});
}
}
// 转换到tracking坐标系下,并转换数据格式
std::unique_ptr<carto::sensor::ImuData> SensorBridge::ToImuData(
const sensor_msgs::Imu::ConstPtr& msg) {
CHECK_NE(msg->linear_acceleration_covariance[0], -1)
<< "Your IMU data claims to not contain linear acceleration measurements "
"by setting linear_acceleration_covariance[0] to -1. Cartographer "
"requires this data to work. See "
"http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html.";
CHECK_NE(msg->angular_velocity_covariance[0], -1)
<< "Your IMU data claims to not contain angular velocity measurements "
"by setting angular_velocity_covariance[0] to -1. Cartographer "
"requires this data to work. See "
"http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html.";
const carto::common::Time time = FromRos(msg->header.stamp);
const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
time, CheckNoLeadingSlash(msg->header.frame_id));
if (sensor_to_tracking == nullptr) {
return nullptr;
}
CHECK(sensor_to_tracking->translation().norm() < 1e-5)
<< "The IMU frame must be colocated with the tracking frame. "
"Transforming linear acceleration into the tracking frame will "
"otherwise be imprecise.";
return carto::common::make_unique<carto::sensor::ImuData>(
carto::sensor::ImuData{
time,
sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)});
}
CollatedTrajectoryBuilder
数据传入:
// 点云数据
void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) override {
AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
}
// IMU数据
void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) override {
AddData(sensor::MakeDispatchable(sensor_id, imu_data));
}
// 轮速计数据
void AddSensorData(const std::string& sensor_id,
const sensor::OdometryData& odometry_data) override {
AddData(sensor::MakeDispatchable(sensor_id, odometry_data));
}
数据统一组织:
template <typename DataType>
class Dispatchable : public Data {
public:
Dispatchable(const std::string &sensor_id, const DataType &data)
: Data(sensor_id), data_(data) {}
common::Time GetTime() const override { return data_.time; }
// 在第一篇中,CollatedTrajectoryBuilder::HandleCollatedSensorData函数调用该函数,且传进来的TrajectoryBuilderInterface是MapBuilder的GlobalTrajectoryBuilder
void AddToTrajectoryBuilder(
mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
// 调用GlobalTrajectoryBuilder的>AddSensorData
trajectory_builder->AddSensorData(sensor_id_, data_);
}
const DataType &data() const { return data_; }
private:
const DataType data_;
};
template <typename DataType>
std::unique_ptr<Dispatchable<DataType>> MakeDispatchable(
const std::string &sensor_id, const DataType &data) {
return common::make_unique<Dispatchable<DataType>>(sensor_id, data);
}
调用同一接口,将数据放入MapBuilder的数据分发器中
void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}
数据分发器:
class Collator : public CollatorInterface {
public:
Collator() {}
Collator(const Collator&) = delete;
Collator& operator=(const Collator&) = delete;
// CollatedTrajectoryBuilder的HandleCollatedSensorData作为回调函数callback传进来
void AddTrajectory(int trajectory_id,
const std::unordered_set<std::string>& expected_sensor_ids,
const Callback& callback) override;
void FinishTrajectory(int trajectory_id) override;
// 数据传入接口
void AddSensorData(int trajectory_id, std::unique_ptr<Data> data) override;
void Flush() override;
common::optional<int> GetBlockingTrajectoryId() const override;
private:
// Queue keys are a pair of trajectory ID and sensor identifier.
// 队列里存放着所有的数据,包括轨迹的轮速计数据和激光数据,并且根据QueueKey分发到不同的轨迹中,并执行相应的回调函数
OrderedMultiQueue queue_;
// Map of trajectory ID to all associated QueueKeys.
// 队列queue_keys_里管理着<trajectory_id, <trajectory_id, sensor_id>>
std::unordered_map<int, std::vector<QueueKey>> queue_keys_;
};
// CollatedTrajectoryBuilder的构造函数调用该函数
void Collator::AddTrajectory(
const int trajectory_id,
const std::unordered_set<std::string>& expected_sensor_ids,
const Callback& callback) {
for (const auto& sensor_id : expected_sensor_ids) {
const auto queue_key = QueueKey{trajectory_id, sensor_id};
// 根据QueueKey,将对应的回调函数callback(CollatedTrajectoryBuilder::HandleCollatedSensorData)放queue_(OrderedMultiQueue::AddQueue)里
queue_.AddQueue(queue_key,
[callback, sensor_id](std::unique_ptr<Data> data) {
callback(sensor_id, std::move(data));
});
// 添加queue_keys_
queue_keys_[trajectory_id].push_back(queue_key);
}
}
// CollatedTrajectoryBuilder的AddSensorData调用该函数,将sensor::Dispatchable数据放到queue_(OrderedMultiQueue::Add)里
void Collator::AddSensorData(const int trajectory_id,
std::unique_ptr<Data> data) {
// 构造 QueueKey
QueueKey queue_key{trajectory_id, data->GetSensorId()};
// 将sensor::Dispatchable及其对应的QueueKey放入queue_(OrderedMultiQueue::Add)里
queue_.Add(std::move(queue_key), std::move(data));
}
数据分发器将数据接收后,将数据及其处理函数(CollatedTrajectoryBuilder::HandleCollatedSensorData)放入OrderedMultiQueue中。
按时间分发队列OrderedMultiQueue
class OrderedMultiQueue {
public:
using Callback = std::function<void(std::unique_ptr<Data>)>;
OrderedMultiQueue();
~OrderedMultiQueue();
// 将轨迹中QueueKey和对应的回调函数,放到queues_里
// Adds a new queue with key 'queue_key' which must not already exist.
// 'callback' will be called whenever data from this queue can be dispatched.
void AddQueue(const QueueKey& queue_key, Callback callback);
// Marks a queue as finished, i.e. no further data can be added. The queue
// will be removed once the last piece of data from it has been dispatched.
void MarkQueueAsFinished(const QueueKey& queue_key);
// 数据传入接口
// Adds 'data' to a queue with the given 'queue_key'. Data must be added
// sorted per queue.
void Add(const QueueKey& queue_key, std::unique_ptr<Data> data);
// Dispatches all remaining values in sorted order and removes the underlying
// queues.
void Flush();
// Must only be called if at least one unfinished queue exists. Returns the
// key of a queue that needs more data before the OrderedMultiQueue can
// dispatch data.
QueueKey GetBlocker() const;
private:
struct Queue {
common::BlockingQueue<std::unique_ptr<Data>> queue;
Callback callback;
bool finished = false;
};
void Dispatch();
void CannotMakeProgress(const QueueKey& queue_key);
common::Time GetCommonStartTime(int trajectory_id);
// Used to verify that values are dispatched in sorted order.
common::Time last_dispatched_time_ = common::Time::min();
std::map<int, common::Time> common_start_time_per_trajectory_;
std::map<QueueKey, Queue> queues_;
QueueKey blocker_;
};
将数据的回调函数及queue_key加入队列:
// 将轨迹中QueueKey和对应的回调函数,放到queues_里
void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) {
CHECK_EQ(queues_.count(queue_key), 0);
queues_[queue_key].callback = std::move(callback);
}
加入数据:
void OrderedMultiQueue::Add(const QueueKey& queue_key,
std::unique_ptr<Data> data) {
// 根据QueueKey(trajectory_id,sensor_id),找到该数据对应的队列
auto it = queues_.find(queue_key);
if (it == queues_.end()) {
LOG_EVERY_N(WARNING, 1000)
<< "Ignored data for queue: '" << queue_key << "'";
return;
}
// 将数据放入具有指定QueueKey的队列中
it->second.queue.Push(std::move(data));
// 分发数据
Dispatch();
}
分发数据:
// 数据分发函数
void OrderedMultiQueue::Dispatch() {
while (true) {
const Data* next_data = nullptr;
Queue* next_queue = nullptr;
QueueKey next_queue_key;
// 遍历所有的队列,找到所有队列中最老的数据next_data
for (auto it = queues_.begin(); it != queues_.end();) {
const auto* data = it->second.queue.Peek<Data>();
if (data == nullptr) {
// 删除finished队列,不再给finished的trajectory分发数据
if (it->second.finished) {
queues_.erase(it++);
continue;
}
// 标记没有收到数据
CannotMakeProgress(it->first);
return;
}
if (next_data == nullptr || data->GetTime() < next_data->GetTime()) {
next_data = data;
next_queue = &it->second;
next_queue_key = it->first;
}
// 必须是时间有序的数据,否则会报错
CHECK_LE(last_dispatched_time_, next_data->GetTime())
<< "Non-sorted data added to queue: '" << it->first << "'";
++it;
}
// 队列为空的情况
if (next_data == nullptr) {
CHECK(queues_.empty());
return;
}
// 分发最老的数据next_data
// If we haven't dispatched any data for this trajectory yet, fast forward
// all queues of this trajectory until a common start time has been reached.
const common::Time common_start_time =
GetCommonStartTime(next_queue_key.trajectory_id);
// 如果待分发数据时间戳大于等于起始时间,则正常分发
if (next_data->GetTime() >= common_start_time) {
// Happy case, we are beyond the 'common_start_time' already.
last_dispatched_time_ = next_data->GetTime();
// 执行queue的callbac函数,该callback即是在OrderedMultiQueue::AddQueue函数中加入的
next_queue->callback(next_queue->queue.Pop());
} else if (next_queue->queue.Size() < 2) {
// 待分发数据时间戳小于等于起始时间,只有一个数据的情况,不分发数据
if (!next_queue->finished) {
// We cannot decide whether to drop or dispatch this yet.
CannotMakeProgress(next_queue_key);
return;
}
last_dispatched_time_ = next_data->GetTime();
next_queue->callback(next_queue->queue.Pop());
} else {
// 待分发数据时间戳小于等于起始时间,如果有多个数据,就分发大于起始时间的数据
// We take a peek at the time after next data. If it also is not beyond
// 'common_start_time' we drop 'next_data', otherwise we just found the
// first packet to dispatch from this queue.
std::unique_ptr<Data> next_data_owner = next_queue->queue.Pop();
if (next_queue->queue.Peek<Data>()->GetTime() > common_start_time) {
last_dispatched_time_ = next_data->GetTime();
next_queue->callback(std::move(next_data_owner));
}
}
}
}
至此,从Node的数据经过处理,按照时间分发到相应的GlobalTrajectoryBuilder,并通过调用GlobalTrajectoryBuilder的AddSensorData,将数据传入前端和后端:
GlobalTrajectoryBuilder
(1)激光数据
// 激光数据接收接口
void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) override {
CHECK(local_trajectory_builder_)
<< "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
// 将激光数据送入LocalTrajectoryBuilder进行CSM匹配,并生成submap
std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
matching_result = local_trajectory_builder_->AddRangeData(
sensor_id, timed_point_cloud_data);
if (matching_result == nullptr) {
// The range data has not been fully accumulated yet.
return;
}
kLocalSlamMatchingResults->Increment();
std::unique_ptr<InsertionResult> insertion_result;
if (matching_result->insertion_result != nullptr) {
kLocalSlamInsertionResults->Increment();
// 将前端计算的Node传入后端PoseGraph中计算回环和参与优化
auto node_id = pose_graph_->AddNode(
matching_result->insertion_result->constant_data, trajectory_id_,
matching_result->insertion_result->insertion_submaps);
CHECK_EQ(node_id.trajectory_id, trajectory_id_);
insertion_result = common::make_unique<InsertionResult>(InsertionResult{
node_id, matching_result->insertion_result->constant_data,
std::vector<std::shared_ptr<const Submap>>(
matching_result->insertion_result->insertion_submaps.begin(),
matching_result->insertion_result->insertion_submaps.end())});
}
if (local_slam_result_callback_) {
local_slam_result_callback_(
trajectory_id_, matching_result->time, matching_result->local_pose,
std::move(matching_result->range_data_in_local),
std::move(insertion_result));
}
}
(2)轮速计数据
// 轮速计数据接收接口
void AddSensorData(const std::string& sensor_id,
const sensor::OdometryData& odometry_data) override {
CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
if (local_trajectory_builder_) {
// 前端接收轮速计数据,放在位姿外推器中,校正运动畸变和提供CSM初值
local_trajectory_builder_->AddOdometryData(odometry_data);
}
// 后端接收轮速计数据,用于构建轮速计边参与优化
pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
}
(3)IMU
void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) override {
if (local_trajectory_builder_) {
// 前端接收IMU数据,放在位姿外推器中,校正运动畸变和提供CSM初值
local_trajectory_builder_->AddImuData(imu_data);
}
// 后端接收IMU数据,在3DSLAM里,会构建IMU残差,参与优化,而在2D里,不会参与优化
pose_graph_->AddImuData(trajectory_id_, imu_data);
}
至此,相关数据分发到前后端。