cartographer探秘第四章之代码解析（二） --- 传感器数据处理过程

最新推荐文章于 2024-09-11 09:46:22 发布

李太白lx

最新推荐文章于 2024-09-11 09:46:22 发布

阅读量2.7k

点赞数 3

分类专栏： cartographer 文章标签： cartographer代码

本文链接：https://blog.csdn.net/tiancailx/article/details/96138354

版权

cartographer 专栏收录该内容

20 篇文章 64 订阅

订阅专栏

本次阅读的源码为 release-1.0 版本的代码

https://github.com/googlecartographer/cartographer_ros/tree/release-1.0

https://github.com/googlecartographer/cartographer/tree/release-1.0

也可以下载我上传的全套工作空间的代码，包括 protobuf, cartographer, cartographer_ros, ceres,

https://download.csdn.net/download/tiancailx/11224156

第一篇文章简要分析了下 carto的代码是如何使用的，以及如何进行SLAM的。这篇文章将进雷达数据处理流程的分析。

在看 trajectory_builder_interface.h 之前需要了解几个数据类型。

cartographer/mapping/submaps.h

// An individual submap, which has a 'local_pose' in the local map frame, keeps
// track of how many range data were inserted into it, and sets the
// 'finished_probability_grid' to be used for loop closing once the map no
// longer changes.
// 一个单独的子图，在local map frame中有一个'local_pose'，跟踪插入到其中的范围数据的数量，
// 并设置'finished_probability_grid'用于在地图不再更改时 loop closing。
class Submap {
 public:
  Submap(const transform::Rigid3d& local_submap_pose)
      : local_pose_(local_submap_pose) {}

// ...

 private:
  const transform::Rigid3d local_pose_;
  int num_range_data_ = 0;
  bool finished_ = false;
};

cartographer/mapping/trajectory_node.h

struct TrajectoryNodePose {
  struct ConstantPoseData {
    common::Time time;
    transform::Rigid3d local_pose;
  };
  // The node pose in the global SLAM frame.
  transform::Rigid3d global_pose;

  common::optional<ConstantPoseData> constant_pose_data;
};

struct TrajectoryNode {
  struct Data {
    common::Time time;

    // Transform to approximately gravity align the tracking frame as
    // determined by local SLAM.
    Eigen::Quaterniond gravity_alignment;

    // Used for loop closure in 2D: voxel filtered returns in the
    // 'gravity_alignment' frame.
    sensor::PointCloud filtered_gravity_aligned_point_cloud;

    // Used for loop closure in 3D.
    sensor::PointCloud high_resolution_point_cloud;
    sensor::PointCloud low_resolution_point_cloud;
    Eigen::VectorXf rotational_scan_matcher_histogram;

    // The node pose in the local SLAM frame.
    transform::Rigid3d local_pose;
  };

  common::Time time() const { return constant_data->time; }

  // This must be a shared_ptr. If the data is used for visualization while the
  // node is being trimmed, it must survive until all use finishes.
  std::shared_ptr<const Data> constant_data;

  // The node pose in the global SLAM frame.
  transform::Rigid3d global_pose;
};

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 {
 public:
  struct InsertionResult {
    NodeId node_id;
    std::shared_ptr<const TrajectoryNode::Data> constant_data;
    std::vector<std::shared_ptr<const Submap>> insertion_submaps;
  };

  // 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>)>;

SensorId

type 是传感器类型，id 是就是ros 的　话题，多个相同传感器时在　话题　名字后加　_1。

  struct SensorId {
    enum class SensorType {
      RANGE = 0,
      IMU,
      ODOMETRY,
      FIXED_FRAME_POSE,
      LANDMARK,
      LOCAL_SLAM_RESULT
    };

    SensorType type;
    std::string id;
｝

数据处理过程　AddSensorData()：

trajectory_builder->AddSensorData(kRangeSensorId.id, measurement);

cartographer/mapping/internal/collated_trajectory_builder.h

// cartographer/mapping/internal/collated_trajectory_builder.cc
class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface {
// ...
}

CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
    sensor::CollatorInterface* const sensor_collator, const int trajectory_id,
    const std::set<SensorId>& 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()) {
  std::unordered_set<std::string> expected_sensor_id_strings;
  for (const auto& sensor_id : expected_sensor_ids) {
    expected_sensor_id_strings.insert(sensor_id.id);
  }
  sensor_collator_->AddTrajectory(
      trajectory_id, expected_sensor_id_strings,
      [this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
        HandleCollatedSensorData(sensor_id, std::move(data));
      });

// cartographer/sensor/collator_interface.h
using Callback =
      std::function<void(const std::string&, std::unique_ptr<Data>)>;

  // Adds a trajectory to produce sorted sensor output for. Calls 'callback'
  // for each collated sensor data.
  virtual void AddTrajectory(
      int trajectory_id,
      const std::unordered_set<std::string>& expected_sensor_ids,
      const Callback& callback) = 0;


// cartographer/sensor/internal/collator.cc

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);
  }
}

