SLAM 将点云地图转换成俯视图(鸟瞰图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::PointCloudPointType; 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.atcv::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附近
