Cartographer Standalone移植（1）—移植思路

最新推荐文章于 2023-05-18 13:38:53 发布

火种源码

最新推荐文章于 2023-05-18 13:38:53 发布

阅读量1.2k

点赞数 11

分类专栏：算法文章标签： c++ linux

本文链接：https://blog.csdn.net/zhzwang/article/details/108884357

版权

算法专栏收录该内容

21 篇文章 33 订阅

订阅专栏

任务动机：Cartographer建图对硬件资源要求高，为了实现在RK3399开发板上高质量的建图，需要使用各种方法降低资源消耗，包括操作系统对资源的消耗、ROS Core对资源的消耗。本文将Cartographer与ROS剥离，目的是在不启动ROS Core的基础上正常使用Cartographer建图，并通过对ROS Bridge的支持完成对分布式ROS开发板的通信。

任务描述：根据任务动机，研究移植研发思路，形成文档。

1. 输入/输出要求

输入：激光雷达/drv/laser
输出：地图 .pgm/.pbstream

2. 移植思路

只安装cartographer和ceres-solver，以cartographer_ros为示例，编写一个自己的程序。

3. 移植过程

在分析源码的过程中，我把所有的与ROS有关的类和相关库全部过滤。这个代码写的就像拔萝卜，开始拔以后发现地下除了泥还有树根。我打算直接用硬件激光雷达测试(因为没安装ros，还要写py脚本去从rosbag中解压激光雷达数据确实有些麻烦)。因此我在此写出我的思路，并在结尾提出疑问。

3.1 建图，开始拔萝卜之看见萝卜->建图前准备：载入参数

  NodeOptions node_options; //创建NodeOptions参数结构体
  TrajectoryOptions trajectory_options; //创建TrajectoryOptions参数结构体
  //把Lua的参数加载到程序中
  std::tie(node_options, trajectory_options) =
      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);

......

std::tuple<NodeOptions, TrajectoryOptions> LoadOptions( //加载参数
    const std::string& configuration_directory,         //参数所在目录
    const std::string& configuration_basename) {
  //标准库，把字符串参数所在目录放到file_resolver这个目录中
  auto file_resolver =
      absl::make_unique<cartographer::common::ConfigurationFileResolver>(
          std::vector<std::string>{configuration_directory});
  //标准库+cartographer库，目的是把文件中的字符提取出来
  const std::string code =
      file_resolver->GetFileContentOrDie(configuration_basename);
  //cartographer库，相当于configuration_directory+configuration_basename
  cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
      code, std::move(file_resolver));
  //返回参数
  return std::make_tuple(CreateNodeOptions(&lua_parameter_dictionary),
                         CreateTrajectoryOptions(&lua_parameter_dictionary));
}

......

//node结构体
struct NodeOptions {
  ::cartographer::mapping::proto::MapBuilderOptions map_builder_options;
  std::string map_frame;
  double lookup_transform_timeout_sec;
  double submap_publish_period_sec;
  double pose_publish_period_sec;
  double trajectory_publish_period_sec;
  bool use_pose_extrapolator = true;
};

//创建node参数，只依赖标准库和cartographer库
NodeOptions CreateNodeOptions(
    ::cartographer::common::LuaParameterDictionary* const
        lua_parameter_dictionary) {
  NodeOptions options;
  options.map_builder_options =
      ::cartographer::mapping::CreateMapBuilderOptions(
          lua_parameter_dictionary->GetDictionary("map_builder").get());
  options.map_frame = lua_parameter_dictionary->GetString("map_frame");
  options.lookup_transform_timeout_sec =
      lua_parameter_dictionary->GetDouble("lookup_transform_timeout_sec");
  options.submap_publish_period_sec =
      lua_parameter_dictionary->GetDouble("submap_publish_period_sec");
  options.pose_publish_period_sec =
      lua_parameter_dictionary->GetDouble("pose_publish_period_sec");
  options.trajectory_publish_period_sec =
      lua_parameter_dictionary->GetDouble("trajectory_publish_period_sec");
  if (lua_parameter_dictionary->HasKey("use_pose_extrapolator")) {
    options.use_pose_extrapolator =
        lua_parameter_dictionary->GetBool("use_pose_extrapolator");
  }
  return options;
}

