由双目立体视差图重建三维点云【Win10+OpenCV3.4.1+PCL1.8】

基于Win10+OpenCV3.4.1+PCL1.8环境，使用双目立体视差图进行三维点云的重建。

原理

使用双目视差图重建三维点云的原理其实很简单，与使用RGB-D相机进行三维点云重建的过程并没有太大区别，主要是通过双目立体视觉获取物体的深度信息($Z_c$)，并计算$X_c、Y_c$坐标值，配合原图提供的RGB信息，即可构成三维点云（PointXYZRGB）。

• 双目立体视觉获取视差图和深度图的原理可参考： 双目视觉测距原理深度剖析：一个被忽略的小问题
• 三维点云重建相关流程可参考：三维重建技术概述

代码

/*
相机参数：
	cam0 = [4152.073 0 1288.147; 0 4152.073 973.571; 0 0 1]
	cam1 = [4152.073 0 1501.231; 0 4152.073 973.571; 0 0 1]
	 doffs = 213.084	
	baseline = 176.252
	width = 2872
	height = 1984
相机内参数矩阵：
	K=[fx 0 u0; 0 fy v0; 0 0 1]
	
	doffs = |u1 - u0| 
*/

#include 
#include   
#include   
#include   
#include   

using namespace cv;
using namespace std;
using namespace pcl;

int user_data;
// 相机内参
const double u0 = 1288.147;
const double v0 = 973.571;
const double fx = 4152.073;
const double fy = 4152.073;
const double baseline = 176.252;
const double doffs = 213.084;	// 代表两个相机主点在x方向上的差距, doffs = |u1 - u0|

void viewerOneOff(visualization::PCLVisualizer& viewer)
{
	viewer.setBackgroundColor(0.0, 0.0, 0.0);
}

int main()
{
	// 读入数据
	Mat color = imread("im0.png"); // RGB
	Mat depth = imread("disp0.png",IMREAD_UNCHANGED);// depth
	if (color.empty() || depth.empty())
	{
		cout << "The image is empty, please check it!" << endl;
		return -1;
	}
	
	// 相机坐标系下的点云
	PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>);

	for (int row = 0; row < depth.rows; row++) 
	{
		for (int col = 0; col < depth.cols; col++)
		{
			ushort d = depth.ptr<ushort>(row)[col];

			if (d==0)
				continue;
			PointXYZRGB p;
			
			// depth			
			p.z = fx * baseline / (d + doffs); // Zc = baseline * f / (d + doffs)
			p.x = (col - u0) * p.z / fx; // Xc向右，Yc向下为正
			p.y = (row - v0) * p.z / fy;
			
			p.y = -p.y;  // 为便于显示，绕x轴三维旋转180°
			p.z = -p.z;
			
			// RGB
			p.b = color.ptr<uchar>(row)[col * 3];
			p.g = color.ptr<uchar>(row)[col * 3 + 1];
			p.r = color.ptr<uchar>(row)[col * 3 + 2];

			cloud->points.push_back(p);
		}
	}

	cloud->height = depth.rows;
	cloud->width = depth.cols;
	cloud->points.resize(cloud->height * cloud->width);

	visualization::CloudViewer viewer("Cloud Viewer");
	viewer.showCloud(cloud);
	viewer.runOnVisualizationThreadOnce(viewerOneOff);

	while (!viewer.wasStopped())
	{
		user_data = 9;
	}
	return 0;
}

有一点需要注意的是，在计算$(X_c,Y_c)$时，如果按照如下公式计算：
$$\{\begin{array}{l}{X}_c=(u-u_0)\ast Z_c/f_x\\ Y_c=(v-v_0)\ast Z_c/f_y\end{array}$$

则所得坐标以：$x$轴向右、$y$轴向下为正方向，如下图。

图像坐标系与相机坐标系的关系

因此，为了正常显示，需要将结果绕$x$轴旋转180°（三维旋转）。而绕$x$轴进行三维旋转的变换矩阵如下所示：

取 $\Theta =180°$，可知绕$x$轴旋转180°，即相当于：
$$\{\begin{array}{l}{y}^{\prime }=-y\\ z^{\prime }=-z\end{array}$$

结果如图：

素材及处理

博主使用的RGB和depth图取自：vision middlebury_Adirondack，大家可自行下载。

• 问题
  因为middlebury提供的groundtruth是.pfm格式的文件，而OpenCV是不支持.pfm格式文件操作的，因此就需要自己造“轮子”或者转成OpenCV支持的格式。
• 解决方法
  这里使用的是图片格式在线转换工具，将.pfm格式图片转成.png格式：多种格式在线互转网站，亲测好用。
  当然，github上大神也提供了开源的“轮子”，可直接读入.pfm文件并转成Mat格式：点击此处链接，操作十分简单，如下是作者给出的例子。

/**
 * Example of how to read and save PFM files.
 */

#include 
#include 

#include "PFMReadWrite.h"

using namespace std;
using namespace cv;

int main(void)
{
    //Example that loads an image and stores it under another name
    Mat image = loadPFM(string("image.pfm"));

    //Display the image
    imshow("Image", image);
    waitKey(0);

    savePFM(image, "image_saved.pfm");

    return 0;
}

备注：

• 上述代码同样支持其他图片，只要修改对应的相机内参即可。
• 所有相关文件下载【CSDN】：双目立体视差图进行三维点云重建【OpenCV3.4.1+PCL1.8】
• 参考：用VS+Opencv3.1从双目立体视差图中重建三维点云