// cartographer/sensor/internal/ordered_multi_queue.cc
// 添加一个关键词是key的队列,并用比较函数Callback排序
void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) {
  CHECK_EQ(queues_.count(queue_key), 0);
  queues_[queue_key].callback = std::move(callback);
}

sensor_collator->AddTrajectory() 的参数为：1 一个数据包就是一个trajectory_id，跑定位时会有２个id，多机器人SLAM会有很多个id， 2 expected_sensor_ids_strings 就是所有的　topic ，3 传入回调函数的函数名 - HandleCollatedSensorData() 。

调用了trajectory_builder_interface.h的AddSensorData()方法，这个方法是在collated_trajectory_builder.h　进行了具体的实现，它将　trajectory_id　和　传感器的数据　传入到队列中，cartographer/sensor/internal/ordered_multi_queue.h　这个文件声明了队列的定义. ordered_multi_queue.h 具体的说明请去　参考文献３进行查看，我就不多说了。

// cartographer/mapping/trajectory_builder_interface.h
  virtual void AddSensorData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data) = 0;
  virtual void AddSensorData(const std::string& sensor_id,
                             const sensor::ImuData& imu_data) = 0;
  virtual void AddSensorData(const std::string& sensor_id,
                             const sensor::OdometryData& odometry_data) = 0;
  virtual void AddSensorData(
      const std::string& sensor_id,
      const sensor::FixedFramePoseData& fixed_frame_pose) = 0;
  virtual void AddSensorData(const std::string& sensor_id,
                             const sensor::LandmarkData& landmark_data) = 0;

// cartographer/mapping/internal/collated_trajectory_builder.h
// 传感器的数据　data 是通过一个模板类　cartographer/sensor/internal/dispatchable.h　进行了多种数据类型的重载。
  void AddSensorData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data) override {
    AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
  }

void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
  sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}

// cartographer/sensor/internal/collator.cc
void Collator::AddSensorData(const int trajectory_id,
                             std::unique_ptr<Data> data) {
  QueueKey queue_key{trajectory_id, data->GetSensorId()};
  queue_.Add(std::move(queue_key), std::move(data));
}

// cartographer/sensor/internal/ordered_multi_queue.cc
// 根据key找到队列,并添加data元素, 调用一次Add()就要调用一次Dispatch()
void OrderedMultiQueue::Add(const QueueKey& queue_key,
                            std::unique_ptr<Data> data) {
  auto it = queues_.find(queue_key);
  if (it == queues_.end()) {
    LOG_EVERY_N(WARNING, 1000)
        << "Ignored data for queue: '" << queue_key << "'";
    return;
  }
  it->second.queue.Push(std::move(data));
  Dispatch();
}

/*
OrderedMultiQueue，用于管理多个有序的传感器数据，
是有序的多队列类,每个队列有一个key,并且有一个自定义排序函数
queues_的形式为：
key1:Queue
key2：Queue
key3：Queue

Queue的形式为
  struct Queue {
    common::BlockingQueue<std::unique_ptr<Data>> queue;
    Callback callback;
    bool finished = false;
  };

OrderedMultiQueue的数据成员有
1,common_start_time_per_trajectory_:轨迹id及对应创建轨迹时间
2,last_dispatched_time_
3,std::map<QueueKey, Queue> queues_;按照key排序的map
4,QueueKey blocker_;

--------------------- 
作者：slamcode 
来源：CSDN 
原文：https://blog.csdn.net/learnmoreonce/article/details/76218106 
版权声明：本文为博主原创文章，转载请附上博文链接！
*/

也就是说，队列的个数为：每个轨迹所订阅的话题数。如下，２个轨迹，轨迹１订阅２个话题，轨迹２订阅１个话题，那我的队列的map就只有３个key，也就是３个队列。

  // These are keys are chosen so that they sort first, second, third.
  const QueueKey kFirst{1, "a"};
  const QueueKey kSecond{1, "b"};
  const QueueKey kThird{2, "b"};

