cartographer二维点云地图

fzd1999

已于 2022-04-07 10:21:11 修改

阅读量2.3k

点赞数 6

分类专栏： cartographer 二维点云地图文章标签： c++

于 2022-04-06 21:11:45 首次发布

本文链接：https://blog.youkuaiyun.com/fatecutter/article/details/123996966

版权

cartographer 同时被 2 个专栏收录

1 篇文章

订阅专栏

二维点云地图

1 篇文章

订阅专栏

文章目录

前言
一、修改思路
- 还是TrajectoryNode
二、计算步骤
- 1.gravity_alignment 坐标系到local坐标系
- 2.local坐标系到global坐标系
三、具体替换代码步骤
4.结果展示
写在最后

前言

cartographer不能实时显示三维点云地图, 这是大家公认的cartographer 3d建图的弊端. ————李太白

本文实现的cartographer二维点云地图的构建是基于对李太白三维点云地图代码的修改。本人小白一枚，由于论文内容需要，急求cartographer的二维点云地图，但是大佬只做了三维点云地图的构建，无奈自己上手改，最终花不少时间才搞出来，希望能帮到同样处境的同学。——感谢李太白学长

原文链接：李太白三维点云地图

一、修改思路

还是TrajectoryNode

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

既然要构建二维点云地图，就需要获取二维点云，原文中，点云是通过sensor::PointCloud high_resolution_point_cloud获取的，就像注释中写的：Used for loop closure in 3D.

二维点云是通过sensor::PointCloud filtered_gravity_aligned_point_cloud获取的，但是注意它的注释：Used for loop closure in 2D: voxel filtered returns in the ‘gravity_alignment’ frame，也就是说二维点云与三维点云不同，三维点云是建立在local坐标系下的，而二维点云是建立在gravity_alignment坐标系下的。

再往上看，发现有：Eigen::Quaterniond gravity_alignment，它的数据类型为：Eigen::Quaterniond，也就是四元数，再加上注释的说明可以确定，这是二维点云由local坐标系转到gravity_alignment坐标系转角的四元数表示。

换句话来说，如果试图将点云像原文中那样直接移动到global坐标系下，结果是糟糕的。所以需要先将点云由gravity_alignment坐标系转到local坐标系，再由local转到global，才能构建出理想的点云地图

二、计算步骤

1.gravity_alignment 坐标系到local坐标系

1) 四元数转欧拉角
四元数gravity_alignment中含有的数值为：x,y,z,w，分别对应下图中的 $q_1,q_2,q_3,q_0$
在这里插入图片描述
$\psi，\theta，\phi$ 分表表示转角Yaw、Pitch、Roll，在二维问题中我们只需要关注转角Roll\

2)点云坐标逆转Roll（由gravity_alignment坐标系转local坐标系）

旋转矩阵： $\begin{bmatrix} cos(-\phi) & -sin(-\phi)\\ sin(-\phi) & cos(-\phi) \end{bmatrix} \quad$

2.local坐标系到global坐标系

与原文相同的操作

        RangefinderPoint range_finder_point = global_pose.cast<float>() * point;
        node_point_cloud->push_back(pcl::PointXYZ(
            range_finder_point.position.x(), 
            range_finder_point.position.y(),
            range_finder_point.position.z()));
      }
————————————————
版权声明：本文为优快云博主「李太白lx」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.youkuaiyun.com/tiancailx/article/details/120280113

三、具体替换代码步骤

1.map_builder_bridge.h

添加public的函数, 如下所示：

std::shared_ptr<MapById<NodeId, TrajectoryNode>> GetTrajectoryNodes();

在cartographer_ros命名空间下添加引用：

using ::cartographer::mapping::NodeId;
using ::cartographer::mapping::MapById;
using ::cartographer::mapping::TrajectoryNode;

头文件：

