将彩色和深度图转成点云图

将彩色和深度图转成点云图

1.方法1

//
// Created by ty on 20-9-30.
//

#ifndef VISIONGRAB_CLOUDMAKER_H
#define VISIONGRAB_CLOUDMAKER_H

#include 
#include 
#include 

using namespace std;
using namespace cv;

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud PointCloud;

/**
 * 相机内参
 *
 * 合成点云（彩色图，深度图）
 */
class CloudMaker {

public:
    Mat cameraMatrix;
    explicit CloudMaker(Mat& cameraMatrix) {
        this->cameraMatrix = move(cameraMatrix);
    }

    /**
     * 将彩色和深度图转成点云图
     * @param color
     * @param depth
     * @param cloud
     */
    void convert(Mat &color, Mat &depth, PointCloud::Ptr &cloud){
        double fx = cameraMatrix.at(0, 0);
        double fy = cameraMatrix.at(1, 1);
        double cx = cameraMatrix.at(0, 2);
        double cy = cameraMatrix.a