cartographer实时显示三维点云地图

本文主要是介绍cartographer实时显示三维点云地图，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

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_ 里面,包含了所有的submapMapById<SubmapId, InternalSubmapData> submap_data;// Global submap poses currently used for displaying data.// submap 在 global 坐标系下的坐标MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_2d;MapById<SubmapId, optimization::SubmapSpec3D> global_submap_poses_3d;// Data that are currently being shown.// 所有的轨迹节点的id与 节点的在global坐标系下的坐标, 在local map 下的坐标与时间MapById<NodeId, TrajectoryNode> trajectory_nodes;// Global landmark poses with all observations.std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>landmark_nodes;// How our various trajectories are related.TrajectoryConnectivityState trajectory_connectivity_state;// 节点的个数int num_trajectory_nodes = 0;// 轨迹与轨迹的状态std::map<int, InternalTrajectoryState> trajectories_state;// Set of all initial trajectory poses.std::map<int, PoseGraph::InitialTrajectoryPose> initial_trajectory_poses;// 所有的约束数据std::vector<PoseGraphInterface::Constraint> constraints;
};

这是后端代码中保存所有数据的数据结构, 其中有个 MapById<NodeId, TrajectoryNode> 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_ptr<const Data> constant_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<MapById<NodeId, TrajectoryNode>> GetTrajectoryNodes();

之后需要在 cartographer_ros 的命名空间下使用using引用如下变量. 这里还要添加一下头文件, 具体添加啥我不记得了, 如果感兴趣的就去看我的代码吧.

namespace cartographer_ros {// lx add
using ::cartographer::mapping::NodeId;
using ::cartographer::mapping::MapById;
using ::cartographer::mapping::TrajectoryNode;

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

这段函数的作用就是, 生成一个指向 MapById<NodeId, TrajectoryNode> 的共享智能指针, 然后将这个智能指针返回.

2.2 Node

2.2.1 node.h

我们通过MapBuilderBridge生成了一个获取TrajectoryNode数据的接口, 所以在Node类中, 使用这个接口进行三维点云地图的可视化就可以了.

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

  // lx add::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_;

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

  // lx addvoid PublishPointCloudMap(const ::ros::WallTimerEvent& timer_event);

2.2.2 node.cc

首先, 添加头文件的引入以及变量名的引入.

变量名的引入是将 using ::cartographer::mapping::NodeId; 等放在了 cartographer_ros 命名空间之下.

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

之后, 在构造函数中, 添加如下代码. 当使用3d建图时, 初始化点云发布器.

  // lx addif (node_options_.map_builder_options.use_trajectory_builder_3d()) {point_cloud_map_publisher_ =node_handle_.advertise<sensor_msgs::PointCloud2>("point_cloud_map", kLatestOnlyPublisherQueueSize, true);}

在构造函数的最后, 为发布点云函数添加定时器, 设置为10秒种执行一次.

  // lx addif (node_options_.map_builder_options.use_trajectory_builder_3d()) {wall_timers_.push_back(node_handle_.createWallTimer(::ros::WallDuration(10),  // 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);for (; node_it != end_it; ++node_it) {auto& trajectory_node = trajectory_nodes->at(node_it->id);auto& high_resolution_point_cloud = trajectory_node.constant_data->high_resolution_point_cloud;auto& global_pose = trajectory_node.global_pose;if (trajectory_node.constant_data != nullptr) {node_point_cloud->clear();node_point_cloud->resize(high_resolution_point_cloud.size());// 遍历点云的每一个点, 进行坐标变换for (const RangefinderPoint& point : high_resolution_point_cloud.points()) {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()));}// 将每个节点的点云组合在一起*point_cloud_map += *node_point_cloud;}} // end forros_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_);
}

通过 std::shared_ptr<MapById<NodeId, TrajectoryNode>> trajectory_nodes = map_builder_bridge_.GetTrajectoryNodes(); 这条语句调用了之前我们在 map_builder_bridge 设置的接口, 获取了TrajectoryNode数据, 也就是点云与优化后的坐标.

之后, 将点云先进行坐标变换, 再添加到pcl中的点云里.

这里要说明一下, 为什么要进行坐标变换.
这是因为TrajectoryNode里存的点云, 都是以local坐标系为原点的点云, 是围绕着local坐标系的, 所以, 要根据优化出来的位姿, 将这帧点云变换到优化之后的位姿上去, 变换了之后才能进行拼接.

同时, 使用的点云是high_resolution_point_cloud, 没有使用低分辨率的.

最后, 通过ros的发布器发布出去, 就完成了三维点云地图的可视化.

还剩最后一个问题, 如何将点云地图直接保存成pcd格式的数据.

将HandleWriteState函数改写一下, 每次调用服务进行pbstream保存的时候, 同时也保存一下pcd格式的数据.

默认是不进行保存的, 需要将 save_pcd 这个变量改成 true 重新编译之后才能保存.

bool Node::HandleWriteState(::cartographer_ros_msgs::WriteState::Request& request,::cartographer_ros_msgs::WriteState::Response& response) {{absl::MutexLock lock(&mutex_);// 直接调用cartographer的map_builder_的SerializeStateToFile()函数进行文件的保存if (map_builder_bridge_.SerializeState(request.filename,request.include_unfinished_submaps)) {response.status.code = cartographer_ros_msgs::StatusCode::OK;response.status.message =absl::StrCat("State written to '", request.filename, "'.");} else {response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;response.status.message =absl::StrCat("Failed to write '", request.filename, "'.");}}// lx addconstexpr bool save_pcd = false;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";}return true;
}