#include "absl/synchronization/mutex.h"
#include "cartographer/mapping/map_builder_interface.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer_ros/node_options.h"
#include "cartographer_ros/sensor_bridge.h"
#include "cartographer_ros/tf_bridge.h"
#include "cartographer_ros/trajectory_options.h"
#include "cartographer_ros_msgs/SubmapEntry.h"
#include "cartographer_ros_msgs/SubmapList.h"
#include "cartographer_ros_msgs/SubmapQuery.h"
#include "cartographer_ros_msgs/TrajectoryQuery.h"
#include "geometry_msgs/TransformStamped.h"
#include "nav_msgs/OccupancyGrid.h"

2.map_builder_bridge.cc

在 cartographer_ros/cartographer_ros/map_builder_bridge.cc 中的最下面, 添加如下函数

std::shared_ptr<MapById<NodeId, TrajectoryNode>> MapBuilderBridge::GetTrajectoryNodes() {
  std::shared_ptr<MapById<NodeId, TrajectoryNode>> trajectory_nodes =
      std::make_shared<MapById<NodeId, TrajectoryNode>>(
        map_builder_->pose_graph()->GetTrajectoryNodes());
  return trajectory_nodes;
}

具体函数有什么含义，就去看原文吧

3.node.h

在 node.h 中添加如下私有的成员变量

  ::ros::Publisher point_cloud_map_publisher_;
  absl::Mutex point_cloud_map_mutex_;
  bool load_state_ = false;
  size_t last_trajectory_nodes_size_ = 0;
  sensor_msgs::PointCloud2 ros_point_cloud_map_;

添加如下的共有的成员函数

void PublishPointCloudMap(const ::ros::WallTimerEvent& timer_event);

4.node.cc

添加头文件的引入以及变量名的引入

#include "cartographer/mapping/id.h"
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer/sensor/point_cloud.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl_conversions/pcl_conversions.h"
#include <pcl/io/pcd_io.h>

namespace cartographer_ros {

namespace carto = ::cartographer;

using carto::transform::Rigid3d;
using TrajectoryState =
    ::cartographer::mapping::PoseGraphInterface::TrajectoryState;

// lx add
using ::cartographer::mapping::NodeId;
using ::cartographer::mapping::MapById;
using ::cartographer::mapping::TrajectoryNode;
using ::cartographer::sensor::RangefinderPoint;

在构造函数中, 添加如下代码（use_trajectory_builder_3d换成use_trajectory_builder_2d）

if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
    point_cloud_map_publisher_ =
        node_handle_.advertise<sensor_msgs::PointCloud2>(
            "point_cloud_map", kLatestOnlyPublisherQueueSize, true);
  }

在构造函数的最后, 为发布点云函数添加定时器, 设置为1秒种执行一次.(原文是10秒一次，但是我感觉一秒两次视觉上比较爽)

if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
    wall_timers_.push_back(node_handle_.createWallTimer(
        ::ros::WallDuration(0.5),  // 10s
        &Node::PublishPointCloudMap, this));
  }

将之前声明的 PublishPointCloudMap 这个函数进行实现（主要改动区域）