······

//TrajectoryOptions结构体
struct TrajectoryOptions {
  ::cartographer::mapping::proto::TrajectoryBuilderOptions
      trajectory_builder_options;
  std::string tracking_frame;
  std::string published_frame;
  std::string odom_frame;
  bool provide_odom_frame;
  bool use_odometry;
  bool use_nav_sat;
  bool use_landmarks;
  bool publish_frame_projected_to_2d;
  int num_laser_scans;
  int num_multi_echo_laser_scans;
  int num_subdivisions_per_laser_scan;
  int num_point_clouds;
  double rangefinder_sampling_ratio;
  double odometry_sampling_ratio;
  double fixed_frame_pose_sampling_ratio;
  double imu_sampling_ratio;
  double landmarks_sampling_ratio;
};

//TrajectoryOptions参数加载
TrajectoryOptions CreateTrajectoryOptions(
    ::cartographer::common::LuaParameterDictionary* const
        lua_parameter_dictionary) {
  TrajectoryOptions options;
  options.trajectory_builder_options =
      ::cartographer::mapping::CreateTrajectoryBuilderOptions(
          lua_parameter_dictionary->GetDictionary("trajectory_builder").get());
  options.tracking_frame =
      lua_parameter_dictionary->GetString("tracking_frame");
  options.published_frame =
      lua_parameter_dictionary->GetString("published_frame");
  options.odom_frame = lua_parameter_dictionary->GetString("odom_frame");
  options.provide_odom_frame =
      lua_parameter_dictionary->GetBool("provide_odom_frame");
  options.use_odometry = lua_parameter_dictionary->GetBool("use_odometry");
  options.use_nav_sat = lua_parameter_dictionary->GetBool("use_nav_sat");
  options.use_landmarks = lua_parameter_dictionary->GetBool("use_landmarks");
  options.publish_frame_projected_to_2d =
      lua_parameter_dictionary->GetBool("publish_frame_projected_to_2d");
  options.num_laser_scans =
      lua_parameter_dictionary->GetNonNegativeInt("num_laser_scans");
  options.num_multi_echo_laser_scans =
      lua_parameter_dictionary->GetNonNegativeInt("num_multi_echo_laser_scans");
  options.num_subdivisions_per_laser_scan =
      lua_parameter_dictionary->GetNonNegativeInt(
          "num_subdivisions_per_laser_scan");
  options.num_point_clouds =
      lua_parameter_dictionary->GetNonNegativeInt("num_point_clouds");
  options.rangefinder_sampling_ratio =
      lua_parameter_dictionary->GetDouble("rangefinder_sampling_ratio");
  options.odometry_sampling_ratio =
      lua_parameter_dictionary->GetDouble("odometry_sampling_ratio");
  options.fixed_frame_pose_sampling_ratio =
      lua_parameter_dictionary->GetDouble("fixed_frame_pose_sampling_ratio");
  options.imu_sampling_ratio =
      lua_parameter_dictionary->GetDouble("imu_sampling_ratio");
  options.landmarks_sampling_ratio =
      lua_parameter_dictionary->GetDouble("landmarks_sampling_ratio");
  CheckTrajectoryOptions(options);
  return options;
}

以上是Lua中的内容，当我们正确把参数加载到程序中以后，我们就可以开始建图了。

3.2 拨开上层土，发现树根->建图中：理解如何调用cartographer库

//创建map_builder，直接从cartographer库中获取的默认参数
auto map_builder = absl::make_unique<cartographer::mapping::MapBuilder>(
    node_options.map_builder_options);
