描述

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_构造了一个新的键值对,并且要检查是否插入成功