void Node::PublishPointCloudMap(const ::ros::WallTimerEvent& timer_event) {
  // 纯定位时不发布点云地图
  if (load_state_ || point_cloud_map_publisher_.getNumSubscribers() == 0) {
    return;
  }

  // 只发布轨迹id 0 的点云地图
  constexpr int trajectory_id = 0;

  // 获取优化后的节点位姿与节点的点云数据
  std::shared_ptr<MapById<NodeId, TrajectoryNode>> trajectory_nodes =
      map_builder_bridge_.GetTrajectoryNodes();

  // 如果个数没变就不进行地图发布
  size_t trajectory_nodes_size = trajectory_nodes->SizeOfTrajectoryOrZero(trajectory_id);
  if (last_trajectory_nodes_size_ == trajectory_nodes_size)
    return;
  last_trajectory_nodes_size_ = trajectory_nodes_size;

  absl::MutexLock lock(&point_cloud_map_mutex_);
  pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_map(new pcl::PointCloud<pcl::PointXYZ>());
  pcl::PointCloud<pcl::PointXYZ>::Ptr node_point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
  
  // 遍历轨迹0的所有优化后的节点
  auto node_it = trajectory_nodes->BeginOfTrajectory(trajectory_id);
  auto end_it = trajectory_nodes->EndOfTrajectory(trajectory_id);
  float cosphi;
  float sinphi;
  for (; node_it != end_it; ++node_it) {
    auto& trajectory_node = trajectory_nodes->at(node_it->id);
    auto& filtered_gravity_aligned_point_cloud = trajectory_node.constant_data->filtered_gravity_aligned_point_cloud;
    auto& test = trajectory_node.constant_data->gravity_alignment;
    auto& global_pose = trajectory_node.global_pose;
    
    if (trajectory_node.constant_data != nullptr) {
      node_point_cloud->clear();
      node_point_cloud->resize(filtered_gravity_aligned_point_cloud.size());
      // 遍历点云的每一个点, 进行坐标变换
      for (const RangefinderPoint& point :
           filtered_gravity_aligned_point_cloud.points()) {
           //std::cout << test.coeffs().z() << std::endl;
           
           double x = test.coeffs().x();
           double y = test.coeffs().y();
           double z = test.coeffs().z();
           double w = test.coeffs().w();
           cosphi = cos(atan2(2*(z*w + x*y),1 - 2*(y*y + z*z)));
           sinphi = sin(atan2(2*(z*w + x*y),1 - 2*(y*y + z*z)));
           
        //std::cout << cosphi << " " << sinphi << std::endl;
           
           double point_x = point.position.x() * cosphi + point.position.y() * sinphi;// + global_pose.translation().x();//point_x = point.position.x() * cosphi + point_x - point.position.y() * sinphi;
           double point_y = -point.position.x() * sinphi + point.position.y() * cosphi;//+ global_pose.translation().y();//point_x = point.position.x() * sinphi + point_x - point.position.y() * sinphi;
           //std::cout << point_x << " " << point.position.x() << std::endl;
           //point.position.x() = point_x;
           //point.position.y() = point_y;
           RangefinderPoint range_finder_point_pre;
            range_finder_point_pre.position.x() = point_x;
            range_finder_point_pre.position.y() = point_y;
            range_finder_point_pre.position.z() = 0;
           
        RangefinderPoint range_finder_point = global_pose.cast<float>() * range_finder_point_pre;//global_pose.cast<float>() * 
        //std::cout << global_pose.rotation().coeffs().x()<< global_pose.rotation().coeffs().y() << global_pose.rotation().coeffs().z() << global_pose.rotation().coeffs().w()  << std::endl;
        //std::cout << global_pose.translation().x() << std::endl;
        //exit(-1);
        
        
        node_point_cloud->push_back(pcl::PointXYZ(
            range_finder_point.position.x(), range_finder_point.position.y(),
            range_finder_point.position.z()));
   
      }
      // 将每个节点的点云组合在一起
      *point_cloud_map += *node_point_cloud;
    }
  } // end for

  ros_point_cloud_map_.data.clear();
  pcl::toROSMsg(*point_cloud_map, ros_point_cloud_map_);
  ros_point_cloud_map_.header.stamp = ros::Time::now();
  ros_point_cloud_map_.header.frame_id = node_options_.map_frame;
  LOG(INFO) << "publish point cloud map";
  point_cloud_map_publisher_.publish(ros_point_cloud_map_);
}

将HandleWriteState函数改写一下（我直接默认save_pcd为true）

  constexpr bool save_pcd = true;
  if (node_options_.map_builder_options.use_trajectory_builder_3d() &&
      save_pcd) {
    absl::MutexLock lock(&point_cloud_map_mutex_);
    const std::string suffix = ".pbstream";
    std::string prefix =
        request.filename.substr(0, request.filename.size() - suffix.size());
    
    LOG(INFO) << "Saving map to pcd files ...";
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_point_cloud_map(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::fromROSMsg(ros_point_cloud_map_, *pcl_point_cloud_map);
    pcl::io::savePCDFileASCII(prefix + ".pcd", *pcl_point_cloud_map);
    LOG(INFO) << "Pcd written to " << prefix << ".pcd";
  }
}