描述
Run()函数中的建图功能的代码解析
解析
node_main.cc中的Run()函数中有这样一段核心代码
NodeOptions node_options;
TrajectoryOptions trajectory_options;
std::tie(node_options, trajectory_options) =
LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
auto map_builder =
cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
Node node(node_options, std::move(map_builder), &tf_buffer,
FLAGS_collect_metrics);
if (!FLAGS_load_state_filename.empty()) {
node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
}
if (FLAGS_start_trajectory_with_default_topics) {
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
这段代码中的以下语句是什么含义呢
if (FLAGS_start_trajectory_with_default_topics) {
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
开始建图
FLAGS_start_trajectory_with_default_topics已经被定义为了true，那么node.StartTrajectoryWithDefaultTopics的源码声明在cartographer_ros/node.h中，它的实现在cartographer_ros/node.cc
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
absl::MutexLock lock(&mutex_);
CHECK(ValidateTrajectoryOptions(options));
AddTrajectory(options);
}
首先对变量加锁，并检查上一步骤加载的参数是否正常，这是前两句代码的含义。
核心的语句是
AddTrajectory(options);
AddTrajectory的源码也在cartographer_ros/node.cc中
int Node::AddTrajectory(const TrajectoryOptions& options) {
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
expected_sensor_ids = ComputeExpectedSensorIds(options);
const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
AddExtrapolator(trajectory_id, options);
AddSensorSamplers(trajectory_id, options);
LaunchSubscribers(options, trajectory_id);
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kTopicMismatchCheckDelaySec),
&Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
for (const auto& sensor_id : expected_sensor_ids) {
subscribed_topics_.insert(sensor_id.id);
}
return trajectory_id;
}
核心语句包括以下几个步骤，展开来分析，今天这篇只分析1、2
- ComputeExpectedSensorIds 确定传感器topic
- AddTrajectory 增加轨迹
- AddExtrapolator
- AddSensorSamplers
- LaunchSubscribers
- createWallTimer
1. 确定传感器topic
ComputeExpectedSensorIds的源码也在cartographer_ros/node.cc中
std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
Node::ComputeExpectedSensorIds(const TrajectoryOptions& options) const {
using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
using SensorType = SensorId::SensorType;
std::set<SensorId> expected_topics;
// Subscribe to all laser scan, multi echo laser scan, and point cloud topics.
for (const std::string& topic :
ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
expected_topics.insert(SensorId{SensorType::RANGE, topic});
}
for (const std::string& topic : ComputeRepeatedTopicNames(
kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
expected_topics.insert(SensorId{SensorType::RANGE, topic});
}
for (const std::string& topic :
ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
expected_topics.insert(SensorId{SensorType::RANGE, topic});
}
// For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
// required.
if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
(node_options_.map_builder_options.use_trajectory_builder_2d() &&
options.trajectory_builder_options.trajectory_builder_2d_options()
.use_imu_data())) {
expected_topics.insert(SensorId{SensorType::IMU, kImuTopic});
}
// Odometry is optional.
if (options.use_odometry) {
expected_topics.insert(SensorId{SensorType::ODOMETRY, kOdometryTopic});
}
// NavSatFix is optional.
if (options.use_nav_sat) {
expected_topics.insert(
SensorId{SensorType::FIXED_FRAME_POSE, kNavSatFixTopic});
}
// Landmark is optional.
if (options.use_landmarks) {
expected_topics.insert(SensorId{SensorType::LANDMARK, kLandmarkTopic});
}
return expected_topics;
}
这段代码实现的功能就是，创建一个set类expected_topics，向其中推送各种传感器的SensorId
SensorId是一个struct，它的代码在cartographer_ros/trajectory_builder_interface.h中
struct SensorId {
enum class SensorType {
RANGE = 0,
IMU,
ODOMETRY,
FIXED_FRAME_POSE,
LANDMARK,
LOCAL_SLAM_RESULT
};
SensorType type;
std::string id;
bool operator==(const SensorId& other) const {
return std::forward_as_tuple(type, id) ==
std::forward_as_tuple(other.type, other.id);
}
bool operator<(const SensorId& other) const {
return std::forward_as_tuple(type, id) <
std::forward_as_tuple(other.type, other.id);
}
};
ComputeRepeatedTopicNames函数实现的就是将各种传感器的topic的名字（即使重复），也以“ _ ” 连接成这样的形式 “ 话题名_数字”，并将这种形式全部推送到一个vector中。
std::vector<std::string> ComputeRepeatedTopicNames(const std::string& topic,
const int num_topics) {
CHECK_GE(num_topics, 0);
if (num_topics == 1) {
return {topic};
}
std::vector<std::string> topics;
topics.reserve(num_topics);
for (int i = 0; i < num_topics; ++i) {
topics.emplace_back(topic + "_" + std::to_string(i + 1));
}
return topics;
}
实际运行中，在backpack_2d.lua中有这样三个参数
num_laser_scans = 0,
num_multi_echo_laser_scans = 1,
num_point_clouds = 0
第一个是普通单线雷达，第二个是多重回波单线雷达，第三个是多线雷达。
参数是0的时候：进入以上的函数，由于num_topics == 0,既不会进入if (num_topics == 1)语句，也不会进入for (int i = 0; i < num_topics; ++i)。
参数是1的时候：进入以上的函数，会进入if (num_topics == 1)语句，直接返回了topic
参数是2的时候：进入以上的函数，会进入for (int i = 0; i < num_topics; ++i)，会进入for (int i = 0; i < num_topics; ++i)。cartographer官方代码没有参数是2的情况，我们可以设想一下如果是2的时候。推送到topic中的，将是topic_1和topic_2，很明显，实现的就是两个雷达的意思
2. 增加轨迹
上一个步骤生成了expected_sensor_ids变量，这一个步骤就要用到了，这一步的代码是
const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
AddTrajectory函数是很多类的成员函数，注意这里的AddTrajectory定义在cartographer_ros/map_builder_bridge.cc中
int MapBuilderBridge::AddTrajectory(
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids,
const TrajectoryOptions& trajectory_options) {
const int trajectory_id = map_builder_->AddTrajectoryBuilder(
expected_sensor_ids, trajectory_options.trajectory_builder_options,
[this](const int trajectory_id, const ::cartographer::common::Time time,
const Rigid3d local_pose,
::cartographer::sensor::RangeData range_data_in_local,
const std::unique_ptr<
const ::cartographer::mapping::TrajectoryBuilderInterface::
InsertionResult>) {
OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
});
LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
// Make sure there is no trajectory with 'trajectory_id' yet.
CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
trajectory_options.num_subdivisions_per_laser_scan,
trajectory_options.tracking_frame,
node_options_.lookup_transform_timeout_sec, tf_buffer_,
map_builder_->GetTrajectoryBuilder(trajectory_id));
auto emplace_result =
trajectory_options_.emplace(trajectory_id, trajectory_options);
CHECK(emplace_result.second == true);
return trajectory_id;
}
第一部分，添加Trajectory Builder
-
抛开参数不看，核心的语句是
const int trajectory_id = map_builder_->AddTrajectoryBuilder，一点一点的来理解，显然这个函数的返回值是一个int型 -
map_builder_是定义在map_builder_bridge.h中，std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
，它是一个指针。 -
AddTrajectoryBuilder定义在map_builder_interface.h中// Creates a new trajectory builder and returns its index. virtual int AddTrajectoryBuilder( const std::set<SensorId>& expected_sensor_ids, const proto::TrajectoryBuilderOptions& trajectory_options, LocalSlamResultCallback local_slam_result_callback) = 0;它的注释也很清楚，就是创建一个新的轨迹建造者，并且返回它的id。按照我现在的理解，一个轨迹建设者就会生成一张子图（submap）
-
第一个参数：
expected_sensor_ids它是函数
AddTrajectory的形参，实际传入的值就是上一步生成的expected_sensor_ids，两者是同名的 -
第二个参数：
trajectory_options.trajectory_builder_options它的实例类是
trajectory_options，也是函数AddTrajectory的形参，实际传入的值就是再上一层函数Node::AddTrajectory的形参，最终是node_main.cc中的Run函数中得到的结构体trajectory_options，它是从lua文件中读取到的参数，trajectory_builder_options是结构体的一个成员变量，定义在cartographer_ros/trajectory_options.h中::cartographer::mapping::proto::TrajectoryBuilderOptions trajectory_builder_options; -
第三个参数:一个函数
[this](const int trajectory_id, const ::cartographer::common::Time time, const Rigid3d local_pose, ::cartographer::sensor::RangeData range_data_in_local, const std::unique_ptr< const ::cartographer::mapping::TrajectoryBuilderInterface:: InsertionResult>) { OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local); }这是lambda表达式，我找时间研究一下，在这儿先说一下结论吧（应该是对的）
我们可以暂时理解为，执行的是这句话：OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);看一下函数的定义吧，在
cartographer/mapping/trajectory_builder_interface.h中// This interface is used for both 2D and 3D SLAM. Implementations wire up a // global SLAM stack, i.e. local SLAM for initial pose estimates, scan matching // to detect loop closure, and a sparse pose graph optimization to compute // optimized pose estimates. class TrajectoryBuilderInterface {注释的翻译：该接口可用于2D和3D SLAM。实现连接了一个全局SLAM堆栈，即用于初始姿态估计的局部SLAM，用于检测环路闭合的扫描匹配，以及用于计算优化姿态估计的稀疏姿态图优化。
// A callback which is called after local SLAM processes an accumulated // 'sensor::RangeData'. If the data was inserted into a submap, reports the // assigned 'NodeId', otherwise 'nullptr' if the data was filtered out. using LocalSlamResultCallback = std::function<void(int /* trajectory ID */, common::Time, transform::Rigid3d /* local pose estimate */, sensor::RangeData /* in local frame */, std::unique_ptr<const InsertionResult>)>;注释的翻译：在local SLAM处理累积的‘sensor::RangeData’后调用的回调函数。如果数据被插入到submap中，报告分配的’NodeId’，否则，如果数据被过滤掉，报告’nullptr’。
实际执行的就是以下的代码，定义在
cartographer_ros/map_builder_bridge.cc中void MapBuilderBridge::OnLocalSlamResult( const int trajectory_id, const ::cartographer::common::Time time, const Rigid3d local_pose, ::cartographer::sensor::RangeData range_data_in_local) { std::shared_ptr<const LocalTrajectoryData::LocalSlamData> local_slam_data = std::make_shared<LocalTrajectoryData::LocalSlamData>( LocalTrajectoryData::LocalSlamData{time, local_pose, std::move(range_data_in_local)}); absl::MutexLock lock(&mutex_); local_slam_data_[trajectory_id] = std::move(local_slam_data); }shared_ptr定义指定类型的shared_ptr，make_shared可以返回一个shared_ptr，两者可以一起出现这个叫做
local_slam_data的shared_ptr，指向的对象是一个struct，它定义在map_builder_bridge.h，源码在下面struct LocalTrajectoryData { // Contains the trajectory data received from local SLAM, after // it had processed accumulated 'range_data_in_local' and estimated // current 'local_pose' at 'time'. struct LocalSlamData { ::cartographer::common::Time time; ::cartographer::transform::Rigid3d local_pose; ::cartographer::sensor::RangeData range_data_in_local; }; std::shared_ptr<const LocalSlamData> local_slam_data; cartographer::transform::Rigid3d local_to_map; std::unique_ptr<cartographer::transform::Rigid3d> published_to_tracking; TrajectoryOptions trajectory_options; };注释的翻译：包含从本地SLAM接收到的轨迹数据，发生在处理累计的“range_data_in_local”和在“time”时估计当前的“local_pose”之后。
local_slam_data_是一个哈希表，定义在map_builder_bridge.hstd::unordered_map<int, std::shared_ptr<const LocalTrajectoryData::LocalSlamData>> local_slam_data_ GUARDED_BY(mutex_);整个函数干的事情就是，
local_slam_data_这个哈希表被推送了新的local slam 数据
第二部分，检查是否还有trajectory_id的轨迹
// Make sure there is no trajectory with 'trajectory_id' yet.
CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
trajectory_options.num_subdivisions_per_laser_scan,
trajectory_options.tracking_frame,
node_options_.lookup_transform_timeout_sec, tf_buffer_,
map_builder_->GetTrajectoryBuilder(trajectory_id));
auto emplace_result =
trajectory_options_.emplace(trajectory_id, trajectory_options);
CHECK(emplace_result.second == true);
return trajectory_id;
-
检查哈希表是否还存在trajectory_id的轨迹
CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);sensor_bridges_是一个哈希表，定义在map_builder_bridge.hstd::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;map中使用count，返回的是被查找元素的个数。如果有，返回1；否则，返回0。注意，map中不存在相同元素，所以返回值只能是1或0。
这句话的意思是，检查一下还有没有key== trajectory_id的value，也就是实现检查是否还有trajectory_id的轨迹。 -
按照我的理解应该是，新建key为trajectory_id的一个指针，确保以后能够找到该id的轨迹（待修正观点）
sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>( trajectory_options.num_subdivisions_per_laser_scan, trajectory_options.tracking_frame, node_options_.lookup_transform_timeout_sec, tf_buffer_, map_builder_->GetTrajectoryBuilder(trajectory_id));大体看过去，意思就是为sensor_bridges_这个哈希表，添加一个key为trajectory_id，value为右值的新键值对，右值将是一个
SensorBridge的unique_strSensorBridge的定义在cartographer_ros/sensor_bridge.h中，这个小节可以按照这个定义来梳理参数，下面是它的显示（explicit）构造函数explicit SensorBridge( int num_subdivisions_per_laser_scan, const std::string& tracking_frame, double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer, ::cartographer::mapping::TrajectoryBuilderInterface* trajectory_builder);上面说过了，
map_builder_是定义在map_builder_bridge.h中，std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
，它是一个指针。GetTrajectoryBuilder定义在cartographer/mapping/map_builder_interface.h中，是一个虚函数// Returns the 'TrajectoryBuilderInterface' corresponding to the specified // 'trajectory_id' or 'nullptr' if the trajectory has no corresponding // builder. virtual mapping::TrajectoryBuilderInterface* GetTrajectoryBuilder( int trajectory_id) const = 0;根据指定的’trajectory_id’或’nullptr’(如果轨迹没有相应的生成器)，返回
trajectory builderinterface。在
map_builder.h中对该函数进行了重写，mapping::TrajectoryBuilderInterface *GetTrajectoryBuilder( int trajectory_id) const override { return trajectory_builders_.at(trajectory_id).get(); }override保留字表示当前函数重写了基类的虚函数
-
将当前轨迹选项，添加总轨迹选项中
auto emplace_result = trajectory_options_.emplace(trajectory_id, trajectory_options); CHECK(emplace_result.second == true);trajectory_options_声明在map_builder_bridge.h，它是一个哈希表，key代表trajectory_id，value是TrajectoryOptions类型的structstd::unordered_map<int, TrajectoryOptions> trajectory_options_;map 容器的成员函数 emplace() 可以在适当的位置直接构造新元素，从而避免复制和移动操作。它的参数通常是构造元素。只有当容器中现有元素的键与这个元素的键不同时，才会构造这个元素。成员函数 emplace()返回的 pair 对象提供的指示相同。pair 的成员变量 first 是一个指向插入元素或阻止插入的元素的迭代器；成员变量 second 是个布尔值，如果元素插入成功，second 就为 true。
显然这句话的意思就是为
trajectory_options_构造了一个新的键值对，并且要检查是否插入成功
扫一扫
1946
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