//申请了一个node类型的函数，传入了四个参数，让我们看看他里面干了什么
Node node(node_options, std::move(map_builder), &tf_buffer,
        FLAGS_collect_metrics);

以下是node.h的理解

// 这个函数包含了所有的slam步骤。部分已经有英文解释了。
// Wires up ROS topics to SLAM.
class Node {
 public:
  //上个步骤传入的参数进入了这里,其中第三个tf2_ros::Buffer*是ros的指针，我们要用类似的结构替换掉他
  Node(const NodeOptions& node_options,
       std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
       tf2_ros::Buffer* tf_buffer, bool collect_metrics);
  ~Node();

  Node(const Node&) = delete;
  Node& operator=(const Node&) = delete;
  //以下省略
  ...
  ...
  ...
};

我们继续往下看

//如果没有.pbstream文件则自己创建一个，有的话直接加载
if (!FLAGS_load_state_filename.empty()) {
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
}
······LoadState原函数
void MapBuilderBridge::LoadState(const std::string& state_filename,
                                 bool load_frozen_state) {
  // Check if suffix of the state file is ".pbstream".
  const std::string suffix = ".pbstream";
  CHECK_EQ(state_filename.substr(
               std::max<int>(state_filename.size() - suffix.size(), 0)),
           suffix)
      << "The file containing the state to be loaded must be a "
         ".pbstream file.";
  LOG(INFO) << "Loading saved state '" << state_filename << "'...";
  cartographer::io::ProtoStreamReader stream(state_filename);//加载函数
  //调用原始库std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
  map_builder_->LoadState(&stream, load_frozen_state);
}

好的到这一步我有个问题需要解答： 1.NodeOptions参数传到哪儿去了？是通过下面这个步骤直接传给cartographer库了吗？还是说node这个options对我根本无用，我不需要加载这个参数。

Node(const NodeOptions& node_options,
       std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
       tf2_ros::Buffer* tf_buffer, bool collect_metrics);
~Node();

问题下面还有，我们继续。

//开始建图
if (FLAGS_start_trajectory_with_default_topics) {
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
······
//检查参数是否正确
bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
  if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
    return options.trajectory_builder_options
        .has_trajectory_builder_2d_options();
  }
  if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
    return options.trajectory_builder_options
        .has_trajectory_builder_3d_options();
  }
  return false;
}
......
//找到这个函数
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
  absl::MutexLock lock(&mutex_);
  CHECK(ValidateTrajectoryOptions(options));//检查参数是否正确
  AddTrajectory(options);//添加轨迹，开始建图
······
//找到这个函数
int Node::AddTrajectory(const TrajectoryOptions& options) {
  //给已知的传感器添加id
  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options);
  //来源：MapBuilderBridge(
  //const NodeOptions& node_options,
  //std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
  //tf2_ros::Buffer* tf_buffer);
  //MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);
  //创建轨迹并生成该轨迹id
  //调用map_builder_bridge_中的AddTrajectory来处理,再次会调用到
  //map_builder_.AddTrajectoryBuilder,会最终调用到cartographer底层算法的
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
  //插入一个位姿解析器，解析当前车的位姿
  AddExtrapolator(trajectory_id, options);
  //添加传感器相关采样参数
  AddSensorSamplers(trajectory_id, options);
  //通过ROS启用相关话题
  LaunchSubscribers(options, trajectory_id);
  //ROS相关，这些定时器告诉cartographer传入数据时间，另外，是否可以用linux标准定时器直接替代它
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kTopicMismatchCheckDelaySec),
      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
  //添加sensor.id
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }
  return trajectory_id;
}

一个个来理解

1. 赋予传感器id
我们可以参考这个把/drv/laser手动给一个id，用的都是标准库。很不错。

//给已知的传感器赋予id
Node::ComputeExpectedSensorIds(const TrajectoryOptions& options) const {
  using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
  using SensorType = SensorId::SensorType;
  std::set<SensorId> expected_topics;
  //如果有多个激光雷达，给他赋予一个id
  // Subscribe to all laser scan, multi echo laser scan, and point cloud topics.
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
    //添加id
    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});
  }
  //GPS
  // 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;
}

