cartographer 处理IMU（激光，里程计等）流程

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

xiaoma_bk

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

阅读量1.2w

点赞数 10

分类专栏： # Cartographer 文章标签： IMU cartographer data

本文链接：https://blog.csdn.net/xiaoma_bk/article/details/81387979

版权

Cartographer 专栏收录该内容

27 篇文章 18 订阅

订阅专栏

1、cartographer_ros
入口文件：node_main.cc
入口函数main,如下图：

  ::ros::init(argc, argv, "cartographer_node");
  ::ros::start();

  cartographer_ros::ScopedRosLogSink ros_log_sink;
  cartographer_ros::Run();

2、void Run() 启动轨迹处

  if (FLAGS_start_trajectory_with_default_topics) {
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }

3、文件node.cc

AddTrajectory(options, DefaultSensorTopics()); 函数

LaunchSubscribers(options, topics, trajectory_id); 订阅有关话题

4、LaunchSubscribers

{SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
                                        trajectory_id, topic,
                                       &node_handle_, this),

5、HandleImuMessage兵分两路一路用于位姿推算一路用于后面的计算

  if (imu_data_ptr != nullptr) {
    extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
  }
  sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);

6、位姿推算这儿

文件：cartographer/mapping/nternal/pose_extrapolator.cc

void PoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) {
  CHECK(timed_pose_queue_.empty() ||
        imu_data.time >= timed_pose_queue_.back().time);
  imu_data_.push_back(imu_data);
  TrimImuData();
}

7、后面的IMU运算到了 sensor_bridge.cc

void SensorBridge::HandleImuMessage(const std::string& sensor_id,
                                    const sensor_msgs::Imu::ConstPtr& msg) {
  std::unique_ptr<carto::sensor::ImuData> imu_data = ToImuData(msg);
  if (imu_data != nullptr) {
    trajectory_builder_->AddSensorData(
        sensor_id,
        carto::sensor::ImuData{imu_data->time, imu_data->linear_acceleration,
                               imu_data->angular_velocity});
  }
}

8、在添加轨迹中定义了变量 CollatedTrajectoryBuilder 继承TrajectoryBuilderInterface（虚函数，接口）

  void AddSensorData(const std::string& sensor_id,
                     const sensor::ImuData& imu_data) override {
    AddData(sensor::MakeDispatchable(sensor_id, imu_data));
  }

9、而 sensor_collator_->AddSensorData中， sensor::Collator sensor_collator_;在map_builder.h中定义了。继而找到在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));
}

10、这里就会把IMU数据加入队列啦。std::unique_ptr是标准库的智能指针，与auto_ptr真的非常类似，auto_ptr可以说可以随便赋值，但赋值完后原来的对象就不知不觉报废，而unique_ptr不让你随便赋值，只能move内存转移。

11、注意到在collator.h中一个函数：（ 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);
  }
}

在 CollatedTrajectoryBuilder的构造函数中回调了 HandleCollatedSensorData

CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( //整理的轨迹生成器
    const proto::TrajectoryBuilderOptions& trajectory_options,
    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),
      collate_landmarks_(trajectory_options.collate_landmarks()),
      collate_fixed_frame_(trajectory_options.collate_fixed_frame()),
      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) {
    if (sensor_id.type == SensorId::SensorType::LANDMARK &&
        !collate_landmarks_) {
      continue;
    }
    if (sensor_id.type == SensorId::SensorType::FIXED_FRAME_POSE &&
        !collate_fixed_frame_) {
      continue;
    }
    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));
      });
}

12、它在CollatedTrajectoryBuilder的构造函数中调用了，传入的回调函数名为HandleCollatedSensorData，处理整理后的传感器数据，到这里就开始进入关键正题啦！

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

13、在头文件cartographer/mapping_2d/global_trajectory_builder.cc中，知道GlobalTrajectoryBuilder类继承了GlobalTrajectoryBuilderInterface类，如图：

template <typename LocalTrajectoryBuilder, typename PoseGraph>
class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {
 public:

14、自然就能找到GlobalTrajectoryBuilder::AddImuData的具体实现了

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

15、pose_graph是PoseGraph2D的父类，虚函数，多态

void PoseGraph2D::AddImuData(const int trajectory_id,
                             const sensor::ImuData& imu_data) {
  AddWorkItem([=]() EXCLUDES(mutex_) {
    common::MutexLocker locker(&mutex_);
    if (CanAddWorkItemModifying(trajectory_id)) {
      optimization_problem_->AddImuData(trajectory_id, imu_data);
    }
    return WorkItem::Result::kDoNotRunOptimization;
  });
}

16、OptimizationProblem类

void OptimizationProblem2D::AddImuData(const int trajectory_id,
                                       const sensor::ImuData& imu_data) {
  imu_data_.Append(trajectory_id, imu_data);
}

17、map_by_time.h 将数据附加到'trajectory_id'，根据需要创建轨迹。

  void Append(const int trajectory_id, const DataType& data) {
    CHECK_GE(trajectory_id, 0);
    auto& trajectory = data_[trajectory_id];
    if (!trajectory.empty()) {
      CHECK_GT(data.time, std::prev(trajectory.end())->first);
    }
    trajectory.emplace(data.time, data);
  }