SLAM 将PCL点云地图转换成俯视图(鸟瞰图BEV)

这个程序可以把3D的点云地图转换成2D的俯视图(或鸟瞰图,Bird-Eye View,BEV)。

先看一下输入的3D点云地图的俯视视角：

能看出来的信息非常少，因为所有点云都是同一个颜色。

接下来查看一下这个俯视视角里的深度图：

在这个深度图里，距离激光雷达越远，颜色越接近蓝色。

因为采集数据的激光雷达是在地面上，俯视角度自然只能看到距离最远的点（偏蓝）。

深度图里能看出来的信息就要比普通模式多一些。

现在看一下程序运行之后生成的2D俯视图：

现在就能看出来这个院子里停了很多小汽车，好像是个停车场。

输入：map_example.pcd        3D点云地图

输出：bev.png        2D俯视图

运行：直接双击会自动加载map_example.pcd。如果需要输入其他点云，需要在cmd里运行。

示例：program.exe –pcd_path path_to_your_pcd

视频演示：https://www.bilibili.com/video/BV1QyLj63EQ9/

代码：

#include <gflags/gflags.h>
#include <glog/logging.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

using PointType = pcl::PointXYZI;
using PointCloudType = pcl::PointCloud<PointType>;

DEFINE_string(pcd_path, "./data/ch5/map_example.pcd", "点云文件路径");
DEFINE_double(image_resolution, 0.1, "俯视图分辨率");
DEFINE_double(min_z, 0.2, "俯视图最低高度");
DEFINE_double(max_z, 2.5, "俯视图最高高度");

/// 本节演示如何将一个点云转换为俯视图像
void GenerateBEVImage(PointCloudType::Ptr cloud) {
// 计算点云边界
auto minmax_x = std::minmax_element(cloud->points.begin(), cloud->points.end(),
[](const PointType& p1, const PointType& p2) { return p1.x < p2.x; });
auto minmax_y = std::minmax_element(cloud->points.begin(), cloud->points.end(),
[](const PointType& p1, const PointType& p2) { return p1.y < p2.y; });
double min_x = minmax_x.first->x;
double max_x = minmax_x.second->x;
double min_y = minmax_y.first->y;
double max_y = minmax_y.second->y;

const double inv_r = 1.0 / FLAGS_image_resolution;

const int image_rows = int((max_y – min_y) * inv_r);
const int image_cols = int((max_x – min_x) * inv_r);

float x_center = 0.5 * (max_x + min_x);
float y_center = 0.5 * (max_y + min_y);
float x_center_image = image_cols / 2;
float y_center_image = image_rows / 2;

// 生成图像
cv::Mat image(image_rows, image_cols, CV_8UC3, cv::Scalar(255, 255, 255));

for (const auto& pt : cloud->points) {
int x = int((pt.x – x_center) * inv_r + x_center_image);
int y = int((pt.y – y_center) * inv_r + y_center_image);
if (x < 0 || x >= image_cols || y < 0 || y >= image_rows || pt.z < FLAGS_min_z || pt.z > FLAGS_max_z) {
continue;
}

image.at<cv::Vec3b>(y, x) = cv::Vec3b(227, 143, 79);
}

cv::imwrite("./bev.png", image);
}

int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
FLAGS_stderrthreshold = google::INFO;
FLAGS_colorlogtostderr = true;
google::ParseCommandLineFlags(&argc, &argv, true);

if (FLAGS_pcd_path.empty()) {
LOG(ERROR) << "pcd path is empty";
return -1;
}

// 读取点云
PointCloudType::Ptr cloud(new PointCloudType);
pcl::io::loadPCDFile(FLAGS_pcd_path, *cloud);

if (cloud->empty()) {
LOG(ERROR) << "cannot load cloud file";
return -1;
}

LOG(INFO) << "cloud points: " << cloud->size();
GenerateBEVImage(cloud);

return 0;
}

参考：高翔《自动驾驶与机器人中的SLAM技术》P143附近