三维重构学习笔记（6）：双目立体视觉梳理

前话可跳：
学习笔记（6）之前的笔记都是基於单目的，比如SFM相关的。通过单目相机从不同的角度采集图片，然后通过增量式的SFM对每一张图片采集图片等。在做实验的过程中出现以下问题：

特征点计算耗时
增量式的SFM随着图片数量的增加，误差增加
匹配点对较少，有时计算本征矩阵都会遇到麻烦（比如特征点太少，无法计算出本征矩阵，那么后续的图片就更计算不出来了）
点云稀疏，应用场景欠缺
想重构特定位置的物体，但是无法确定如何圈定该位置的特征点等
……
总之，上述问题的出现让我一度怀疑“不小心路又双叒叕走歪了”。感觉与我想要的应用场景不是很匹配，单是运算速度这一点就使我献出膝盖了。所以我决定试试双目立体视觉，看看它与我第一接触的SFM有啥异同。虽然我期望双目提高速度的愿望并没有实现，但是，对于我理解立体视觉具有很好的促进，所以我决定记录下来。
参考博客：
双目实现流程
 由视差图和对应图片显示3D点云

双目立体视觉不同于SFM的流程

双目立体视觉与SFM都是基于实际物体通过相机拍摄到图片时产生的仿射变换这同一个原理工作的。相机标定过程就是求解这个仿射变换的过程，三维重构就是通过这个变换矩阵+一对匹配点求解匹配点在三维座标中的位置。
双目流程：

标定相机
图片立体校正
计算视差图
计算深度图
获得3D座标

双目与SFM的不同点在于：

增量式SFM后续添加的图片都需要计算出本征矩阵进而求解该图片对应的 $R,T$ ，而一般双目会通过标定直接确定两个相机之间的 $R,T$ 。
计算（特征点，匹配点对）的方式不太一样：我见到的双目立体视觉大都是对图片进行立体校正，然后计算两张图片之间的视差，通过视差可以计算像素点的深度；而增量式SFM通过特征点提取与匹配，计算 $R,T$ ，然后直接进行三角测量计算匹配点对的三维座标。

我认为双目的视差计算更像是一种群体匹配；而SFM通过特征点的匹配则更加具有针对性，且更加准确。
更细节的区别我觉得需要研究清楚双目的视差算法才好说，这一点暂时放在后面的学习中。

双目算法中获得视差的方法（OpenCV）

只要获得了比较好的视差，双目立体重构就算是成功了。计算视差之前需要完成：相机标定、立体校正，然后才是视差计算。上述过程都可以依赖于OpenCV。

双目标定

OpenCV标定和MATLAB的工具箱标定都可以，标定结果稍有不同，根据极线约束误差进行判断标定好坏。我的相机分辨率480*640（使用的淘宝上最便宜的CHUSEI双目相机），OpenCV的极线约束误差大概在0.2个像素。但是用OpenCV获得的标定结果进行立体校正效果不是很好，所以我用的MATLAB的标定参数。

立体校正

立体校正表示将两张图片投影到同一个平面上，目的是为了降低立体视觉的计算量。校正平面的扫描线与外极线相同，平行于基线，且通过再投影点。（外极线一般是基线与图像的交点+光线与图像的交点共同构成的线，校正之后的平面平行于基线，则光线与再投影平面的交点构成外极线，同时这条线是校正平面的扫描线）。如下图：

stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, 
		imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
		0, imageSize, &validROIL, &validROIR);

函数原型

CV_EXPORTS_W void stereoRectify( InputArray cameraMatrix1, InputArray distCoeffs1,
                                 InputArray cameraMatrix2, InputArray distCoeffs2,
                                 Size imageSize, InputArray R, InputArray T,
                                 OutputArray R1, OutputArray R2,
                                 OutputArray P1, OutputArray P2,
                                 OutputArray Q, int flags = CALIB_ZERO_DISPARITY,
                                 double alpha = -1, Size newImageSize = Size(),
                                 CV_OUT Rect* validPixROI1 = 0, CV_OUT Rect* validPixROI2 = 0 );

