global_trajectory_bulider.cc
对Cartographer的阅读我们按照从数据的(传感器)的输入开始,直到结果的输出为止。因此代码从global_trajectory_bulider.cc开始阅读,里面包含了激光数据、IMU、里程计数据等的输入。
文件在Cartographer/mapping/internal/global_trajectory_builder.cc
1.加入激光数据
初始化函数:
GlobalTrajectoryBuilder(
std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder,
const int trajectory_id, PoseGraph* const pose_graph,
const LocalSlamResultCallback& local_slam_result_callback)
: trajectory_id_(trajectory_id),
pose_graph_(pose_graph),
local_trajectory_builder_(std::move(local_trajectory_builder)),
local_slam_result_callback_(local_slam_result_callback) {}
加入激光数据:在这里函数的实现非常简单,主要是因为此处多为调用函数,主要分为了前端匹配,调用了 Cartographer/mapping/internal/2d/scan_matching/local_trajectory_builder_2d.cc->AddRangeData()函数来实现;后端调用pose_graph_->AddNode
local_trajectory_builder_2d.cc:因为研究方向为2d,所以只对2d的代码进行阅读.local_trajectory_builder_相当于局部匹配器,trajectory实际是机器人走过的一段轨迹,cartographer允许分段建立多段轨迹.
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.";
//进行局部的子图匹配,cartographer采用CSM(相关性匹配)+基于优化的前端匹配方式,接下来我们将学到
//调用了LocalTrajectoryBuilder->AddRangeData,在接下来将学到,具体的实现函数
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();
//pose-graph增加一个节点,本函数会进行回环检测和后端优化.
//此处节点就是指的关键帧,一个节点就是一个关键帧数据
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 = absl::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.加入IMU数据
跟激光数据的加入一样,均调用了local_trajectory_builder及pose_graph_里的函数,因此,接下来就要去查看这两处的代码.
//加入IMU数据
void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) override
{
if (local_trajectory_builder_)
{
local_trajectory_builder_->AddImuData(imu_data);
}
pose_graph_->AddImuData(trajectory_id_, imu_data);
}
3.加入里程计数据
同上
//加入里程计数据.
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_)
{
local_trajectory_builder_->AddOdometryData(odometry_data);
}
pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
}
4.加入Landmark
//加入landmark数据.
void AddSensorData(const std::string& sensor_id,
const sensor::LandmarkData& landmark_data) override
{
pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);
}
5.加入局部slam数据
//加入局部slam数据.
void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>
local_slam_result_data) override
{
CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with "
"local_trajectory_builder_ present.";
local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_);
}
总结
由global_trajectory_builder.cc代码阅读之后发现,无论是激光数据还是里程计和IMU数据,它们的前端匹配均调用local_trajectory_builder_里的函数,后端优化均调用pose_graph_里的函数,因此接下来将对这两部分代码进行阅读.