【PCL】深度图创建与显示

1. 相关说明

• PCL版本 AllInOne
• 数据文件 room_scan1.pcd

2. CMakeLists.txt

# 指定最低的CMAKE版本
CMAKE_MINIMUM_REQUIRED(VERSION 3.22 FATAL_ERROR)

# 创建项目
PROJECT(PCLSamples LANGUAGES CXX)

# 指定CPLUSPLUS标准
SET(CMAKE_CXX_STANDARD_REQUIRED ON)
SET(CMAKE_CXX_STANDARD 14)
SET(CMAKE_C_STANDARD 14)
SET(CMAKE_C_STANDARD_REQUIRED ON)

# 查找第三方依赖库
FIND_PACKAGE(PCL 1.11 REQUIRED)

# 添加可执行文件
ADD_EXECUTABLE(PCLSample src/main.cpp)

# 添加头文件目录
TARGET_INCLUDE_DIRECTORIES(PCLSample PRIVATE ${PCL_INCLUDE_DIRS})

# 添加库目录
TARGET_LINK_DIRECTORIES(PCLSample PRIVATE ${PCL_LIBRARY_DIRS})

# 添加编译器定义
TARGET_COMPILE_DEFINITIONS(PCLSample PRIVATE ${PCL_DEFINITIONS})
# 取消已有的编译器定义
#SET_TARGET_PROPERTIES(PCLSample PROPERTIES COMPILE_FLAGS "/UBOOST_ALL_NO_LIB-DBOOST_ALL_NO_LIB")

# 添加依赖库文件
target_link_libraries(PCLSample PRIVATE ${PCL_LIBRARIES})

3. main.cpp

#include <iostream>                                   // C++标准输入输出流
#include <pcl/io/pcd_io.h>                            // PCD文件读写头文件
#include <pcl/range_image/range_image.h>              // PCL深度图像头文件
#include <pcl/visualization/pcl_visualizer.h>         // PCL可视化头文件
#include <pcl/visualization/range_image_visualizer.h> // PCL深度图像可视化头文件
#include <pcl/console/parse.h>                        // PCL命令行参数解析

/***********************************************************************************************************************
 * 类型别名定义
 **********************************************************************************************************************/
typedef pcl::PointXYZ PointType; // 点云数据类型

/***********************************************************************************************************************
 * 全局参数
 **********************************************************************************************************************/
// 定义X方向相邻的像素点所对应的光束之间的角度差(单位：度)
float angular_resolution_x = 0.5f; // X方向角度分辨率

// 定义Y方向相邻的像素点所对应的光束之间的角度差(单位：度)
float angular_resolution_y = 0.5f; // Y方向角度分辨率

// 用于说明深度图构建系统的坐标系
// CAMERA_FRAME：X轴向右，Y轴向下，Z轴向前
// LASER_FRAME ：X轴向前，Y轴向左，Z轴向上
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;

// 用于指定是否根据点云图的当前视角来更新深度图
bool live_update = false;

/***********************************************************************************************************************
 * 全局函数
 **********************************************************************************************************************/
void printHelper(const char *progName)
{
    std::cout << "\n\nUsage: " << progName << " [options] <scene.pcd>\n\n"
              << "Options:\n"
              << "-------------------------------------------\n"
              << "-rx <float>  angular resolution in degrees (default " << angular_resolution_x << ")\n"
              << "-ry <float>  angular resolution in degrees (default " << angular_resolution_y << ")\n"
              << "-c <int>     coordinate frame (default " << (int)coordinate_frame << ")\n"
              << "-l           live update - update the range image according to the selected view in the 3D viewer.\n"
              << "-h           this help\n"
              << "\n\n";
}

