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<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 add
void 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 add
if (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 add
if (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 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_);
}
通过 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 add
constexpr 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;
}
这里, 如果添加了这里要在 CMakeLists.txt 中将 PCL 引入的库加一个io, 才能编译通过.
find_package(PCL REQUIRED COMPONENTS common io)
3 三维点云地图显示
3.1 运行
通过如下指令运行代码, 注意我这里是跑的bag.
roslaunch cartographer_ros lx_rs16_3d.launch
3.2 三维点云地图可视化
在rviz中订阅 /point_cloud_map 话题的 pointcloud2 格式的数据, 即可看到点云地图.
建图过程中
建图结束
可以看到, cartographer三维建的图, 墙是挺厚的, 效果应该没有lio-sam好.
到此为止, 就实现了cartographer三维点云地图的可视化, 以及保存pcd格式地图的操作.
具体的代码都开源在文章开头处的地址中, 感兴趣的同学可以好好看看.