输入左右相机内参数、畸变参数、图片尺寸以及右相机相对于左相机的旋转矩阵 $R$ 、平移向量 $T$ ，输出两个相机的校正变换(rotation matrix) $R_1,R_2$ 、校正座标系中两个相机的的仿射投影变换 $P_1,p_2$ 、以及视差转化为深度的计算需要用的仿射矩阵 $Q$ 。其中 $Q$ 主要用于函数reprojectImageTo3D()。最后两个输出，用于记录图片有效区域，其输出受限于参数double alpha，若alpha为0，则只显示有效部分，若是1，则显示全部图片，也可以取值在0-1之间，其效果在0和1之间。

得到上述矫正之后的各个变换矩阵之后，利用initUndistortRectifyMap函数计算畸变矫正和立体校正的映射变换。

initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl(Rect(0, 0, 3, 3)), imageSize, CV_32FC1, mapLx, mapLy);
initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr(Rect(0, 0, 3, 3)), imageSize, CV_32FC1, mapRx, mapRy);

函数原型

CV_EXPORTS_W void initUndistortRectifyMap( InputArray cameraMatrix, InputArray distCoeffs,
                           InputArray R, InputArray newCameraMatrix,
                           Size size, int m1type, OutputArray map1, OutputArray map2 );

这个函数的目的是：计算没有畸变和校正的转换图，也就是以remap的形式计算“联合的无畸变校正”的图。无畸变的图片就像是从一个内参为newCameraMatrix的且无畸变的新相机采集的原图；新相机与原相机的空间座标系稍有不同，因为在立体校正之后，左右两个相机采集到的图片处于同一个平面，所以相机的 $R$ 与原相机是不同的（理解是否正确有待接下来的学习）。这个函数实际上建立了用于remap的map1，map2。这两个map用于将原始图片向目标图片进行投影。
得到上述的投影map之后，使用remap函数即可将原图转化成“无畸变+校正的图片”：

remap(ImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
remap(ImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);

其中ImageL，ImageR表示原图，输入可以是RGB图也可是灰度图。
查看立体校正的结果可以通过并排画出rectifyImageL，rectifyImageR进行查看：

		Mat canvas;
		int w = imageSize.width ;
		int h = imageSize.height;
		canvas.create(h, w * 2, CV_8UC1);   //通道
											//左图
		Mat canvasPart = canvas(Rect(w * 0, 0, w, h));                                //得到画布的一部分  
		resize(rectifyImageL, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);     //把图像缩放到跟canvasPart一样大小 ，此时左图已经保存在canvas左半边
		Rect vroiL(cvRound(validROIL.x), cvRound(validROIL.y),   
			cvRound(validROIL.width), cvRound(validROIL.height));
		//右图像画到画布上
		canvasPart = canvas(Rect(w, 0, w, h));                                      //获得另一半  
		resize(rectifyImageR, canvasPart, canvasPart.size(), 0, 0, INTER_LINEAR);
		//右保存在canvas右半边
		Rect vroiR(cvRound(validROIR.x * sf), cvRound(validROIR.y*sf),
		cvRound(validROIR.width * sf), cvRound(validROIR.height * sf));
	

		//画上线条
		for (int i = 0; i < canvas.rows; i += 16)
		line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);
		imshow("rectified", canvas);

我的结果如下：

计算视差

通过remap之后，我们得到了“位于同一个平面的无畸变+校正”的两张图片rectifyImageL，rectifyImageR。此时可以通过专门计算视差的函数计算两图的视差。计算视差的函数有很多种，SAD，BM，SGBM，GC等。参考网上对于上述算法的分析，加上目前我用的OpenCV没有实现GC算法，所以我主要使用了SGBM算法。SAD算法、BM算法计算较快，但是结果不理想，SGBM据说是比SAD、BM的效果好，但是计算速度较慢。我也尝试了SAD算法，结果却是不理想，计算出来的视差杂乱无比。需要说明的是，我就算是使用SGBM算法计算的视差也达不到我的要求。导致这一现象的原因有两个：相机标定不够准确+算法参数不合适。计算视差的过程不是一帆风顺，结果差强人意。

OpenCV3提供了SGBM算法接口
首先定义一个SGBM算法对象（输入默认参数）

cv::Ptr<cv::StereoSGBM> sgbm = StereoSGBM::create(0, 16, 3);