void setViewerPose(pcl::visualization::PCLVisualizer &viewer, const Eigen::Affine3f &viewer_pose)
{
    Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0);
    Eigen::Vector3f look_at_vector = viewer_pose.rotation() * Eigen::Vector3f(0, 0, 1) + pos_vector;
    Eigen::Vector3f up_vector = viewer_pose.rotation() * Eigen::Vector3f(0, -1, 0);
    viewer.setCameraPosition(pos_vector[0], pos_vector[1], pos_vector[2],
                             look_at_vector[0], look_at_vector[1], look_at_vector[2],
                             up_vector[0], up_vector[1], up_vector[2]);
}

/***********************************************************************************************************************
 * 主函数调用
 **********************************************************************************************************************/
int main(int argc, char **argv)
{
    /*******************************************************************************************************************
     * 参数解析
     ******************************************************************************************************************/
    // 查看命令行参数中是否存在 -h 选项，如果存在打印使用帮助信息
    if (pcl::console::find_argument(argc, argv, "-h") >= 0)
    {
        printHelper(argv[0]);
        return 0;
    }

    // 查看命令行参数中是否存在 -l 选项，如果存在开启根据点云视图实时跟新深度图
    if (pcl::console::find_argument(argc, argv, "-l") >= 0)
    {
        live_update = true;
        std::cout << "Live update is on." << std::endl;
    }

    // 查找并解析 -rx 选项，如果存在将对应的值解析为X方向的角度分辨率
    if (pcl::console::parse(argc, argv, "-rx", angular_resolution_x) >= 0)
        std::cout << "Setting angular resolution in x-direction to " << angular_resolution_x << "deg." << std::endl;

    // 查找并解析 -ry 选项，如果存在将对应的值解析为Y方向的角度分辨率
    if (pcl::console::parse(argc, argv, "-ry", angular_resolution_y) >= 0)
        std::cout << "Setting angular resolution in y-direction to " << angular_resolution_y << "deg." << std::endl;

    // 查找并解析 -c 选项，如果存在将对应的值解析为构建深度图的坐标系统
    int tmp_coordinate_frame;
    if (pcl::console::parse(argc, argv, "-c", tmp_coordinate_frame) >= 0)
    {
        coordinate_frame = pcl::RangeImage::CoordinateFrame(tmp_coordinate_frame);
        std::cout << "Using coordinate frame " << (int)coordinate_frame << "." << std::endl;
    }

    // 角度分辨率角度转弧度
    angular_resolution_x = pcl::deg2rad(angular_resolution_x);
    angular_resolution_y = pcl::deg2rad(angular_resolution_y);

    /*******************************************************************************************************************
     * 读取或创建点云文件
     ******************************************************************************************************************/
    // 定义点云智能指针对象
    pcl::PointCloud<PointType>::Ptr point_cloud_ptr(new pcl::PointCloud<PointType>);

    // 从点云智能指针对象中获取点云对象
    pcl::PointCloud<PointType> &point_cloud = *point_cloud_ptr;

    // 初始化构建深度图的相机姿态
    Eigen::Affine3f scene_sensor_pose(Eigen::Affine3f::Identity());

    // 查看并解析命令行参数中是否包含扩展名为 pcd 的参数，如果右获取其在命令行参数中的索引
    std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument(argc, argv, "pcd");

    // 判断命令行参数中是否包含扩展名为 pcd 的参数。如果包含，则加载此点云文件；否则创建一个点云
    if (!pcd_filename_indices.empty())
    {
        // 从命令行参数中获取点云文件的路径名
        std::string filename = argv[pcd_filename_indices[0]];

        // 加载点云文件
        if (pcl::io::loadPCDFile(filename, point_cloud) == -1)
        {
            std::cout << "Was not able to open file \"" << filename << "\".\n";
            printHelper(argv[0]);
            return 0;
        }

        // 根据点云文件更新构建深度图的相机姿态
        Eigen::Translation3f translation(point_cloud.sensor_origin_[0], point_cloud.sensor_origin_[1], point_cloud.sensor_origin_[2]);
        scene_sensor_pose = translation * Eigen::Affine3f(point_cloud.sensor_orientation_);
    }
    else
    {
        // 如果命令行参数中未包含以pcd为扩展名的参数，构建 z = 2.0 - y，定义域为X:[-0.5, 0.5],Y:[-0.5, 0.5]的平面点云
        std::cout << "\nNo *.pcd file given => Generating example point cloud.\n\n";
        for (float x = -0.5f; x <= 0.5f; x += 0.01f)
        {
            for (float y = -0.5f; y <= 0.5f; y += 0.01f)
            {
                PointType point;
                point.x = x;
                point.y = y;
                point.z = 2.0f - y;
                point_cloud.push_back(point);
            }
        }
        point_cloud.width = point_cloud.size();
        point_cloud.height = 1;
    }

    /*******************************************************************************************************************
     * 通过点云构建深度图
     ******************************************************************************************************************/
    // noise_level = 0 是指只用一个归一化的Z缓冲器来创建深度图，但是如果想让临近点集都落在同一个像素单元，可以设置一个较高的指值。
    // 例如noise_level = 0.05，可以理解为，深度距离值是通过查询点半径为5cm的圆内包含的点以平均计算而得到的。
    float noise_level = 0.0;

    // 如果min_range > 0，则所有模拟器所在位置半径为min_range范围内的临近点都将被忽略，即盲区。
    float min_range = 0.0f;

    // 在裁剪图像时，如果border_size > 0，将在图像周围留下当前视点不可见的边界点
    int border_size = 1;

    // 定义深度图智能指针对象
    pcl::RangeImage::Ptr range_image_ptr(new pcl::RangeImage);

    // 从深度图智能指针对象中获取深度图对象
    pcl::RangeImage &range_image = *range_image_ptr;

    // 构建深度图
    range_image.createFromPointCloud(point_cloud,
                                     angular_resolution_x,
                                     angular_resolution_y,
                                     pcl::deg2rad(360.0f),
                                     pcl::deg2rad(180.0f),
                                     scene_sensor_pose,
                                     coordinate_frame,
                                     noise_level,
                                     min_range,
                                     border_size);

    /*******************************************************************************************************************
     * 显示点云
     ******************************************************************************************************************/
    pcl::visualization::PCLVisualizer viewer("3D Viewer");

    // 设置背景
    viewer.setBackgroundColor(1, 1, 1);

    // 创建自定义颜色处理对象
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler(range_image_ptr, 0, 0, 0);

    // 添加点云到显示窗口
    viewer.addPointCloud(range_image_ptr, range_image_color_handler, "range image");

    // 设置点云显示的属性
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
    // viewer.addCoordinateSystem (1.0f, "global");
    // PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
    // viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");

    // 初始化相机
    viewer.initCameraParameters();

    // 设置查看姿态
    setViewerPose(viewer, range_image.getTransformationToWorldSystem());

    /*******************************************************************************************************************
     * 显示深度图
     ******************************************************************************************************************/
    pcl::visualization::RangeImageVisualizer range_image_widget("Range image");
    range_image_widget.showRangeImage(range_image);

    /*******************************************************************************************************************
     * 显示循环
     ******************************************************************************************************************/
    while (!viewer.wasStopped())
    {
        range_image_widget.spinOnce();
        viewer.spinOnce();
        pcl_sleep(0.01);

        if (live_update)
        {
            scene_sensor_pose = viewer.getViewerPose();
            range_image.createFromPointCloud(point_cloud,
                                             angular_resolution_x,
                                             angular_resolution_y,
                                             pcl::deg2rad(360.0f),
                                             pcl::deg2rad(180.0f),
                                             scene_sensor_pose,
                                             pcl::RangeImage::CAMERA_FRAME,
                                             noise_level,
                                             min_range,
                                             border_size);
            range_image_widget.showRangeImage(range_image);
        }
    }
}

4. 效果图

4.1 点云图

4.2 深度图