2. 开始建图、添加路径

  //下面函数用到的结构体
  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结果，储存方式未知(好像是结构体数组？？)，这个函数不太明白
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_);
  //来源：std::unordered_map<int,std::shared_ptr<const LocalTrajectoryData::LocalSlamData>> local_slam_data_ GUARDED_BY(mutex_);
  local_slam_data_[trajectory_id] = std::move(local_slam_data);
}
............
//重头戏，添加轨迹
//输入 传感器id,轨迹参数
//输出 轨迹id
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 =
  //来源std::unordered_map<int, TrajectoryOptions> trajectory_options_;
      trajectory_options_.emplace(trajectory_id, trajectory_options);
  CHECK(emplace_result.second == true);
  return trajectory_id;
}

解析小车自身的位姿

//位姿解析器，和imu有关。
//参考：For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is required.
void Node::AddExtrapolator(const int trajectory_id,
                           const TrajectoryOptions& options) {
  constexpr double kExtrapolationEstimationTimeSec = 0.001;  // 1 ms
  CHECK(extrapolators_.count(trajectory_id) == 0);
  const double gravity_time_constant =
      node_options_.map_builder_options.use_trajectory_builder_3d()
          ? options.trajectory_builder_options.trajectory_builder_3d_options()
                .imu_gravity_time_constant()
          : options.trajectory_builder_options.trajectory_builder_2d_options()
                .imu_gravity_time_constant();
  extrapolators_.emplace(
      std::piecewise_construct, std::forward_as_tuple(trajectory_id),
      std::forward_as_tuple(
          ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
          gravity_time_constant));
}

加载与传感器有关的参数。采样参数等。

//添加传感器相关采样参数
void Node::AddSensorSamplers(const int trajectory_id,
                             const TrajectoryOptions& options) {
  CHECK(sensor_samplers_.count(trajectory_id) == 0);
  //载入传感器相关参数
  sensor_samplers_.emplace(
      std::piecewise_construct, std::forward_as_tuple(trajectory_id),
      std::forward_as_tuple(
          options.rangefinder_sampling_ratio, options.odometry_sampling_ratio,
          options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio,
          options.landmarks_sampling_ratio));
}

ROS相关，看看传感器数据是怎么载入进去的，我们可以在这里参考做文章

void Node::LaunchSubscribers(const TrajectoryOptions& options,
                             const int trajectory_id) {
  //订阅雷达
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::LaserScan>(
             &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
             this),
         topic});
  }
  //如果有多个雷达
  for (const std::string& topic : ComputeRepeatedTopicNames(
           kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>(
             &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
             &node_handle_, this),
         topic});
  }
  //订阅点云数据
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::PointCloud2>(
             &Node::HandlePointCloud2Message, trajectory_id, topic,
             &node_handle_, this),
         topic});
  }

  //判断是否需要imu
  // 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())) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
                                                trajectory_id, kImuTopic,
                                                &node_handle_, this),
         kImuTopic});
  }

  //订阅里程计
  if (options.use_odometry) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
                                                  trajectory_id, kOdometryTopic,
                                                  &node_handle_, this),
         kOdometryTopic});
  }
  //订阅GPS
  if (options.use_nav_sat) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::NavSatFix>(
             &Node::HandleNavSatFixMessage, trajectory_id, kNavSatFixTopic,
             &node_handle_, this),
         kNavSatFixTopic});
  }
  //订阅标志点
  if (options.use_landmarks) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>(
             &Node::HandleLandmarkMessage, trajectory_id, kLandmarkTopic,
             &node_handle_, this),
         kLandmarkTopic});
  }
}

示例：雷达数据是如何和c++库交互的

