在 MapBuilder::AddTrajectoryBuilder中
1.看2d的局部slam
local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>(
trajectory_options.trajectory_builder_2d_options(),
SelectRangeSensorIds(expected_sensor_ids));
创建局部SLAM的所有功能,包括位姿估计、扫描匹配等
看构造函数:
::LocalTrajectoryBuilder2D(
const proto::LocalTrajectoryBuilderOptions2D& options, //轨迹跟踪器的配置选项
const std::vector<std::string>& expected_range_sensor_ids) //传感器id
: options_(options),
active_submaps_(options.submaps_options()), //当前正在维护的子图
motion_filter_(options_.motion_filter_options()), //运动滤波器,对位姿相关的数据进行降采样
real_time_correlative_scan_matcher_( //实时相关性分析的扫描匹配器
options_.real_time_correlative_scan_matcher_options()),
ceres_scan_matcher_(options_.ceres_scan_matcher_options()), //使用Ceres库将扫描数据放置到地图中的扫描匹配器
range_data_collator_(expected_range_sensor_ids) {}
2. 来管理和收集传感器数据:CollatedTrajectoryBuilder 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
trajectory_options, sensor_collator_.get(), trajectory_id,
expected_sensor_ids,
CreateGlobalTrajectoryBuilder2D(
std::move(local_trajectory_builder), trajectory_id,
static_cast<PoseGraph2D*>(pose_graph_.get()),
local_slam_result_callback, pose_graph_odometry_motion_filter)));
构造函数
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
const proto::TrajectoryBuilderOptions& trajectory_options, //配置
sensor::CollatorInterface* const sensor_collator, const int trajectory_id,
const std::set<SensorId>& expected_sensor_ids,
std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)
: sensor_collator_(sensor_collator), //传感器集
collate_landmarks_(trajectory_options.collate_landmarks()),//landmark集
collate_fixed_frame_(trajectory_options.collate_fixed_frame()),
trajectory_id_(trajectory_id),
wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),//轨迹集
last_logging_time_(std::chrono::steady_clock::now()) {
absl::flat_hash_set<std::string> expected_sensor_id_strings;
for (const auto& sensor_id : expected_sensor_ids) { //遍历传感器
if (sensor_id.type == SensorId::SensorType::LANDMARK &&
!collate_landmarks_) {
continue;//否是不接受Landmark
}
if (sensor_id.type == SensorId::SensorType::FIXED_FRAME_POSE &&
!collate_fixed_frame_) {
continue;
}
expected_sensor_id_strings.insert(sensor_id.id);//插入sensor id
}
sensor_collator_->AddTrajectory(
trajectory_id, expected_sensor_id_strings,
[this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
HandleCollatedSensorData(sensor_id, std::move(data)); //添加传感器数据回调
});
}
传感器数据回调
void CollatedTrajectoryBuilder::HandleCollatedSensorData(
const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
auto it = rate_timers_.find(sensor_id);//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());///
}
data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());//添加传感器数据
void AddToTrajectoryBuilder(
mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
trajectory_builder->AddSensorData(sensor_id_, data_);
}
trajectory_builder->AddSensorData(sensor_id_, data_);
我们看到GlobalTrajectoryBuilder为各种数据类型重载了了一个叫AddSensorData的函数,有点云的,有imu的,里程计的,gps的等
void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) override {
AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
}
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));
}
void AddSensorData(
const std::string& sensor_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) override {
if (collate_fixed_frame_) {
AddData(sensor::MakeDispatchable(sensor_id, fixed_frame_pose_data));
return;
}
wrapped_trajectory_builder_->AddSensorData(sensor_id,
fixed_frame_pose_data);
}
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_->AddSensorData(sensor_id, landmark_data);
}
3,构建全局轨迹集 后端的入口
CreateGlobalTrajectoryBuilder2D( std::move(local_trajectory_builder), trajectory_id,
static_cast<PoseGraph2D*>(pose_graph_.get()), local_slam_result_callback)));
// trajectory_id轨迹索引
//PoseGraph 位姿图,后端的核心对象,其数据类型是一个模板参数
//LocalTrajectoryBuilder 位姿跟踪器,前端的核心对象
//LocalSlamResultCallback前端数据更新后的回调函数
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
const TrajectoryBuilderInterface::LocalSlamResultCallback&
local_slam_result_callback) {
return absl::make_unique<
GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
std::move(local_trajectory_builder), trajectory_id, pose_graph,
local_slam_result_callback);//进行扫描匹配,估计机器人位姿,并将传感器数据插入子图中,更新子图
}