Cartographer实时显示三维点云地图

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

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

这篇文章就带着大家将cartographer三维地图实时显示出来.

这部分的代码已经开源在 https://github.com/xiangli0608/cartographer_detailed_comments_ws

1 需要的数据

既然想要显示三维点云地图, 就必须要有每帧数据的点云, 以及这帧点云经过后端优化后的坐标, 这2个数据.

通过阅读代码可知, cartographer中前端是不保存点云的, 前端也不会有后端优化后的坐标.

所以, 我们想要找到这2个数据就要在后端的代码中找.

1.1 PoseGraphData

struct PoseGraphData {// Submaps get assigned an ID and state as soon as they are seen, even// before they take part in the background computations.// submap_data_ 里面,包含了所有的submap  MapById submap_data;// Global submap poses currently used for displaying data.// submap 在 global 坐标系下的坐标  MapById global_submap_poses_2d;  MapById global_submap_poses_3d;// Data that are currently being shown.// 所有的轨迹节点的id与 节点的在global坐标系下的坐标, 在local map 下的坐标与时间  MapById trajectory_nodes;// Global landmark poses with all observations.std::map      landmark_nodes;// How our various trajectories are related.  TrajectoryConnectivityState trajectory_connectivity_state;// 节点的个数int num_trajectory_nodes = 0;// 轨迹与轨迹的状态std::map trajectories_state;// Set of all initial trajectory poses.std::map initial_trajectory_poses;// 所有的约束数据std::vectorconstraints;};

这是后端代码中保存所有数据的数据结构, 其中有个 MapById trajectory_nodes;

1.2 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_ptrconstant_data;// The node pose in the global SLAM frame.  transform::Rigid3d global_pose;};

可以看到, TrajectoryNode这个数据结构即保存了点云, 又保存了前端与后端的位姿, 其中后端的位姿global_pose正是我们需要的.

1.3 PoseGraph3D::GetTrajectoryNodes()

现在有了数据了, 如果能有接口就更好了. 通过寻找, 发现还真有, 就是PoseGraph3D::GetTrajectoryNodes(), 这个函数直接返回TrajectoryNodes.

所以, 我们可以不用修改cartographer的代码, 只修改cartographer_ros的代码就可以了, 因为cartographer的代码存在接口满足我们的要求.

2 修改代码 2.1 MapBuilderBridge

cartographer_ros中从后端位姿图中拿数据的只有 MapBuilderBridge, 所以在 这个类中要添加获取TrajectoryNodes的接口.

由于cartographer_ros 中进行可视化操作的代码都在Node类中, 我们在MapBuilderBridge类这只需要添加获取TrajectoryNodes的接口就行了, 具体的可视化要在Node类中进行.

2.1.1 map_builder_bridge.h

首先, 在 cartographer_ros/cartographer_ros/map_builder_bridge.h 文件中, 添加public的函数, 如下所示. std::shared_ptr