//传入雷达数据
void SensorBridge::HandleLaserScanMessage(
    const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
  carto::sensor::PointCloudWithIntensities point_cloud;
  carto::common::Time time;
  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
  //调用carto库函数，把激光雷达的点云传入
  //提示：把雷达驱动改成输出相关的数据直接用这个让carto建图即可
  HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}
············
//来自carto库
void HandleLaserScan(
      const std::string& sensor_id, ::cartographer::common::Time start_time,
      const std::string& frame_id,
      const ::cartographer::sensor::PointCloudWithIntensities& points);

树根也清理的差不多了，当你盯着上位机，按下了停止建图按钮。于是调用了下面函数。

  node.FinishAllTrajectories();
  node.RunFinalOptimization();

  if (!FLAGS_save_state_filename.empty()) {
    node.SerializeState(FLAGS_save_state_filename,
                        true /* include_unfinished_submaps */);
  }

当觉得差不多完成建图了，启用这个函数关闭(完成)所有轨迹的建图

void Node::FinishAllTrajectories() {
  absl::MutexLock lock(&mutex_);
  for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
    if (entry.second == TrajectoryState::ACTIVE) {
     //检查标志位
      const int trajectory_id = entry.first;
      //更改标志位
      CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code,
               cartographer_ros_msgs::StatusCode::OK);
    }
  }
}
.................
bool Node::FinishTrajectory(const int trajectory_id) {
  absl::MutexLock lock(&mutex_);
  return FinishTrajectoryUnderLock(trajectory_id).code ==
         cartographer_ros_msgs::StatusCode::OK;
}

闭环优化，只有运行上一步才能调用这个函数

void Node::RunFinalOptimization() {
  {
    for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
      const int trajectory_id = entry.first;
      if (entry.second == TrajectoryState::ACTIVE) {
        LOG(WARNING)
            << "Can't run final optimization if there are one or more active "
               "trajectories. Trying to finish trajectory with ID "
            << std::to_string(trajectory_id) << " now.";
        CHECK(FinishTrajectory(trajectory_id))
            << "Failed to finish trajectory with ID "
            << std::to_string(trajectory_id) << ".";
      }
    }
  }
  // Assuming we are not adding new data anymore, the final optimization
  // can be performed without holding the mutex.
  map_builder_bridge_.RunFinalOptimization();
}
..................
void MapBuilderBridge::RunFinalOptimization() {
  LOG(INFO) << "Running final trajectory optimization...";
  map_builder_->pose_graph()->RunFinalOptimization();
}

保存地图，直接把这个文件通过TCP发送到导航板

void Node::SerializeState(const std::string& filename,
                          const bool include_unfinished_submaps) {
  absl::MutexLock lock(&mutex_);
  CHECK(
      map_builder_bridge_.SerializeState(filename, include_unfinished_submaps))
      << "Could not write state.";
}

4. 总结

通过以上理解，完成代码编写有几个步骤

按照上述思路，写一个大概的代码，编译通过。
改造雷达驱动，使其输出''carto::sensor::PointCloudWithIntensities point_cloud''兼容的数据。
上面已经详细描述了cartographer_ros工作的详细过程，现在要做的就是把他们的结构体剥离出来，特别是AddTrajectory中体现的结构体到自制的.h文件。我们的目的是直接用/drv/laser把carographer跑起来。
Node类应该有特别多可以精简的地方，需要确定是不是可用。
使用TCP socket和导航板驱动(ROS)联络。

火种源码

关注

11
点赞
踩
13

收藏

觉得还不错? 一键收藏
2
评论
Cartographer Standalone移植（1）—移植思路

任务动机：任务描述：1.输入/输出要求输入：激光雷达/drv/laser 输出：地图 .pgm/.pbstream 撸码工具：Visual Studio Code2. 移植思路只安装cartographer和ceres-solver，以cartographer_ros为示例，编写一个自己的程序。3. 移植过程3.1 开始建图在分析源码的过程中，我把所有的与ROS有关的类和相关库全部过滤。这个代码写的就像拔萝卜，开始拔以后发现...
复制链接

扫一扫