然后设置参数：下图式计算左视差的参数

	int numberOfDisparities = 200 & -16;	///视差搜索范围是16的倍数
	int numDisparities = 6;
	sgbm->setPreFilterCap(63);				//映射滤波器的大小
	int SADWindowSize = 9;//SAD窗口大小
	int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3;
	sgbm->setBlockSize(sgbmWinSize);
	int cn = rectifyImageL.channels();
	sgbm->setP1(8 * cn*sgbmWinSize*sgbmWinSize);//惩罚系数
	sgbm->setP2(32 * cn*sgbmWinSize*sgbmWinSize);//惩罚系数
	sgbm->setMinDisparity(0);//最小视差，默认是0
	sgbm->setNumDisparities(numberOfDisparities);
	sgbm->setUniquenessRatio(10);
	sgbm->setSpeckleWindowSize(100);
	sgbm->setSpeckleRange(32);
	sgbm->setDisp12MaxDiff(1);
	sgbm->setMode(cv::StereoSGBM::MODE_SGBM);

参数设置好之后，调用compute计算视差

sgbm->compute(rectifyImageL,rectifyImageR , disp);//输入图像必须为灰度图

此时左图的视差图就保存于disp变量。根据OpenCV的官方文档，disp 中的视差被放大了16倍，所以为了得到准确的视差，应该把disp中的所有元素都除以16，就能够得到准确的视差图。

Mat disp8;
disp.convertTo(disp8, disp.depth(), 1.0 / 16 );

计算深度

得到视差图之后，有两种计算方式得到三维座标
可以使用OpenCV的官方函数reprojectImageTo3D

	cv::reprojectImageTo3D(disp, xyz, Q, true);

得到三维座标xyz。注意如果这里使用的disp是由sgbm->compute出来的，那么disp被放大16倍，所以计算出来的xyz的座标被缩小了16倍，因此xyz*16之后才得到实际的空间座标。

可以根据视差图自行计算
计算思路为根据视差计算出深度，然后根据三角几何关系计算出对应的x,y座标，注意下面的参数中doffs表示两个相机内参数中u0的差

int rowNumber = rectifyImageL.rows;
int colNumber = rectifyImageL.cols;
param_3D.cloud_a.height = rowNumber;
param_3D.cloud_a.width = colNumber;
param_3D.cloud_a.points.resize(colNumber* rowNumber);
for (unsigned int u = 0; u < rowNumber; ++u)
	{
		for (unsigned int v = 0; v < colNumber; ++v)
		{
		double Xw = 0, Yw = 0, Zw = 0;
		Zw = fx * Tx / (((double)disp8.at<Vec3b>(u, v)[0]) + doffs);
		if (Zw > 1000)
		Zw = 0;
		Xw = (v + 1 - u0) * Zw / fx;
		Yw = (u + 1 - v0) * Zw / fy;
		param_3D.cloud_a.points[num].x = Xw;
		param_3D.cloud_a.points[num].y = Yw;
		param_3D.cloud_a.points[num].z = Zw;
		}
}

结果如下：

原图是：

到这里一个粗略的双目立体视觉计算深度的流程就结束了，后面的工作就是对上述过程进行各种优化。

注意点

视差图填充，视差图格式，深度范围限制（防止出现inf出现，导致视图显示不完整），视差图转色温图（为了显示好看）
另外，在保存视差图的时候发现：视差图的精度会受格式的影响，有的博主说可以将视差图同意保存xml类型的数据。
还有SGBM算法是怎么工作的？如何计算得到视差的，函数可以随意调用，但是参数设置应该和原理有关，因此还是需要细致学习一下
重构出的点那么丑，有一些点看起来不属于重构物体或者误差看起来很大，考虑删除等等……
这些注意点内容在下一篇细致探讨，理解了才会用。。。

三维重构（6）：双目立体视觉梳理（主要与SFM对比理解）

三维重构学习笔记（6）：双目立体视觉梳理

双目立体视觉不同于SFM的流程

双目算法中获得视差的方法（OpenCV）

双目标定

立体校正

计算视差

计算深度

注意点

論文閱讀：Disp R-CNN:Stereo 3D Object Detection via Shape Prior Guided Instance Disparity Estimation

kalman濾波算法與仿真

PCL庫使用過程中遇到的編譯錯誤

三維重構學習筆記（2）：理解SFM

退出函數時Mat.release()隨機觸發中斷

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結