MapBuilder的成员变量sensor::Collator sensor_collator_;
再次阅读MapBuilder::AddTrajectoryBuilder方法。首先构造了mapping::GlobalTrajectoryBuilder实例，接着作为参数构造了CollatedTrajectoryBuilder实例。
trajectory_builders_.push_back(
common::make_unique<CollatedTrajectoryBuilder>(
&sensor_collator_, trajectory_id, expected_sensor_ids,
common::make_unique<mapping::GlobalTrajectoryBuilder<mapping_2d::LocalTrajectoryBuilder,mapping_2d::proto::LocalTrajectoryBuilderOptions,mapping_2d::PoseGraph>>
(trajectory_options.trajectory_builder_2d_options(),trajectory_id, pose_graph_2d_.get(),local_slam_result_callback)
)
);
这里sensor_collator_作为参数传入，参与CollatedTrajectoryBuilder构造。查看构造函数：
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(sensor::Collator* const sensor_collator, const int trajectory_id, const std::unordered_set<std::string>& expected_sensor_ids, std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)
: sensor_collator_(sensor_collator)
, trajectory_id_(trajectory_id)
, wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder))
, last_logging_time_(std::chrono::steady_clock::now())
{
sensor_collator_->AddTrajectory(trajectory_id, expected_sensor_ids,
[this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data)
{
HandleCollatedSensorData(sensor_id, std::move(data));
}
);
}
这里是回调函数，std::unique_ptr是表示参数为智能指针。
[this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data)
{
HandleCollatedSensorData(sensor_id, std::move(data));
}
（1）查看sensor::Collator的AddTrajectory方法：
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};
queue_.AddQueue(queue_key, [callback, sensor_id](std::unique_ptr<Data> data)
{
callback(sensor_id, std::move(data));
});
queue_keys_[trajectory_id].push_back(queue_key);
}
}
for (const auto& sensor_id : expected_sensor_ids)用到了C++11的auto新特性。
（2）查看HandleCollatedSensorData方法。调用了data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());这里wrapped_trajectory_builder_是在CollatedTrajectoryBuilder构造函数中赋值的。为GlobalTrajectoryBuilder对象。因而查看sensor::Data的AddToTrajectoryBuilder() 方法。
virtual void AddToTrajectoryBuilder(mapping::TrajectoryBuilderInterface *trajectory_builder) = 0;是sensor::Data类的一个虚方法。内部执行了trajectory_builder->AddSensorData(sensor_id_, data_);
最后调用的是GlobalTrajectoryBuilder对象的AddSensorData(xx)方法。
void CollatedTrajectoryBuilder::HandleCollatedSensorData( const std::string& sensor_id, std::unique_ptr<sensor::Data> data)
{
auto it = rate_timers_.find(sensor_id);
if (it == rate_timers_.end())
{
it = rate_timers_ .emplace(
std::piecewise_construct, std::forward_as_tuple(sensor_id),
std::forward_as_tuple(common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds))) .first;
}
it->second.Pulse(data->GetTime()); if (std::chrono::steady_clock::now() - last_logging_time_ >
common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds))
{
for (const auto& pair : rate_timers_)
{
LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();
}
last_logging_time_ = std::chrono::steady_clock::now();
} data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
} }
CollatedTrajectoryBuilder::HandleCollatedSensorData
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; } void AddToTrajectoryBuilder( mapping::TrajectoryBuilderInterface *const trajectory_builder) override
{
trajectory_builder->AddSensorData(sensor_id_, data_);
} private:
const DataType data_;
};
再以IMU数据为例，GlobalTrajectoryBuilder类的AddSensorData(xx)：
void AddSensorData(const std::string& sensor_id, const sensor::ImuData& imu_data) override
{
local_trajectory_builder_.AddImuData(imu_data);
pose_graph_->AddImuData(trajectory_id_, imu_data);
}
再看一下激光点云的数据
void AddSensorData( const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) override
{
std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult> matching_result =
local_trajectory_builder_.AddRangeData( timed_point_cloud_data.time,
sensor::TimedRangeData {timed_point_cloud_data.origin,
timed_point_cloud_data.ranges, {}}
);
if (matching_result == nullptr)
{
// The range data has not been fully accumulated yet.
return;
}
std::unique_ptr<mapping::NodeId> node_id;
if (matching_result->insertion_result != nullptr)
{
node_id = ::cartographer::common::make_unique<mapping::NodeId>(
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_);
}
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(node_id));
}
}
这里有两个重要的步骤一个是local_trajectory_builder_.AddRangeData（xxx），一个是 pose_graph_->AddNode(xxx)方法。同时std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult> matching_result作为AddNode方法的参数。
mapping::NodeId PoseGraph::AddNode(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose); common::MutexLocker locker(&mutex_);
AddTrajectoryIfNeeded(trajectory_id);
const mapping::NodeId node_id = trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
++num_trajectory_nodes_; // Test if the 'insertion_submap.back()' is one we never saw before.
if (submap_data_.SizeOfTrajectoryOrZero(trajectory_id) == ||
std::prev(submap_data_.EndOfTrajectory(trajectory_id))->data.submap !=
insertion_submaps.back()) {
// We grow 'submap_data_' as needed. This code assumes that the first
// time we see a new submap is as 'insertion_submaps.back()'.
const mapping::SubmapId submap_id =
submap_data_.Append(trajectory_id, SubmapData());
submap_data_.at(submap_id).submap = insertion_submaps.back();
} // We have to check this here, because it might have changed by the time we
// execute the lambda.
const bool newly_finished_submap = insertion_submaps.front()->finished();
AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForNode(node_id, insertion_submaps,
newly_finished_submap);
});
return node_id;
}
PoseGraph::AddNode
PoseGraph::AddNode方法很重要，分析节点和子图的关系。
此处强调一下GlobalTrajectoryBuilder的两个关键对象local_trajectory_builder_和pose_graph_。
PoseGraph* const pose_graph_;
LocalTrajectoryBuilder local_trajectory_builder_;
接下来按照准备安装ROS消息发布和处理的流程进行分析，即数据流。
参考资料：