【RealSense】深度图和RGB图生成点云

rgb_depth_2_pointcloud.cpp

#include <iostream>
#include <pcl/console/print.h>
#include <pcl/filters/voxel_grid.h>
#include <opencv2/opencv.hpp>
#include <pcl/common/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/range_image/range_image.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/common/float_image_utils.h> // 保存深度图
#include <pcl/io/png_io.h>                              // 保存深度图
#include <pcl/filters/passthrough.h>
#include <pcl/console/time.h> // TicToc
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>

using namespace std;
using namespace cv;

// 宏定义，简化定义
typedef pcl::PointXYZRGB PointT;
typedef pcl::visualization::PCLVisualizer::Ptr pViewer;

// 此函数采用4x4矩阵的引用，并以人类可读的方式打印刚性变换
void print4x4Matrix(const Eigen::Matrix4d &matrix)
{
    printf("Rotation matrix :\n");
    printf("    | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
    printf("R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
    printf("    | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
    printf("Translation vector :\n");
    printf("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix(0, 3), matrix(1, 3), matrix(2, 3));
}

/**
 * 方法1：通过双重循环遍历
 * @brief 将彩色图和深度图合并成点云
 * @param matrix        相机内参矩阵 3✖3
 * @param rgb           彩色图
 * @param depth         深度图
 * @param cloud         输出点云
 */
static void convert(Mat &matrix, Mat &rgb, Mat &depth, pcl::PointCloud<PointT>::Ptr &cloud)
{
    double camera_fx = matrix.at<double>(0, 0);
    double camera_fy = matrix.at<double>(1, 1);
    double camera_cx = matrix.at<double>(0, 2);
    double camera_cy = matrix.at<double>(1, 2);

    cout << "fx: " << camera_fx << endl;
    cout << "fy: " << camera_fy << endl;
    cout << "cx: " << camera_cx << endl;
    cout << "cy: " << camera_cy << endl;

    // 遍历深度图
    for (int v = 0; v < depth.rows; v++)
    {
        for (int u = 0; u < depth.cols; u++)
        {
            // 获取深度图中(m, n)处的值
            ushort d = depth.ptr<ushort>(v)[u];
            // d 可能没有值，若如此，跳过此点， isnan用来判断一个值是否是nan
            if (isnan(d) && abs(d) < 0.0001)
                continue;
            // 若 d 存在值，则向点云增加一个点
            PointT p;

            // 计算这个点的空间坐标
            p.z = double(d) / 1000; // 单位是 m
            p.x = (u - camera_cx) * p.z / camera_fx;
            p.y = (v - camera_cy) * p.z / camera_fy;

            // 从rgb图像中获取它的颜色
            // rgb是三通道的RGB格式图，所以按像下面的顺序获取颜色
            // 这边因为是使用opencv读取图片的，所以三通道顺序应该是bgr
            Vec3b bgr = rgb.at<Vec3b>(v, u); // 此处：表示行，u表示列
            p.b = bgr[0];
            p.g = bgr[1];
            p.r = bgr[2];

            // 把p加入到点云中
            cloud->points.emplace_back(p);
        }
    }

    // 设置并保存点云
    cloud->height = 1;
    cloud->width = cloud->points.size();
    cout << "point cloud size = " << cloud->points.size() << endl;
    cloud->is_dense = false;

    // // 添加下采样
    // pcl::console::print_highlight("Downsampling...\n");
    // pcl::VoxelGrid<PointT> grid;
    // const float leaf = 0.1f;
    // grid.setLeafSize(leaf, leaf, leaf); // 设置下采样率
    // grid.setInputCloud(cloud);          // 输入对象点云
    // grid.filter(*cloud);                // 对象下采样
    // cout << "PointCloud after filtering: " << cloud->width * cloud->height << " data point ("
    //      << pcl::getFieldsList(*cloud) << ")." << endl;

    // 增加直通滤波
    // pcl::console::print_highlight("passThrough");
    // pcl::PassThrough<PointT> pass;
    // pass.setInputCloud(cloud);    // 1. 设置输入源
    // pass.setFilterFieldName("z"); // 2. 设置过滤域名
    // pass.setFilterLimits(0, 0.5); // 3. 设置过滤范围
    // pass.filter(*cloud);          // 4. 执行过滤
    // cout << "PointCloud after passThrough: " << cloud->width * cloud->height << " data point ("
    //      << pcl::getFieldsList(*cloud) << ")." << endl;
}

int main(int argc, char *argv[])
{

    cout << "opencv version = " << CV_VERSION << endl;
    cout << "pcl version = " << PCL_VERSION << endl;

    cv::Mat cameraMatrix = (Mat_<double>(3, 3) << 619.058195277193, 0, 319.512748241151, 0, 620.152772683851, 224.326042533956, 0, 0, 1); // realsense rgb相机的内参

    Mat rgb1 = imread("/home/bck18vm/桌面/dataset/1.10/rgb_31.png", IMREAD_UNCHANGED);
    Mat depth1 = imread("/home/bck18vm/桌面/dataset/1.10/depth_31.png", IMREAD_UNCHANGED);
    Mat rgb2 = imread("/home/bck18vm/桌面/dataset/1.10/rgb_32.png", IMREAD_UNCHANGED);
    Mat depth2 = imread("/home/bck18vm/桌面/dataset/1.10/depth_32.png", IMREAD_UNCHANGED);

    pcl::PointCloud<PointT>::Ptr pCloud1(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr pCloud2(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr pCloudicp(new pcl::PointCloud<PointT>);

    convert(cameraMatrix, rgb1, depth1, pCloud1);
    convert(cameraMatrix, rgb2, depth2, pCloud2);

    // 点云可视化
    pViewer viewer(new pcl::visualization::PCLVisualizer("viewer"));
    viewer->setBackgroundColor(255, 255, 255); // 显示白色
    // 添加点云
    viewer->addPointCloud<PointT>(pCloud1, "cloud1");
    viewer->addPointCloud<PointT>(pCloud2, "cloud2");

    // 保存点云
    pcl::io::savePCDFile("../1.pcd", *pCloud1);
    pcl::io::savePCDFile("../2.pcd", *pCloud2);

    // 添加坐标系
    viewer->addCoordinateSystem(1);
    while (!viewer->wasStopped())
    {
        viewer->spinOnce();
    }

    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(2pointCloud)

# set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_BUILD_TYPE "Debug")
add_definitions("-DENABLE_SSE")
set(CMAKE_CXX_FLAGS "-std=c++14 -O2 ${SSE_FLAGS} -msse4")

find_package(OpenCV 4 REQUIRED)

include_directories(
        ${OpenCV_INCLUDE_DIRS}
        "/usr/include/eigen3/"
)
 
# 带有点云相关的
# PCL
find_package(PCL REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
# 从rgb 和 的depth 生成点云
add_executable(rgb_depth_2_pointcloud rgb_depth_2_pointcloud.cpp)
target_link_libraries(rgb_depth_2_pointcloud ${PCL_LIBRARIES} ${OpenCV_LIBS})