Cartographer源码阅读(4)：Node和MapBuilder对象2

合集 - SLAM(55)

1.个人资源索引2018-03-03 2.SLAM学习笔记（1）基本概念2015-08-06 3.SLAM学习笔记（2）SLAM算法2015-08-21 4.[ROS]2 尝试编译OrbSLAM2016-01-18 5.[ROS]1 小乌龟2016-01-18 6.SLAM最近的工作2015-11-05 7.SLAM学习笔记（3）相关概念2015-09-16 8.[SLAM]2D激光线特征提取2016-07-16 9.[SLAM] GMapping SLAM源码阅读（草稿）2016-07-01 10.[QGLViewer]First Demo2016-06-29 11.SLAM数据集2016-06-23 12.[SLAM]2D激光扫描匹配方法2016-06-15 13.[g2o]一个备忘2016-04-21 14.[ROS] slam_gmapping2016-01-24 15.[ROS]3 Linux编程练习2016-01-20 16.Linux学习和ROS安装（1）2017-12-19 17.小豆包的学习之旅：机器人定位2017-05-02 18.小豆包的学习之旅：传感器观测模型2017-04-21 19.Kinect2.0相机标定2017-04-10 20.小豆包的学习之旅：里程计运动模型2017-04-03 21.小豆包的学习之旅：入门篇2017-04-02 22.Kinect2.0点云数据获取2017-03-30 23.[硬件]Robot运动控制2017-03-22 24.[硬件]Urg_viewer数据读取2017-03-14 25.[硬件]三维点云数据获取2017-03-04 26.Kinect2.0获取数据2017-02-15 27.ndt histogram_direction2016-09-08 28.rplidar & hector slam without odometry2016-08-26 29.rplidar测试2016-08-12 30.[原创]NDT方法在SLAM中的应用2016-08-08 31.[SLAM]Karto SLAM算法学习（草稿）2016-07-22 32.ROS学习备忘2018-07-18 33.[ROS]激光驱动安装2018-07-16 34.OrbSLAM2采集点云数据2018-03-26 35.Cartographer源码阅读(8)：imu_tracker2018-03-21 36.[硬件]点云数据采集22018-03-19 37.Cartographer源码阅读(9)：图优化的前端——闭环检测2018-03-08 38.Cartographer源码阅读(7)：轨迹推算和位姿推算的原理2018-03-08 39.Ethzasl MSF源码阅读(3)：MSF_Core和PoseMeasurement2018-03-07 40.Ethzasl MSF源码阅读(2)：百川汇海2018-03-06 41.Ethzasl MSF源码阅读(1)：程序入口和主题订阅2018-03-04 42.Cartographer源码阅读(6)：LocalTrajectoryBuilder和PoseExtrapolator2018-02-25 43.Cartographer源码阅读(5)：PoseGraph位姿图2018-02-24

44.Cartographer源码阅读(4)：Node和MapBuilder对象22018-01-10

45.Cartographer源码阅读(3)：程序逻辑结构2017-12-29 46.Cartographer源码阅读(2)：Node和MapBuilder对象2017-12-28 47.Cartographer源码阅读(1)：程序入口2017-12-28 48.实时Cartographer测试(1) - rplidar2017-12-21 49.Cartographer安装2017-12-20 50.ROS安装（2）2017-12-20 51.[ROS]一些传感器数据读取融合问题的思考2017-05-06 52.小豆包的学习之旅：占用概率栅格地图和cost-map2017-05-02 53.小豆包的学习之旅：开发记录2017-04-29 54.[概述]移动机器人自主探索2017-04-18 55.MRPT编译2017-04-14

　　MapBuilder的成员变量sensor::Collator sensor_collator_;

　　再次阅读MapBuilder::AddTrajectoryBuilder方法。首先构造了mapping::GlobalTrajectoryBuilder实例，接着作为参数构造了CollatedTrajectoryBuilder实例。

trajectory_builders_.push_back(
common::make_unique<CollatedTrajectoryBuilder>(
&sensor_collator_, trajectory_id, expected_sensor_ids,
common::make_unique<mapping::GlobalTrajectoryBuilder<mapping_2d::LocalTrajectoryBuilder,mapping_2d::proto::LocalTrajectoryBuilderOptions,mapping_2d::PoseGraph>>
(trajectory_options.trajectory_builder_2d_options(),trajectory_id, pose_graph_2d_.get(),local_slam_result_callback)
)
);

　　这里sensor_collator_作为参数传入，参与CollatedTrajectoryBuilder构造。查看构造函数：

CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(sensor::Collator* const sensor_collator, const int trajectory_id, const std::unordered_set<std::string>& expected_sensor_ids,   std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)
    : sensor_collator_(sensor_collator)
    , trajectory_id_(trajectory_id)
    , wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder))
    , last_logging_time_(std::chrono::steady_clock::now()) 
{
     sensor_collator_->AddTrajectory(trajectory_id, expected_sensor_ids,
      [this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data)
      {
        HandleCollatedSensorData(sensor_id, std::move(data));
      }
      );
}

　　这里是回调函数，std::unique_ptr是表示参数为智能指针。

[this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data)
{
       HandleCollatedSensorData(sensor_id, std::move(data));
 }

　　（1）查看sensor::Collator的AddTrajectory方法：

