描述
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
1.ComputeExpectedSensorIds 确定传感器topic
2.AddTrajectory 增加轨迹
3.AddExtrapolator
4.AddSensorSamplers
5.LaunchSubscribers
6.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)
1.第一个参数:expected_sensor_ids
它是函数AddTrajectory的形参,实际传入的值就是上一步生成的expected_sensor_ids,两者是同名的
2.第二个参数: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;
3.第三个参数:一个函数
[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.h
std::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;
1.检查哈希表是否还存在trajectory_id的轨迹
CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
sensor_bridges_
是一个哈希表,定义在map_builder_bridge.h
std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
map中使用count,返回的是被查找元素的个数。如果有,返回1;否则,返回0。注意,map中不存在相同元素,所以返回值只能是1或0。
这句话的意思是,检查一下还有没有key== trajectory_id的value,也就是实现检查是否还有trajectory_id的轨迹。
2.按照我的理解应该是,新建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_str
SensorBridge的定义在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保留字表示当前函数重写了基类的虚函数
3.将当前轨迹选项,添加总轨迹选项中
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类型的struct
std::unordered_map<int, TrajectoryOptions> trajectory_options_;
map 容器的成员函数 emplace() 可以在适当的位置直接构造新元素,从而避免复制和移动操作。它的参数通常是构造元素。只有当容器中现有元素的键与这个元素的键不同时,才会构造这个元素。成员函数 emplace()返回的 pair 对象提供的指示相同。pair 的成员变量 first 是一个指向插入元素或阻止插入的元素的迭代器;成员变量 second 是个布尔值,如果元素插入成功,second 就为 true。
显然这句话的意思就是为trajectory_options_构造了一个新的键值对,并且要检查是否插入成功
评论(0)
您还未登录,请登录后发表或查看评论