总结：每次数据到来，根据，调用trajectory_builder_interface.h的AddSensorData()，再调用ordered_multi_queue.cc的Add()方法，再调用OrderedMultiQueue::Dispatch()方法，Dispatch()方法使用　回调函数CollatedTrajectoryBuilder::HandleCollatedSensorData()　处理　根据数据的时间已经排好序的数据队列。

// cartographer/mapping/internal/collated_trajectory_builder.cc
void CollatedTrajectoryBuilder::HandleCollatedSensorData(
    const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
  auto it = rate_timers_.find(sensor_id);
  if (it == rate_timers_.end()) {
    it = rate_timers_
             .emplace(
                 std::piecewise_construct, std::forward_as_tuple(sensor_id),
                 std::forward_as_tuple(
                     common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)))
             .first;
  }
  it->second.Pulse(data->GetTime());

  if (std::chrono::steady_clock::now() - last_logging_time_ >
      common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
    for (const auto& pair : rate_timers_) {
      LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();
    }
    last_logging_time_ = std::chrono::steady_clock::now();
  }

  data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
}

// 也就是跑carto时候的消息：
// [ INFO]: collated_trajectory_builder.cc:72] imu rate: 10.00 Hz 1.00e-01 s +/- 4.35e-05 s (pulsed at 100.44% real time)
// [ INFO]: collated_trajectory_builder.cc:72] scan rate: 19.83 Hz 5.04e-02 s +/- 4.27e-05 s (pulsed at 99.82% real time)



// cartographer/sensor/internal/dispatchable.h
  void AddToTrajectoryBuilder(
      mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
    trajectory_builder->AddSensorData(sensor_id_, data_);
  }

之后在通过CollatedTrajectoryBuilder::HandleCollatedSensorData() 方法将这个　已经排好序的数据队列集合　传回　Local SLAM.　那这是怎么传的呢？又看了好久才看懂这个转换。

首先，先进入到 map_builder.cc　中。这是整体SLAM的处理过程。

cartographer/mapping/map_builder.cc

// cartographer/mapping/map_builder.cc
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();
  
  if (options_.use_trajectory_builder_3d()) {
...
} else {
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_2d_options()) {
      local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder2D>(
          trajectory_options.trajectory_builder_2d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }
    DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
    trajectory_builders_.push_back(
        // 看这里
        common::make_unique<CollatedTrajectoryBuilder>(
            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)));
...}

AddTrajectoryBuilder()　方法就是一个整体SLAM的初始化方法。

首先我们这里只看2d SLAM 的部分，也就是在上述代码中标记的　看这里　。　这块代码执行了如下功能，首先，将CollatedTrajectoryBuilder() 进行初始化，并且传入　sensor_collator_.get(), trajectory_id　和　expected_sensor_ids　参数。expected_sensor_ids是调用AddTrajectoryBuilder()时的参数输入，代表着所有传感器话题的集合。同时，也传入了一个　CreateGlobalTrajectoryBuilder2D() 的函数名，这是GlobalTrajectoryBuilder类的构造函数，直接传入函数名就是传入函数的地址，而正好CollatedTrajectoryBuilder()构造函数的第四个参数是个指针 std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder . 之后又使用wrapped_trajectory_builder进行右值转换后对 wrapped_trajectory_builder_进行赋值。

之后使用HandleCollatedSensorData()将这个指针指向的数据传入到 data的AddToTrajectoryBuilder()　方法中，也就是将GlobalTrajectoryBuilder类的函数指针（？？？对不对，是传入地址指针还是传入什么）传入进去。

由于GlobalTrajectoryBuilder类是继承于mapping::TrajectoryBuilderInterface的，所以指针的类型是 std::unique_ptr<TrajectoryBuilderInterface> 。

之后在dispatchable.h　文件中的　AddToTrajectoryBuilder() 方法　调用　GlobalTrajectoryBuilder类下的AddSensorData().

// cartographer/mapping/internal/collated_trajectory_builder.cc
void CollatedTrajectoryBuilder::HandleCollatedSensorData() {
  //  ...
  data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
}

// cartographer/sensor/internal/dispatchable.h
  void AddToTrajectoryBuilder(
      mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
    trajectory_builder->AddSensorData(sensor_id_, data_);
  }

GlobalTrajectoryBuilder类下的AddSensorData() 将进行　local slam　的扫描匹配，下篇文章再讲。