void Collator::AddTrajectory( const int trajectory_id, const std::unordered_set<std::string>& expected_sensor_ids, const Callback& callback) 
{
   for (const auto& sensor_id : expected_sensor_ids)
  {
      const auto queue_key = QueueKey{trajectory_id, sensor_id};
      queue_.AddQueue(queue_key, [callback, sensor_id](std::unique_ptr<Data> data)
                    {
                      callback(sensor_id, std::move(data));
                    });
       queue_keys_[trajectory_id].push_back(queue_key);
  }
}

　　for (const auto& sensor_id : expected_sensor_ids)用到了C++11的auto新特性。

　　（2）查看HandleCollatedSensorData方法。调用了data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());这里wrapped_trajectory_builder_是在CollatedTrajectoryBuilder构造函数中赋值的。为GlobalTrajectoryBuilder对象。因而查看sensor::Data的AddToTrajectoryBuilder() 方法。

　　virtual void AddToTrajectoryBuilder(mapping::TrajectoryBuilderInterface *trajectory_builder) = 0;是sensor::Data类的一个虚方法。内部执行了trajectory_builder->AddSensorData(sensor_id_, data_);

最后调用的是GlobalTrajectoryBuilder对象的AddSensorData(xx)方法。

 1 void CollatedTrajectoryBuilder::HandleCollatedSensorData( const std::string& sensor_id, std::unique_ptr<sensor::Data> data)
 2 {
 3   auto it = rate_timers_.find(sensor_id);
 4   if (it == rate_timers_.end())
 5   {
 6       it = rate_timers_ .emplace(
 7                  std::piecewise_construct, std::forward_as_tuple(sensor_id),
 8                  std::forward_as_tuple(common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds))) .first;
 9   }
10   it->second.Pulse(data->GetTime());
11 
12   if (std::chrono::steady_clock::now() - last_logging_time_ >
13       common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) 
14   {
15        for (const auto& pair : rate_timers_) 
16       {
17         LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();
18       }
19       last_logging_time_ = std::chrono::steady_clock::now();
20   }
21 
22    data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
23   }
24 
25 }

CollatedTrajectoryBuilder::HandleCollatedSensorData

template <typename DataType>
class Dispatchable : public Data 
{
 public:
  Dispatchable(const std::string &sensor_id, const DataType &data): Data(sensor_id), data_(data) {}
 
  common::Time GetTime() const override { return data_.time; }
 
  void AddToTrajectoryBuilder( mapping::TrajectoryBuilderInterface *const trajectory_builder) override 
 {
    trajectory_builder->AddSensorData(sensor_id_, data_);
  }
 
 private:
  const DataType data_;
};

　　再以IMU数据为例，GlobalTrajectoryBuilder类的AddSensorData(xx)：

void AddSensorData(const std::string& sensor_id,  const sensor::ImuData& imu_data) override 
{
      local_trajectory_builder_.AddImuData(imu_data);
      pose_graph_->AddImuData(trajectory_id_, imu_data);
}

　　再看一下激光点云的数据

 1 void AddSensorData( const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) override
 2  {
 3     std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>  matching_result = 
 4        local_trajectory_builder_.AddRangeData( timed_point_cloud_data.time, 
 5                                                sensor::TimedRangeData {timed_point_cloud_data.origin, 
 6                                                timed_point_cloud_data.ranges, {}}
 7        );
 8     if (matching_result == nullptr) 
 9     {
10         // The range data has not been fully accumulated yet.
11         return;
12     }
13     std::unique_ptr<mapping::NodeId> node_id;
14     if (matching_result->insertion_result != nullptr) 
15     {
16           node_id = ::cartographer::common::make_unique<mapping::NodeId>(
17           pose_graph_->AddNode(matching_result->insertion_result->constant_data, 
18 trajectory_id_, matching_result->insertion_result->insertion_submaps));
19           CHECK_EQ(node_id->trajectory_id, trajectory_id_);
20     }
21     if (local_slam_result_callback_) 
22     {
23       local_slam_result_callback_( trajectory_id_, matching_result->time, 
24              matching_result->local_pose,
25           std::move(matching_result->range_data_in_local), std::move(node_id));
26      }
27   }

　　这里有两个重要的步骤一个是local_trajectory_builder_.AddRangeData（xxx），一个是 pose_graph_->AddNode(xxx)方法。同时std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult> matching_result作为AddNode方法的参数。

 1 mapping::NodeId PoseGraph::AddNode(
 2     std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
 3     const int trajectory_id,
 4     const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
 5   const transform::Rigid3d optimized_pose(
 6       GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
 7 
 8   common::MutexLocker locker(&mutex_);
 9   AddTrajectoryIfNeeded(trajectory_id);
10   const mapping::NodeId node_id = trajectory_nodes_.Append(
11       trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
12   ++num_trajectory_nodes_;
13 
14   // Test if the 'insertion_submap.back()' is one we never saw before.
15   if (submap_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
16       std::prev(submap_data_.EndOfTrajectory(trajectory_id))->data.submap !=
17           insertion_submaps.back()) {
18     // We grow 'submap_data_' as needed. This code assumes that the first
19     // time we see a new submap is as 'insertion_submaps.back()'.
20     const mapping::SubmapId submap_id =
21         submap_data_.Append(trajectory_id, SubmapData());
22     submap_data_.at(submap_id).submap = insertion_submaps.back();
23   }
24 
25   // We have to check this here, because it might have changed by the time we
26   // execute the lambda.
27   const bool newly_finished_submap = insertion_submaps.front()->finished();
28   AddWorkItem([=]() REQUIRES(mutex_) {
29     ComputeConstraintsForNode(node_id, insertion_submaps,
30                               newly_finished_submap);
31   });
32   return node_id;
33 }