由RGB图像和深度图像获取点云

#include <iostream>
#include <fstream>
#include <string>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/common/transforms.h>                 
#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/io/vtk_io.h>
#include <vtkPolyData.h>
#include <vtkSmartPointer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <opencv2/highgui.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>

using namespace pcl::io;
using namespace pcl::console;
using namespace std;
using namespace cv;
typedef unsigned char uchar;
typedef unsigned short ushort;

// 定义点云类型
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;

// 主函数
int main( int argc, char** argv )
{
    //深度相机参数
    const double camera_factor = 1000;
    const double camera_cx = 319.500000;
    const double camera_cy = 239.500000;
    const double camera_fx = 571.623718;
    const double camera_fy = 571.623718;

    vector<int> indexs;
    Eigen::Matrix4f axisAlignment = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f cIntrinsic = Eigen::Matrix4f::Identity();       // color 内参
    Eigen::Matrix4f dIntrinsic = Eigen::Matrix4f::Identity();       // depth 内参
    Eigen::Matrix4f colorTodepth = Eigen::Matrix4f::Identity();

    float AxisAlignment[4][4] = {{0.945519, 0.325568, 0.000000, -5.384390},
                                {-0.325568, 0.945519, 0.000000, -2.871780},
                                {0.000000, 0.000000, 1.000000, -0.064350},
                                {0.000000, 0.000000, 0.000000, 1.000000}};
    float Pose[4][4] = {{-0.955421, 0.119616, -0.269932, 2.65583},
                        {0.295248, 0.388339, -0.872939, 2.9816},
                        {0.000407581, -0.91372, -0.406343, 1.36865},
                        {0, 0, 0, 1}};
    float CIntrinsic[4][4] = {{1169.62, 0, 646.295, 0},
                            {0, 1167.11, 489.927, 0},
                            {0, 0, 1, 0},
                            {0, 0, 0, 1}};
    float DIntrinsic[4][4] = {{577.591, 0, 318.905, 0},
                              {0, 578.73, 242.684, 0},
                              {0, 0, 1, 0},
                              {0, 0, 0, 1}};
    float ColorTodepth[4][4] = {{0.999973, 0.006791, 0.002776, -0.037886},
                                {-0.006767, 0.999942, -0.008366, -0.003410},
                                {-0.002833, 0.008347, 0.999961, -0.021924},
                                {-0.000000, 0.000000, -0.000000, 1.000000}};
    for (int i=0; i<4; i++)
        for (int j=0; j<4 ;j++)
        {
            axisAlignment(i, j) = AxisAlignment[i][j];
            pose(i, j) = Pose[i][j];
            cIntrinsic(i, j) = CIntrinsic[i][j];        //颜色内参矩阵
            dIntrinsic(i, j) = DIntrinsic[i][j];        //深度内参矩阵
            colorTodepth(i, j) = ColorTodepth[i][j];
        }

    Mat mDepth= imread("scene0000_00/depth/000000.png",-1);
    Mat mColor = imread("scene0000_00/color/000000.jpg");

    PointCloud::Ptr depth_cloud(new PointCloud);        //恢复出的点云
    PointCloud::Ptr depthTocolor_cloud(new PointCloud);    //恢复出的点云投影到RGB图像的平面上
    // 遍历深度图
    for (int m = 0; m < mDepth.rows; m++)
    {
        for (int n = 0; n < mDepth.cols; n++)
        {
            // 获取深度图中(m,n)处的值
            ushort d = mDepth.ptr<ushort>(m)[n];
            // d 没有值则跳过此点
            if (d == 0)
            {
                continue;
            }
            PointT p;
            // 计算这个点的空间坐标
            p.z = double(d) / camera_factor;
            p.x = (n - camera_cx) * p.z / camera_fx;
            p.y = (m - camera_cy) * p.z / camera_fy;
            p.r = rand() % 255;
            p.g = rand() % 255;
            p.b = rand() % 255;
            depth_cloud->points.push_back(p);
        }
    }
    //将深度图恢复出的点云转换到RGB图像所在的空间坐标系中
    pcl::transformPointCloud(*depth_cloud, *depth_cloud, colorTodepth.inverse());
    //将点云投影到RGB图像所在平面      
    pcl::transformPointCloud(*depth_cloud, *depthTocolor_cloud, cIntrinsic);
    float z;
    int x, y;
    Vec3b temp_color;
    for(int i=0; i<depthTocolor_cloud->points.size(); i++)
    {
        z = depthTocolor_cloud->points[i].z;
        x = int(depthTocolor_cloud->points[i].x / z);    //除以系数z以获得平面点
        y = int(depthTocolor_cloud->points[i].y / z);
        temp_color = mColor.at<Vec3b>(y, x);
        //给深度图像恢复出的点云赋上RGB图像上的颜色
        depth_cloud->points[i].r = temp_color[2];        
        depth_cloud->points[i].g = temp_color[1];
        depth_cloud->points[i].b = temp_color[0];
        //给投影后的点云赋颜色
        depthTocolor_cloud->points[i].r = temp_color[2];
        depthTocolor_cloud->points[i].g = temp_color[1];
        depthTocolor_cloud->points[i].b = temp_color[0];

    }

    //显示点云
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("pointcloud"));

    viewer->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerRGBField<PointT> depth_color(depth_cloud);

    viewer->addPointCloud<PointT>(depth_cloud, depth_color, "depth_cloud");
    //viewer->addPointCloud<pcl::PointXYZRGBA>(result_cloud, aim_transformed_color, "aim_color");
    viewer->addCoordinateSystem();                  //添加坐标系

    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
    return 0;
}