cartographer坐标系_大大西瓜/cartographer代码解读

cartographer代码解读

0. 说明

只列主要示意性代码，其他代码省略

以处理LaserScan, 2D为例

采用Collator(参数map_builder.collate_by_trajectory 为false)

使用 --> 表示后续函数调用，或经过某个变量调用的后续函数

使用

使用 >>> 表示topic的回调函数中的后续函数调用

1. 总览

成员变量

MapBuilder* map_builder_;

map sensor_bridges_;

构造函数 MapBuilderBridge(..., map_builder, ...)

设置成员变量 map_builder_ = map_builder;

成员函数 AddTrajectory

初始化 MapBuilder 内容

map_builder_ --> 调用 MapBuilder::AddTrajectoryBuilder，获得trajectory_id

添加 SensorBridge(..., MapBuilder::GetTrajectoryBuilder(trajectory_id)

成员变量

TrajectoryBuilderInterface* (

构造函数 SensorBridge(..., trajectory_builder)

设置成员变量 trajectory_builder_ = trajectory_builder;

成员变量

vector trajectory_builders_;

PoseGraph* (

CollatorInterface* (

ThreadPool thread_pool_;

构造函数 MapBuilder(...)

thread_pool_(options.num_background_threads);

根据配置设置成员变量pose_graph_ 和 sensor_collator_，以开始所假设的配置

pose_graph_ = new PoseGraph2D(..., new optimization::OptimizationProblem2D(...), ...);

sensor_collator_ = new Collator();

成员函数 AddTrajectoryBuilder

int trajectory_id = trajectory_builders_.size();

local_trajectory_builder = new LocalTrajectoryBuilder2D(...);

g_trajectory_builder = new GlobalTrajectoryBuilder(local_trajectory_builder, trajectory_id, pose_graph_, res_callback);

trajectory_builders_.push_back( new CollatedTrajectoryBuilder(..., sensor_collator_, trajectory_id, ..., g_trajectory_builder) );

成员函数 GetTrajectoryBuilder(trajectory_id)

return trajectory_builders_[trajectory_id];

成员函数 FinishTrajectory(trajectory_id)

sensor_collator_ --> 调用Collator::FinishTrajectory(trajectory_id);

pose_graph_ --> 调用 PoseGraph2D::FinishTrajectory(trajectory_id);

成员变量

int trajectory_id_;

CollatorInterface* (

TrajectoryBuilderInterface* (

构造函数 CollatedTrajectoryBuilder(..., sensor_collator, trajectory_id, ..., wrapped_trajectory_builder)

trajectory_id_ = trajectory_id;

sensor_collator_ = sensor_collator;

wrapped_trajectory_builder_ = wrapped_trajectory_builder;

sensor_collator_ --> 调用Collator::AddTrajectory(trajectory_id, ..., HandleCollatedSensorData);

成员函数 AddSensorData(sensor_id, raw_data)

sensor_collator_ --> 调用Collator::AddSensorData(MakeDispatchable(sensor_id, raw_data))

成员变量

string sensor_id_ (基类Data的成员);

DataType data_;

构造函数 Dispatchable(string sensor_id, DataType data)

设置相应成员变量;

成员函数 AddToTrajectoryBuilder(TrajectoryBuilderInterface* trajectory_builder)

trajectory_builder --> 调用 GlobalTrajectoryBuilder::AddSensorData(sensor_id_, data_);

成员变量

OrderedMultiQueue queue_;

成员函数 AddTrajectory(trajectory_id, topicIDList, callback)

key = QueueKey{trajectory_id, topicID};

queue_.AddQueue(key, callback);

成员函数 AddSensorData(topicID, data)

key = QueueKey{trajectory_id, topicID};

queue_.Add(key, data) --> OrderedMultiQueue::Dispatch --> CollatedTrajectoryBuilder::HandleCollatedSensorData

成员函数 FinishTrajectory(trajectory_id)

queue_.MarkQueueAsFinished(queue_key);

成员变量

int trajectory_id_;

PoseGraph* pose_graph_;

LocalTrajectoryBuilder* local_trajectory_builder_;

LocalSlamResultCallback local_slam_result_callback_;

构造函数 GlobalTrajectoryBuilder(local_trajectory_builder, trajectory_id, pose_graph_, result_callback)

设置相应成员变量;

成员函数 AddSensorData(sensor_id, timed_point_cloud_data)

local_trajectory_builder_ --> 调用 matching_result = LocalTrajectoryBuilder2D::AddRangeData(sensor_id, timed_point_cloud_data);

pose_graph_ --> 调用 node_id = PoseGraph2D::AddNode(matching_result..., trajectory_id_, ...);

local_slam_result_callback_(...);

成员函数 AddSensorData(sensor_id, imu_data)

local_trajectory_builder_ --> 调用 LocalTrajectoryBuilder2D::AddImuData(imu_data);

pose_graph_ --> 调用 PoseGraph2D::AddImuData(trajectory_id_, imu_data);

成员函数 AddSensorData(sensor_id, odometry_data)

local_trajectory_builder_ --> 调用 LocalTrajectoryBuilder2D::AddOdometryData(odometry_data);

pose_graph_ --> 调用 PoseGraph2D::AddOdometryData(trajectory_id_, odometry_data);

成员变量

PoseExtrapolator* extrapolator_;

RangeDataCollator range_data_collator_;

proto::LocalTrajectoryBuilderOptions2D options_; // TRAJECTORY_BUILDER_2D

ActiveSubmaps2D active_submaps_;

成员函数 AddImuData(imu_data)

LocalTrajectoryBuilder2D::InitializeExtrapolator(imu_data.time);

extrapolator_->AddImuData(imu_data);

成员函数 AddOdometryData(odometry_data)

extrapolator_->AddOdometryData(odometry_data);

成员函数 AddRangeData(sensor_id, unsynchronized_data)

利用range_data_collator_ 调用 RangeDataCollator::AddRangeData将不同传感器的unsynchronized_data按时间戳先后顺序重新编排，获得synchronized_data;

利用extrapolator_进行运动补偿，将不同时刻的synchronized_data变换到统一时刻下;

按距离对数据切割和初始化;

丢弃掉距离小于options_.min_range的点云数据;

将距离小于 options_.max_range数据累积到accumulated_range_data_.returns里;

将距离大于 options_.max_range数据累积到accumulated_range_data_.misses里，

累积前先把数据的距离变到options_.missing_data_ray_length;

如果累积消息数达到options_.num_accumulated_range_data，则开始处理数据;

调用 TransformToGravityAlignedFrameAndFilter 预处理数据;

调用 AddAccumulatedRangeData 开始正式SLAM处理;

按照重力方向对点云数据进行变换，使得z轴与重力方向重合;

保留z坐标在options_.min_z和options_.max_z间的数据;

用VoxelFilter对点云数据进行滤波，即按照options_.voxel_filter_size参数划分格子，每个格子只保留一个点

利用 extrapolator_生成预测位姿;

按照 options_.adaptive_voxel_filter里面的参数，用AdaptiveVoxelFilter对点云数据进行滤波;

用预测位姿，将滤波后的点云数据和地图做 ScanMatch(预测位姿, 点云数据)，获得位姿pose_estimate;

extrapolator_->AddPose(time, pose_estimate);

利用pose_estimate对点云数据变换到地图坐标系下，InsertIntoSubmap(地图坐标系下的点云数据, ...);

成员函数 ScanMatch(预测位姿，点云数据)

取当前活跃子地图列表中的最新一个作为 matching_submap;

initial_ceres_pose = pose_prediction;

如果 options_.use_online_correlative_scan_matching 为真，按照 options_.real_time_correlative_scan_matcher 里面的参数，调用 RealTimeCorrelativeScanMatcher2D ::Match(预测位姿, 点云数据, matching_submap)，修改initial_ceres_pose;

按照 options_.ceres_scan_matcher 里面的参数，调用 CeresScanMatcher2D::Match(预测位姿, initial_ceres_pose, 点云数据, matching_submap)，获得最终位姿;

成员函数 InsertIntoSubmap(地图坐标系下的点云数据)

active_submaps_ --> 调用 ActiveSubmaps2D::InsertRangeData(地图坐标系下的点云数据);

成员函数 Filter及后续调用函数FilterByMaxRange和AdaptivelyVoxelFiltered

剔除距离大于 adaptive_voxel_filter.max_range的点;

如果点云数小于 adaptive_voxel_filter.min_num_points，返回当前点云;

用 adaptive_voxel_filter.max_length 为参数，用VoxelFilter对点云数据进行滤波，如果点云数大于 adaptive_voxel_filter.min_num_points(点数足够密)，则返回当前点云;

从 adaptive_voxel_filter.max_length 开始，做二分查找VoxelFilter参数，使得滤波后的点云数点云数尽量接近 adaptive_voxel_filter.min_num_points;

2. cartographer_ros

2.1 主函数 Run (node_main.cc) 返回总览

Node node(..., new MapBuilder(...), ...)

node.StartTrajectoryWithDefaultTopics(...)

::ros::spin()

node.FinishAllTrajectories();

node.RunFinalOptimization();

2.2 主节点类 Node (node.h, node.cc)

成员变量

MapBuilderBridge* map_builder_bridge_;

Node(..., map_builder, ...)

设置成员变量 map_builder_bridge_(..., map_builder, ...)

StartTrajectoryWithDefaultTopics(...) --> AddTrajectory(...) 返回总览

map_builder_bridge_.AddTrajectory(topicIDList, ...);

AddExtrapolator(...);

LaunchSubscribers(...);

LaunchSubscribers(...) 返回总览

scanTopic >>> HandleLaserScanMessage

if (trajectory_builder_2d.use_imu_data)

imuTopic >>> HandleImuMessage

if (options.use_odometry)

odomTopic >>> HandleOdometryMessage

HandleLaserScanMessage(trajectory_id, ...) 返回总览

map_builder_bridge_.sensor_bridge_[trajectory_id]->HandleLaserScanMessage(...);

HandleImuMessage(trajectory_id, ...) 返回总览

extrapolators.AddImuData(...);

map_builder_bridge_.sensor_bridge_[trajectory_id]->HandleImuMessage(...);

HandleOdometryMessage(trajectory_id, ...) 返回总览

extrapolators.AddOdometryData(...);

map_builder_bridge_.sensor_bridge_[trajectory_id]->HandleOdometryMessage(...);

FinishAllTrajectories() 返回总览

...

map_builder_bridge_.FinishTrajectory(...);

RunFinalOptimization() 返回总览

...-->map_builder_bridge_.FinishTrajectory(...);

map_builder_bridge_.RunFinalOptimization();

2.3 MapBuilderBridge 类 (map_builder_bridge.h, map_builder_bridge.cc)

成员变量

MapBuilder* map_builder_;

map sensor_bridges_;

MapBuilderBridge(..., map_builder, ...)

设置成员变量 map_builder_ = map_builder;

AddTrajectory(topicIDList, ...) 返回总览

trajectory_id = map_builder_->AddTrajectoryBuilder(topicIDList, ...);

sensor_bridges_[trajectory_id] = new SensorBridge(..., map_builder_->GetTrajectoryBuilder(trajectory_id));

FinishTrajectory(trajectory_id) 返回总览

map_builder_->FinishTrajectory(trajectory_id);

RunFinalOptimization() 返回总览

map_builder_->pose_graph()->RunFinalOptimization();

2.4 SensorBridge 类 (sensor_bridge.h, sensor_bridge.cc)

成员变量

TrajectoryBuilderInterface* (

SensorBridge(..., trajectory_builder)

设置成员变量 trajectory_builder_ = trajectory_builder;

HandleLaserScanMessage(...) --> HandleLaserScan(...) 返回总览

for (num_subdivisions_per_laser_scan){

set subdivision data;

HandleRangefinder(topicID, ..., subdivision) --> trajectory_builder_->AddSensorData(topicID, TimedPointCloudData);

}

HandleImuMessage(...) 返回总览

trajectory_builder_->AddSensorData(topicID, ImuData);

HandleOdometryMessage(...) 返回总览

trajectory_builder_->AddSensorData(topicID, OdometryData);

3. cartographer

3.1 MapBuilder 类 (map_builder.h, map_builder.cc)

成员变量

vector trajectory_builders_

PoseGraph* (

CollatorInterface* (

MapBuilder(...)

if (map_builder.use_trajectory_builder_2d){

pose_graph_ = new PoseGraph2D(..., new optimization::OptimizationProblem2D(...), ...);

}

if (map_builder.use_trajectory_builder_3d){

pose_graph_ = new PoseGraph3D(..., new optimization::OptimizationProblem3D(...), ...);

}

if (map_builder.collate_by_trajectory)

sensor_collator_ = new TrajectoryCollator();

else

sensor_collator_ = new Collator();

AddTrajectoryBuilder(topicIDList, ...) 返回总览

int trajectory_id = trajectory_builders_.size();

if (map_builder.use_trajectory_builder_3d){

if (!map_builder.has_trajectory_builder_3d_options)

local_trajectory_builder = new LocalTrajectoryBuilder3D(topicIDList, ...)

trajectory_builders_.push_back( CollatedTrajectoryBuilder(..., sensor_collator, trajectory_id, topicIDList,

new GlobalTrajectoryBuilder(local_trajectory_builder, trajectory_id, pose_graph, result_callback) ) );

}else{

if (!map_builder.has_trajectory_builder_2d_options)

local_trajectory_builder = new LocalTrajectoryBuilder2D(topicIDList, ...)

trajectory_builders_.push_back( CollatedTrajectoryBuilder(..., sensor_collator, trajectory_id, topicIDList,

new GlobalTrajectoryBuilder(local_trajectory_builder, trajectory_id, pose_graph, result_callback) ) );

}

MaybeAddPureLocalizationTrimmer(trajectory_id, ..., pose_graph_);

if (trajectory_builder.has_initial_trajectory_pose)

pose_graph_->SetInitialTrajectoryPose(trajectory_id, trajectory_builder.initial_trajectory_pose, ...);

GetTrajectoryBuilder(trajectory_id) 返回总览

return trajectory_builders_[trajectory_id]; // CollatedTrajectoryBuilder

FinishTrajectory(...) 返回总览

sensor_collator_->FinishTrajectory(trajectory_id);

pose_graph_->FinishTrajectory(trajectory_id);

3.2 CollatedTrajectoryBuilder 类 (collated_trajectory_builder.h, collated_trajectory_builder.cc)

CollatedTrajectoryBuilder(..., sensor_collator, trajectory_id, topicIDList, wrapped_trajectory_builder)

sensor_collator->AddTrajectory(trajectory_id, topicIDList, HandleCollatedSensorData);

HandleCollatedSensorData(topicID, data) 返回总览

data->AddToTrajectoryBuilder(wrapped_trajectory_builder); // GlobalTrajectoryBuilder

AddSensorData(topicID, TimedPointCloudData/ImuData/OdometryData) --> AddData --> sensor_collator->AddSensorData

3.3 Collator 类 (collator.h, collator.cc) // sensor_collator

AddTrajectory(trajectory_id, topicIDList, callback) 返回总览

for (topicID: topicIDList){

queue_key = QueueKey{trajectory_id, topicID};

queue_.AddQueue(queue_key, callback);

}

AddSensorData(topicID, data) 返回总览

queue_key = QueueKey{trajectory_id, topicID};

queue_.Add(queue_key, data) --> Dispatch --> callback

3.4 GlobalTrajectoryBuilder 类 (map_builder.h, map_builder.cc)

3.5 LocalTrajectoryBuilder2D 类 (map_builder.h, map_builder.cc)

3.6 PoseGraph2D 类 (map_builder.h, map_builder.cc)

RunFinalOptimization(...) 返回总览