一、前言
前面把数据都接收回来之后，装到传感器数据采样器sensor_samplers_里，正式进入整个工程的前端部分，维护位姿估计器extrapolators_。cartographer的定位部分维护的是Node类建立时初始化的map_builder_bridge_，下面将对这部分的代码进行分析
二、定位部分代码梳理
node_main.cc中，在建立Node类之前建立了一个map_builder，所使用的函数在map_builder_bridge.cc里
auto map_builder =
cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
const proto::MapBuilderOptions& options) {
return absl::make_unique<MapBuilder>(options);
}CreateMapBuilder使用一个std::unique_ptr<MapBuilderInterface>e的指针来记录该对象，于是去map_builder_interface.h里面看了一下，全都是虚函数，也就是说实际建立的对象还是参考子类map_builder.h的
// Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps)
// and a PoseGraph for loop closure.
class MapBuilder : public MapBuilderInterface {
public:
//默认的构造函数和析构函数
explicit MapBuilder(const proto::MapBuilderOptions &options);
~MapBuilder() override {}
//禁用了拷贝构造函数和赋值运算符
MapBuilder(const MapBuilder &) = delete;
MapBuilder &operator=(const MapBuilder &) = delete;
//创建一个新的轨迹跟踪器并返回该跟踪器的索引
int AddTrajectoryBuilder(
const std::set<SensorId> &expected_sensor_ids,
const proto::TrajectoryBuilderOptions &trajectory_options,
LocalSlamResultCallback local_slam_result_callback) override;
//新建轨迹跟踪器的接口
int AddTrajectoryForDeserialization(
const proto::TrajectoryBuilderOptionsWithSensorIds
&options_with_sensor_ids_proto) override;
//关闭trajectory_id对应的轨迹跟踪器，该跟踪器将不再响应新的传感器数据
void FinishTrajectory(int trajectory_id) override;
//用于将submap_id所对应的子图信息填充到response中
//如果出错将错误信息以字符串的形式返回，若成功运行则返回空字符串
std::string SubmapToProto(const SubmapId &submap_id,
proto::SubmapQuery::Response *response) override;
//将当前的系统状态转换成一个proto的流，完成序列化
void SerializeState(bool include_unfinished_submaps,
io::ProtoStreamWriterInterface *writer) override;
bool SerializeStateToFile(bool include_unfinished_submaps,
const std::string &filename) override;
//从proto流中加载SLAM状态
std::map<int, int> LoadState(io::ProtoStreamReaderInterface *reader,
bool load_frozen_state) override;
std::map<int, int> LoadStateFromFile(const std::string &filename,
const bool load_frozen_state) override;
//获取用于实现闭环检测的PoseGraph对象
mapping::PoseGraphInterface *pose_graph() override {
return pose_graph_.get();
}
//获取当前轨迹跟踪器的数量
int num_trajectory_builders() const override {
return trajectory_builders_.size();
}
//获取一个索引为trajectory_id的轨迹跟踪器对象，如果输入的索引不对应一个跟踪器对象则返回空指针nullptr
mapping::TrajectoryBuilderInterface *GetTrajectoryBuilder(
int trajectory_id) const override {
return trajectory_builders_.at(trajectory_id).get();
}
//获取所有的轨迹跟踪器的配置
const std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>
&GetAllTrajectoryBuilderOptions() const override {
return all_trajectory_builder_options_;
}
private:
const proto::MapBuilderOptions options_;
common::ThreadPool thread_pool_;
//位姿图，储存整个后端优化过后的所有位姿
std::unique_ptr<PoseGraph> pose_graph_;
//收集所有传感器信息的接口，用来和前面解析完的传感器信息对接
std::unique_ptr<sensor::CollatorInterface> sensor_collator_;
//轨迹跟踪器
std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>
trajectory_builders_;
//保存了关于轨迹跟踪器的选项
std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>
all_trajectory_builder_options_;
};
std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
const proto::MapBuilderOptions& options);
这里private里面构建了几个很重要的东西，pose_graph_是位姿图，储存整个后端优化过后的所有位姿，sensor_collator_是收集所有传感器信息的接口，用来和前面解析完的传感器信息对接，trajectory_builders_是轨迹跟踪器，all_trajectory_builder_options_内保存了关于轨迹跟踪器的选项
关于map_builder的构造函数：
MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
: options_(options), thread_pool_(options.num_background_threads()) {
//选择是按2d还是3d方式来建图
CHECK(options.use_trajectory_builder_2d() ^
options.use_trajectory_builder_3d());
//如果是2d建图
if (options.use_trajectory_builder_2d()) {
//posegraph构建为一个2d的对象
pose_graph_ = absl::make_unique<PoseGraph2D>(
options_.pose_graph_options(),
absl::make_unique<optimization::OptimizationProblem2D>(
options_.pose_graph_options().optimization_problem_options()),
&thread_pool_);
}
//如果是3d建图
if (options.use_trajectory_builder_3d()) {
//posegraph构建为一个3d的对象
pose_graph_ = absl::make_unique<PoseGraph3D>(
options_.pose_graph_options(),
absl::make_unique<optimization::OptimizationProblem3D>(
options_.pose_graph_options().optimization_problem_options()),
&thread_pool_);
}
//根据配置项collate_by_trajectory来构建修正器
if (options.collate_by_trajectory()) {
sensor_collator_ = absl::make_unique<sensor::TrajectoryCollator>();
} else {
sensor_collator_ = absl::make_unique<sensor::Collator>();
}
}比较常规，主要是加载了一些options，构建了pose_graph_和sensor_collator_。构造完了之后就主要通过调用函数来实现整个定位过程了，大致扫了一下map_builder.cc，最主要也最大的函数就是AddTrajectoryBuilder这个函数了
int MapBuilder::AddTrajectoryBuilder(
const std::set<SensorId>& expected_sensor_ids,
const proto::TrajectoryBuilderOptions& trajectory_options,
LocalSlamResultCallback local_slam_result_callback) {
const int trajectory_id = trajectory_builders_.size();
// Takes poses as input and filters them to get fewer poses.
//构建了一个位姿过滤器
absl::optional<MotionFilter> pose_graph_odometry_motion_filter;
if (trajectory_options.has_pose_graph_odometry_motion_filter()) {
LOG(INFO) << "Using a motion filter for adding odometry to the pose graph.";
//将位姿过滤器插入pose_graph_odometry_motion_filter
pose_graph_odometry_motion_filter.emplace(
MotionFilter(trajectory_options.pose_graph_odometry_motion_filter()));
}
//如果使用3d建图模式
if (options_.use_trajectory_builder_3d()) {
std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder;
if (trajectory_options.has_trajectory_builder_3d_options()) {
//构建local_trajectory_builder位姿跟踪器，是前端位姿估计的核心对象（这里构建时使用3d选项）
//看了一下LocalTrajectoryBuilder3D的构造函数，委实恐怖，塞了一堆默认量进去给类,等遇到的时候再去分析吧
local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder3D>(
trajectory_options.trajectory_builder_3d_options(),
SelectRangeSensorIds(expected_sensor_ids));
}
DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph_.get()));
//看着巨长，其实就是trajectory_builders_塞进了一个东西
//构建的过程比较复杂，调用了CollatedTrajectoryBuilder的构造函数
//CollatedTrajectoryBuilder类最后输入的量类型是std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)
//CreateGlobalTrajectoryBuilder3D来自global_trajectory_builder.h
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
trajectory_options, sensor_collator_.get(), trajectory_id,
expected_sensor_ids,
CreateGlobalTrajectoryBuilder3D(
std::move(local_trajectory_builder), trajectory_id,
static_cast<PoseGraph3D*>(pose_graph_.get()),
local_slam_result_callback, pose_graph_odometry_motion_filter)));
} else {
//2d和3d基本类似，名字换了一下
std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
if (trajectory_options.has_trajectory_builder_2d_options()) {
local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>(
trajectory_options.trajectory_builder_2d_options(),
SelectRangeSensorIds(expected_sensor_ids));
}
//将pose_graph_对象强制转换为PoseGraph2D，并检查数据类型是否正确
DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
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)));
}
//决定是否为pose_graph_对象添加一个OverlappingSubmapsTrimmer2D类型的修饰器， 用于根据子图之间重叠的部分修饰地图
//如果配置项要求只进行定位，那么可以在pose_graph_中添加一个PureLocalizationTrimmer类型修饰器，来完成这一功能
MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options,
pose_graph_.get());
//如果有初始位姿的话，写到initial_trajectory_pose和pose_graph_里
if (trajectory_options.has_initial_trajectory_pose()) {
const auto& initial_trajectory_pose =
trajectory_options.initial_trajectory_pose();
pose_graph_->SetInitialTrajectoryPose(
trajectory_id, initial_trajectory_pose.to_trajectory_id(),
transform::ToRigid3(initial_trajectory_pose.relative_pose()),
common::FromUniversal(initial_trajectory_pose.timestamp()));
}
//将轨迹跟踪器的配置信息和传感器配置信息保存到容器all_trajectory_builder_options_中
proto::TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids_proto;
for (const auto& sensor_id : expected_sensor_ids) {
*options_with_sensor_ids_proto.add_sensor_id() = ToProto(sensor_id);
}
*options_with_sensor_ids_proto.mutable_trajectory_builder_options() =
trajectory_options;
all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto);
CHECK_EQ(trajectory_builders_.size(), all_trajectory_builder_options_.size());
return trajectory_id;
}巨长，巨臭，巨调来调去，笔者是半夜一点看的，看完感觉头皮发麻睡不着觉
写的过程中大量参考了无处不在的小土关于map_builder的解读，确实让我学到很多东西，我对AddTrajectoryBuilder的解读比他要稍微多一些，欢迎大家的交流
这里引用他对整个map_builder接口的总结：map_builder对象的接口实现，实际完成局部建图任务的是一个LocalTrajectoryBuilder2D类型的对象， 而与轨迹规划器相关的类型还有CollatedTrajectoryBuilder、trajectory_builders_、TrajectoryBuilderInterface它们应该说是一种封装
由于我是研究定位的，关于proto和serial的部分不太想多做研究，实在是被上面这些东西恶心到了
这个里面除了local_trajectory_builder的建立，还有一个trajectory_builders_的建立也比较关键，粗略的看了一下应该也是用于通信的接口，下面把这两个大东西分析一下：
namespace cartographer {
namespace mapping {
// Wires up the local SLAM stack (i.e. pose extrapolator, scan matching, etc.)
// without loop closure.
// TODO(gaschler): Add test for this class similar to the 3D test.
class LocalTrajectoryBuilder2D {
public:
//用来记录插入子图的信息
struct InsertionResult {
//插入的节点数据
std::shared_ptr<const TrajectoryNode::Data> constant_data;
//被插入的子图
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
};
//记录子图匹配的结果
struct MatchingResult {
common::Time time;
//在局部地图坐标系下的位姿
transform::Rigid3d local_pose;
//局部地图的扫描数据
sensor::RangeData range_data_in_local;
// 'nullptr' if dropped by the motion filter.
std::unique_ptr<const InsertionResult> insertion_result;
};
//构造函数
explicit LocalTrajectoryBuilder2D(
const proto::LocalTrajectoryBuilderOptions2D& options,
const std::vector<std::string>& expected_range_sensor_ids);
~LocalTrajectoryBuilder2D();
//屏蔽了拷贝构造函数和拷贝赋值运算符
LocalTrajectoryBuilder2D(const LocalTrajectoryBuilder2D&) = delete;
LocalTrajectoryBuilder2D& operator=(const LocalTrajectoryBuilder2D&) = delete;
// Returns 'MatchingResult' when range data accumulation completed,
// otherwise 'nullptr'. Range data must be approximately horizontal
// for 2D SLAM. `TimedPointCloudData::time` is when the last point in
// `range_data` was acquired, `TimedPointCloudData::ranges` contains the
// relative time of point with respect to `TimedPointCloudData::time`.
//接受激光传感器、imu和里程计的扫描数据
std::unique_ptr<MatchingResult> AddRangeData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& range_data);
void AddImuData(const sensor::ImuData& imu_data);
void AddOdometryData(const sensor::OdometryData& odometry_data);
//感觉像是记录了什么东西，但是也没有实际用到
static void RegisterMetrics(metrics::FamilyFactory* family_factory);
private:
//对单帧扫描进行匹配运算
std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
common::Time time, const sensor::RangeData& gravity_aligned_range_data,
const transform::Rigid3d& gravity_alignment,
const absl::optional<common::Duration>& sensor_duration);
//单帧扫描坐标变换
sensor::RangeData TransformToGravityAlignedFrameAndFilter(
const transform::Rigid3f& transform_to_gravity_aligned_frame,
const sensor::RangeData& range_data) const;
//单帧扫描插入子图
std::unique_ptr<InsertionResult> InsertIntoSubmap(
common::Time time, const sensor::RangeData& range_data_in_local,
const sensor::PointCloud& filtered_gravity_aligned_point_cloud,
const transform::Rigid3d& pose_estimate,
const Eigen::Quaterniond& gravity_alignment);
// Scan matches 'filtered_gravity_aligned_point_cloud' and returns the
// observed pose, or nullptr on failure.
std::unique_ptr<transform::Rigid2d> ScanMatch(
common::Time time, const transform::Rigid2d& pose_prediction,
const sensor::PointCloud& filtered_gravity_aligned_point_cloud);
// Lazily constructs a PoseExtrapolator.
void InitializeExtrapolator(common::Time time);
//轨迹跟踪器的配置选项
const proto::LocalTrajectoryBuilderOptions2D options_;
//当前正在维护的子图
ActiveSubmaps2D active_submaps_;
//运动滤波器
MotionFilter motion_filter_;
//实时相关性分析扫描匹配器
scan_matching::RealTimeCorrelativeScanMatcher2D
real_time_correlative_scan_matcher_;
//ceres匹配，将扫描数据加载到地图里
scan_matching::CeresScanMatcher2D ceres_scan_matcher_;
//位姿估计器
std::unique_ptr<PoseExtrapolator> extrapolator_;
//累计数据的数量
int num_accumulated_ = 0;
//累计的扫描数量
sensor::RangeData accumulated_range_data_;
absl::optional<std::chrono::steady_clock::time_point> last_wall_time_;
absl::optional<double> last_thread_cpu_time_seconds_;
absl::optional<common::Time> last_sensor_time_;
//数据收集器
RangeDataCollator range_data_collator_;
};
} // namespace mapping
} // namespace cartographer可以看到local_trajectory_builder建立过程中用到的所有接口，包括接受传感器信息的extrapolator_位姿估计器等，里面几个比较重要的函数完成了帧间匹配与插入子图的过程
至于trajectory_builders_的建立：
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子类最后输入的量类型是std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)，这部分是由CreateGlobalTrajectoryBuilder2D构建的，来自global_trajectory_builder.h，最后都塞进去trajectory_builders_里面了，他本身来自trajectory_builder_interface.h，到文件里看了一下，构造函数啥的都是空的，主要的几个函数也都是虚函数，这里继承的子类来自CollatedTrajectoryBuilder，不过应该都是用来把传感器数据塞到指针里面去的，全局搜了一下trajectory_builders_，总感觉这个东西就是用来记录运行过程中每一次优化后的轨迹跟踪器的信息的，后面再遇到的话再做分析吧
三、后话
前前后后看了应该有个三天，定位部分的接口写的还是挺复杂的，写着写着感觉还是把真正做运算的部分留到后面来分析吧，这部分分析了一堆接口，梳理了一下运行的逻辑，感觉没多大用处哈哈，熟悉了一下谷歌的编程习惯
扫一扫
597
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
