OpenCV深度图转点云

需要先进行畸变校正，再通过深度图转点云。
相机的相关参数需要事先通过标定获得。

#include <iostream>
#include <fstream>

#include <opencv2\opencv.hpp>

int main() {
    
    
	cv::Mat src = cv::imread("temp/DepthdataZ.tiff", -1);  //读取深度图
	if (src.empty()) {
    
    
		return -1;
	}

	//图片长宽
	int image_width = src.cols;
	int image_height = src.rows;

	//焦距、中心点
	double fx = 3393.866416936082260;
	double fy = 3392.939030157020625;
	double cx = 657.783535966715249;
	double cy = 534.244397069343677;

	//镜头畸变参数
	double k1 = -0.023412391369262;
	double k2 = -0.456621699069608;
	double p1 = 0.000738096195024;
	double p2 = 0.001507735545219;
	double k3 = 18.187708920601441;

	//fx, 0, cx
	//0, fy, cy
	//0, 0, 1
	cv::Mat cameraMatrix(3, 3, CV_32FC1, cv::Scalar::all(0));   //内参矩阵3*3

	//k1, k2, p1, p2, k3
	//k1: 主径向畸变系数，一般是正值，代表着镜头的畸变会导致图像中心的像素缩小；
	//k2 : 副径向畸变系数，一般是正值，代表着镜头的畸变会导致图像边缘的像素缩小；
	//p1, p2 : 切向畸变系数，通常很小，代表着图像的畸变会使得像素沿着切线方向移动；
	//k3 : 高阶径向畸变系数，一般很小，主要用于描述镜头的非线性畸变。
	cv::Mat distCoeffs(1, 5, CV_32FC1, cv::Scalar::all(0)); //畸变矩阵1*5

	cameraMatrix = (cv::Mat_<double>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
	distCoeffs = (cv::Mat_<double>(1, 5) << k1, k2, p1, p2, k3);

	//去畸变
	cv::Mat dst;  //保存去畸变后的深度图
	cv::undistort(src, dst, cameraMatrix, distCoeffs, cv::noArray());
	
	//将深度图转点云
	cv::Mat img_z = dst.clone();  //点云z值（即深度图保存的数据<去畸变后>）
	cv::Mat img_x(cv::Size(image_width, image_height), CV_32F);  //点云x值（即要还原的数据）
	cv::Mat img_y(cv::Size(image_width, image_height), CV_32F);  //点云y值（即要还原的数据）

	for (int i = 0; i < image_width; ++i) {
    
    
		for (int j = 0; j < image_height; ++j) {
    
    
			img_x.at<float>(j, i) = (i - cx) * img_z.at<float>(j, i) / fx;
			img_y.at<float>(j, i) = (j - cy) * img_z.at<float>(j, i) / fy;
		}
	}

	//保存点云
	std::ofstream out("temp/alldata.asc");
	for (int i = 0; i < image_width; i++) {
    
    
		for (int j = 0; j < image_height; j++) {
    
    
			float _z = img_z.at<float>(j, i);
			float _x = img_x.at<float>(j, i);
			float _y = img_y.at<float>(j, i);

			if ((_z > -9999) && (_z != 0)) {
    
      //过滤无效点
				out << _x << " " << _y << " " << _z << std::endl;
			}
		}
	}
	out.close();

	return 0;
}