pcl的使用

1. pcl 初使用

#pragma warning(disable:4996)
#include <iostream>
#include <opencv2/highgui.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>

using namespace std;
using namespace cv;

int main() {

	//导入深度图
	Mat depth = imread("D:\\学习资料\\C++\\C++ code\\SGBM\\SGBM\\output_depth.png", 0);
	
	//导入rgb图像
	Mat rgb = imread("D:\\学习资料\\C++\\C++ code\\middleburry_dataset\\MiddEval3-data-Q\\MiddEval3\\trainingQ\\Adirondack\\im0.png", -1);

	//相机内参
	const double camera_cx = 322.037;
	const double camera_cy = 243.393;
	const double camera_fx = 1038.018;
	const double camera_fy = 1038.018;

	//创建点云指针
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	//遍历深度图
	for (int i = 0; i < depth.rows; i++) {
		for (int j = 0; j < depth.cols; j++) {
			uchar d = depth.ptr<uchar>(i)[j];
			//如果d为0，则跳过
			if (d == 0) {
				continue;
			}
			//如果d不为0，则增加一个点
			pcl::PointXYZRGB p;
			//计算该点的空间坐标
			p.z = -double(d) / 4.;
			p.x = -(i - camera_cx) * p.z / camera_fx;
			p.y = -(j - camera_cy) * p.z / camera_fy;
			//获取该点的rgb颜色值
			p.b = rgb.ptr<uchar>(i)[j * 3];
			p.g = rgb.ptr<uchar>(i)[j * 3 + 1];
			p.r = rgb.ptr<uchar>(i)[j * 3 + 2];
			//把p加入点云中
			cloud->points.push_back(p);
		}
	}

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

	//显示点云
	pcl::visualization::CloudViewer viewer("cloud viewer");
	viewer.showCloud(cloud);

	//保存点云
	cloud->is_dense = false;  //表示点云有无效值
	pcl::io::savePCDFile("D:\\学习资料\\C++\\C++ code\\usepcl\\usepcl\\pointcloud.pcd", *cloud);

	//清空数据
	cloud->points.clear();
	cout << "Point cloud saved." << endl;

	system("pause");
	return 0;
}

2. SGBM + pcl

#pragma warning(disable:4996)
#include <iostream>
#include <opencv2/highgui.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/opencv.hpp>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>

using namespace std;
using namespace cv;

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

void pointcloud(Mat& depth) {

    // 相机内参
    //const double camera_factor = 1000;
    const double camera_cx = 322.037;
    const double camera_cy = 243.393;
    const double camera_fx = 1038.018;
    const double camera_fy = 1038.018;

    //rgb
    Mat rgb;
    rgb = imread("D:\\学习资料\\C++\\C++ code\\middleburry_dataset\\MiddEval3-data-Q\\MiddEval3\\trainingQ\\Adirondack\\im0.png", -1);

    // 点云变量
    // 使用智能指针，创建一个空点云。这种指针用完会自动释放。
    PointCloud::Ptr cloud(new PointCloud);
    // 遍历深度图
    for (int i = 0; i < depth.rows; i++) {
        for (int j = 0; j < depth.cols; j++) {
            //获取深度图中(i,j)处的值
            double d = depth.ptr<double>(i)[j];
            //d 可能没有值，若如此，跳过此点
            if (d == 0) {
                continue;
            }
            //d 存在值，则向点云增加一个点
            PointT p;

            //计算这个点的空间坐标
            p.z = -d;
            p.x = -(i - camera_cx) * p.z / camera_fx;
            p.y = -(j - camera_cy) * p.z / camera_fy;

            // 从rgb图像中获取它的颜色
            // rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
            p.b = rgb.ptr<uchar>(i)[j * 3];
            p.g = rgb.ptr<uchar>(i)[j * 3 + 1];
            p.r = rgb.ptr<uchar>(i)[j * 3 + 2];

            // 把p加入到点云中
            cloud->points.push_back(p);
        }
    }
    // 设置并保存点云
    cloud->height = 1;
    cloud->width = cloud->points.size();
    cout << "point cloud size = " << cloud->points.size() << endl;

    //显示点云
    pcl::visualization::CloudViewer viewer("cloud viewer");
    viewer.showCloud(cloud);
    while (!viewer.wasStopped()) {

    }

    cloud->is_dense = false;
    pcl::io::savePCDFile("D:\\学习资料\\C++\\C++ code\\SGBM\\SGBM\\pointcloud.pcd", *cloud);
    // 清除数据并退出
    cloud->points.clear();
    cout << "Point cloud saved." << endl;

}

Mat calc_depth(const Mat& disp) {
    
    double f = 1038.018;
    double doffs = 53.271;
    double baseline = 176.252;

    Mat depth = Mat(disp.rows, disp.cols, CV_64FC1);
    for (int i = 0; i < disp.rows; i++) {
        for (int j = 0; j < disp.cols; j++) {
            depth.at<double>(i, j) = (baseline * f) / (double(disp.at<uchar>(i, j)) / (255. / 73) + doffs);
        }
    }
    //生成点云
    pointcloud(depth);
    
    Mat depth8U = Mat(disp.rows, disp.cols, CV_8U);
    normalize(depth, depth8U, 0, 255, NORM_MINMAX, CV_8UC1);

    return depth8U;
}


int main() {

    //读图
    //Mat left_img = imread("D:\\学习资料\\C++\\C++ code\\semiglobalmatching-master\\Data\\Teddy\\im2.png", 0);
    //Mat right_img = imread("D:\\学习资料\\C++\\C++ code\\semiglobalmatching-master\\Data\\Teddy\\im6.png", 0);
    Mat left_img = imread("D:\\学习资料\\C++\\C++ code\\middleburry_dataset\\MiddEval3-data-Q\\MiddEval3\\trainingQ\\Adirondack\\im0.png", 0);
    Mat right_img = imread("D:\\学习资料\\C++\\C++ code\\middleburry_dataset\\MiddEval3-data-Q\\MiddEval3\\trainingQ\\Adirondack\\im1.png", 0);


    //创建SGBM对象
    int mindisparity = 0;
    int ndisparities = 73;
    int SADWindowSize = 11;
    Ptr<StereoSGBM> sgbm = StereoSGBM::create(mindisparity, ndisparities, SADWindowSize);

    //设置SGBM参数
    int P1 = 8 * left_img.channels() * SADWindowSize * SADWindowSize;
    int P2 = 32 * left_img.channels() * SADWindowSize * SADWindowSize;
    sgbm->setP1(P1);
    sgbm->setP2(P2);
    sgbm->setPreFilterCap(15);
    sgbm->setUniquenessRatio(10);
    sgbm->setSpeckleRange(2);
    sgbm->setSpeckleWindowSize(50);
    sgbm->setDisp12MaxDiff(1);
    sgbm->setMode(cv::StereoSGBM::MODE_HH);

    //计算视差
    Mat disp;
    sgbm->compute(left_img, right_img, disp);
    disp.convertTo(disp, CV_32F, 1.0 / 16);  //除以16得到真实视差图

    //视差图显示
    Mat disp8U = Mat(disp.rows, disp.cols, CV_8UC1);
    disp.convertTo(disp8U, CV_8UC1, 255. / ndisparities);
    //normalize(disp, disp8U, 0, 255, NORM_MINMAX, CV_8UC1);  //这种归一化的方法是错误的，误匹配率为99%

    //计算深度图
    Mat depth = calc_depth(disp8U);

    
    imshow("disparity", disp8U);
    waitKey(0);
    imwrite("output_disparity.png", disp8U);
    imwrite("output_depth.png", depth);


    system("pause");
    return 0;
}