最近对双目视觉的应用挺感兴趣的，之前又完成了双目的标定，刚好可以把数据拿过来使用，继续做相关实验，实验里的代码很多都是参考大神的，这里也分享下这些链接：
https://blog.csdn.net/weixin_39449570/article/details/79033314
https://blog.csdn.net/Loser__Wang/article/details/52836042
由于opencv用的版本是2.4.13，使用上跟3.0以上版本还是有所区别的，2.4.13可参考
https://blog.csdn.net/chentravelling/article/details/53672578
3.0以上版本StereoBM等定义为纯虚类，不能直接实例化，可参考
https://blog.csdn.net/chentravelling/article/details/70254682
下面简单说下视觉差的原理：

其中，Tx为两相机光心间的距离，P(Xc,Yc,Zc)为左相机坐标系下的相机坐标，xl为P在左相机像平面X轴坐标，xr为P在右相机像平面X轴坐标(这里只考虑相机在同一水平位置且焦距相同)，根据成像原理有
                                                                
将右相机转换到左相机坐标系下，则P在相机2的坐标转换到相机1后的坐标为P(Xc-Tx,Yc,Zc),模型如下:


视差：
                                                       
所以在这中简单的模型下只有知道视差d就可以求得距离Zc
接下来就是上代码了，在使用代码前需要进行双目标定，将相机的内外参数求出来：
#include <stdio.h>  
#include <iostream>
#include "opencv2/calib3d/calib3d.hpp"  
#include "opencv2/imgproc/imgproc.hpp"  
#include "opencv2/highgui/highgui.hpp"  
#include "opencv2/contrib/contrib.hpp" 

using namespace std;
using namespace cv;

enum { STEREO_BM = 0, 
	STEREO_SGBM = 1, 
	STEREO_HH = 2, 
	STEREO_VAR = 3 };

Size imageSize = Size(640, 480);
Point origin;       //鼠标按下的起始点
Rect selection;     //定义矩形选框
bool selectObject = false;
Mat xyz;              //三维坐标

static void saveXYZ(const char* filename, const Mat& mat)
{
	const double max_z = 1.0e4;
	FILE* fp = fopen(filename, "wt");
	for (int y = 0; y < mat.rows; y++)
	{
		for (int x = 0; x < mat.cols; x++)
		{
			Vec3f point = mat.at<Vec3f>(y, x);
			if (fabs(point[2] - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue;
			fprintf(fp, "%f %f %f\n", point[0], point[1], point[2]);
		}
	}
	fclose(fp);
}

/*给深度图上色*/
void GenerateFalseMap(cv::Mat &src, cv::Mat &disp)
{
	// color map  
	float max_val = 255.0f;
	float map[8][4] = { { 0, 0, 0, 114 }, { 0, 0, 1, 185 }, { 1, 0, 0, 114 }, { 1, 0, 1, 174 },
	{ 0, 1, 0, 114 }, { 0, 1, 1, 185 }, { 1, 1, 0, 114 }, { 1, 1, 1, 0 } };
	float sum = 0;
	for (int i = 0; i < 8; i++)
		sum += map[i][3];

	float weights[8]; // relative   weights  
	float cumsum[8];  // cumulative weights  
	cumsum[0] = 0;
	for (int i = 0; i < 7; i++) {
		weights[i] = sum / map[i][3];
		cumsum[i + 1] = cumsum[i] + map[i][3] / sum;
	}

	int height_ = src.rows;
	int width_ = src.cols;
	// for all pixels do  
	for (int v = 0; v < height_; v++) {
		for (int u = 0; u < width_; u++) {

			// get normalized value  
			float val = std::min(std::max(src.data[v*width_ + u] / max_val, 0.0f), 1.0f);

			// find bin  
			int i;
			for (i = 0; i < 7; i++)
				if (val < cumsum[i + 1])
					break;

			// compute red/green/blue values  
			float   w = 1.0 - (val - cumsum[i])*weights[i];
			uchar r = (uchar)((w*map[i][0] + (1.0 - w)*map[i + 1][0]) * 255.0);
			uchar g = (uchar)((w*map[i][1] + (1.0 - w)*map[i + 1][1]) * 255.0);
			uchar b = (uchar)((w*map[i][2] + (1.0 - w)*map[i + 1][2]) * 255.0);
			//rgb内存连续存放  
			disp.data[v*width_ * 3 + 3 * u + 0] = b;
			disp.data[v*width_ * 3 + 3 * u + 1] = g;
			disp.data[v*width_ * 3 + 3 * u + 2] = r;
		}
	}
}

/*****描述：鼠标操作回调*****/
static void onMouse(int event, int x, int y, int, void*)
{
	if (selectObject)
	{
		selection.x = MIN(x, origin.x);
		selection.y = MIN(y, origin.y);
		selection.width = std::abs(x - origin.x);
		selection.height = std::abs(y - origin.y);
	}

	switch (event)
	{
	case EVENT_LBUTTONDOWN:   //鼠标左按钮按下的事件
		origin = Point(x, y);
		selection = Rect(x, y, 0, 0);
		selectObject = true;
		cout << origin << "in world coordinate is: " << xyz.at<Vec3f>(origin) << endl;
		break;
	case EVENT_LBUTTONUP:    //鼠标左按钮释放的事件
		selectObject = false;
		if (selection.width > 0 && selection.height > 0)
			break;
	}
}

void readCamParam(string& filename, Mat& camL_Matrix, Mat& camL_distcoeff, 
				Mat& camR_Matrix, Mat& camR_distcoeff,Mat& R,Mat& T)
{
	FileStorage fs(filename, FileStorage::READ);
	if (!fs.isOpened())
	{
		cout << "there is not the param file!" << endl;
	}
	if (fs.isOpened())
	{
		camL_Matrix = Mat(3, 3, CV_64F);
		fs["cam1_Matrix"] >> camL_Matrix;
		camL_distcoeff = Mat(3, 1, CV_64F);
		fs["cam1_distcoeff"] >> camL_distcoeff;

		camR_Matrix = Mat(3, 3, CV_64F);
		fs["cam2_Matrix"] >> camR_Matrix;
		camR_distcoeff = Mat(3, 1, CV_64F);
		fs["cam2_distcoeff"] >> camR_distcoeff;

		R = Mat(3, 3, CV_64F);
		fs["R"] >> R;
		T = Mat(3, 1, CV_64F);
		fs["T"] >> T;
	}
}

void images2one(Size& imageSize, Mat& rectifyImageL, Rect& validROIL,
			Mat& rectifyImageR, Rect& validROIR)
{
	//显示在同一张图上
	Mat canvas;
	double sf;
	int w, h;
	sf = 700. / MAX(imageSize.width, imageSize.height);
	w = cvRound(imageSize.width * sf);         //cvRound输入一个double类型返回一个整形
	h = cvRound(imageSize.height * sf);
	canvas.create(h, w * 2, CV_8UC3);   //注意通道

	//左图像画到画布上
	Mat canvasPart = canvas(Rect(w * 0, 0, w, h));                                //得到画布的一部分  
	resize(rectifyImageL, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);     //把图像缩放到跟canvasPart一样大小  
	Rect vroiL(cvRound(validROIL.x*sf), cvRound(validROIL.y*sf),                //获得被截取的区域    
		cvRound(validROIL.width*sf), cvRound(validROIL.height*sf));
	rectangle(canvasPart, vroiL, Scalar(0, 0, 255), 3, 8);                      //画上一个矩形  
	cout << "Painted ImageL" << endl;

	//右图像画到画布上
	canvasPart = canvas(Rect(w, 0, w, h));                                      //获得画布的另一部分  
	resize(rectifyImageR, canvasPart, canvasPart.size(), 0, 0, INTER_LINEAR);
	Rect vroiR(cvRound(validROIR.x * sf), cvRound(validROIR.y*sf),
		cvRound(validROIR.width * sf), cvRound(validROIR.height * sf));
	rectangle(canvasPart, vroiR, Scalar(0, 0, 255), 3, 8);
	cout << "Painted ImageR" << endl;

	//画上对应的线条
	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);
}

void stereoSGBM_match(int alg, Mat& imgL, Mat& imgR, Mat& disp8, Mat& dispf)
{
	int SADWindowSize = 0, numberOfDisparities = 0;
	float scale = 1.f;

	StereoBM bm;
	StereoSGBM sgbm;
	StereoVar var;


	Size img_size = imgL.size();

	Rect roi1, roi2;

	numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : ((img_size.width / 8) + 15) & -16;

	bm.state->roi1 = roi1;
	bm.state->roi2 = roi2;
	bm.state->preFilterCap = 31;
	bm.state->SADWindowSize = SADWindowSize > 0 ? SADWindowSize : 9;
	bm.state->minDisparity = 0;
	bm.state->numberOfDisparities = numberOfDisparities;
	bm.state->textureThreshold = 10;
	bm.state->uniquenessRatio = 15;
	bm.state->speckleWindowSize = 100;
	bm.state->speckleRange = 32;
	bm.state->disp12MaxDiff = 1;

	sgbm.preFilterCap = 63;
	sgbm.SADWindowSize = SADWindowSize > 0 ? SADWindowSize : 3;

	int cn = imgL.channels();

	sgbm.P1 = 8 * cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
	sgbm.P2 = 32 * cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
	sgbm.minDisparity = 0;
	sgbm.numberOfDisparities = numberOfDisparities;
	sgbm.uniquenessRatio = 10;
	sgbm.speckleWindowSize = bm.state->speckleWindowSize;
	sgbm.speckleRange = bm.state->speckleRange;
	sgbm.disp12MaxDiff = 1;
	sgbm.fullDP = alg == STEREO_HH;

	var.levels = 3;                                 // ignored with USE_AUTO_PARAMS  
	var.pyrScale = 0.5;                             // ignored with USE_AUTO_PARAMS  
	var.nIt = 25;
	var.minDisp = -numberOfDisparities;
	var.maxDisp = 0;
	var.poly_n = 3;
	var.poly_sigma = 0.0;
	var.fi = 15.0f;
	var.lambda = 0.03f;
	var.penalization = var.PENALIZATION_TICHONOV;   // ignored with USE_AUTO_PARAMS  
	var.cycle = var.CYCLE_V;                        // ignored with USE_AUTO_PARAMS  
	var.flags = var.USE_SMART_ID | var.USE_AUTO_PARAMS | var.USE_INITIAL_DISPARITY | var.USE_MEDIAN_FILTERING;

	Mat disp ;
	//去黑边
	Mat img1p, img2p;
	copyMakeBorder(imgL, img1p, 0, 0, numberOfDisparities, 0, IPL_BORDER_REPLICATE);
	copyMakeBorder(imgR, img2p, 0, 0, numberOfDisparities, 0, IPL_BORDER_REPLICATE);
	imshow("img1p", img1p);
	imshow("img2p", img2p);

	int64 t = getTickCount();
	if (alg == STEREO_BM)
		bm(imgL, imgR, disp);
	else if (alg == STEREO_VAR) {
		var(imgL, imgR, disp);
	}
	else if (alg == STEREO_SGBM || alg == STEREO_HH)
		sgbm(imgL, imgR, disp);//------  

	t = getTickCount() - t;
	printf("Time elapsed: %fms\n", t * 1000 / getTickFrequency());

	dispf = disp.colRange(numberOfDisparities, img2p.cols - numberOfDisparities);

	if (alg != STEREO_VAR)
		dispf.convertTo(disp8, CV_8U, 255 / (numberOfDisparities*16.));
	else
		dispf.convertTo(disp8, CV_8U);
}

int main()
{
	int alg = STEREO_SGBM;
	int color_mode = alg == STEREO_BM ? 0 : -1;

	string filename = "calibrateResult.xml";
	Mat camL_Matrix, camR_Matrix, camL_distcoeff, camR_distcoeff, R, T;
	Mat Rl, Rr, Pl,Pr,Q;       //校正旋转矩阵R，投影矩阵P 重投影矩阵Q
	Rect validROIL, validROIR;
	readCamParam(filename, camL_Matrix, camL_distcoeff, camR_Matrix, camR_distcoeff, R, T);
	stereoRectify(camL_Matrix, camL_distcoeff, camR_Matrix, camR_distcoeff, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
		0, imageSize, &validROIL, &validROIR);
	Mat mapLx, mapLy, mapRx, mapRy;     //映射表 
	initUndistortRectifyMap(camL_Matrix, camL_distcoeff, Rl, Pl, imageSize, CV_16SC2, mapLx, mapLy);
	initUndistortRectifyMap(camR_Matrix, camR_distcoeff, Rr, Pr, imageSize, CV_16SC2, mapRx, mapRy);

	Mat imgL = imread("L.bmp", color_mode);
	Mat imgR = imread("R.bmp", color_mode);

	Mat rectifyImageL, rectifyImageR;
	remap(imgL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
	remap(imgR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);
	imshow("rectifyImageL", rectifyImageL);
	imshow("rectifyImageR", rectifyImageR);

	//显示在同一张图上
	images2one(imageSize, rectifyImageL, validROIL, rectifyImageR, validROIR);

	namedWindow("color_disparity", CV_WINDOW_NORMAL);
	setMouseCallback("color_disparity", onMouse, 0);


	bool no_display = false;
	Mat disp8,dispf;
	stereoSGBM_match(alg, imgL, imgR, disp8, dispf);
	

	reprojectImageTo3D(dispf, xyz, Q, true);    //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
	xyz = xyz * 16;
	if (!no_display)
	{
		namedWindow("left", 1);
		imshow("left", imgL);

		namedWindow("right", 1);
		imshow("right", imgR);

		namedWindow("disparity", 0);
		imshow("disparity", disp8);

		Mat color(dispf.size(), CV_8UC3);
		GenerateFalseMap(disp8, color);//转成彩图
		imshow("color_disparity", color);

		waitKey(500);
		printf("press any key to continue...");
		fflush(stdout);
		waitKey();
		printf("\n");
	}	
	saveXYZ("xyz.xls", xyz);
	return 0;
}
下面为代码的链接：
https://download.csdn.net/download/logan_lin/10345775

在 OpenCV 机器视觉库中，有许多有趣实用的应用，目标识别，颜色识别，物体检查和测量等。这里和大家分享机器视觉中重要的一个点——双目机器视觉测距。
本场 Chat 首先会带领大家对双目摄像头定标，然后校正摄像，再通过编程实现三维重建，最后和大家进行双目测距，测出物体到摄像头的实际距离，正常误差小于 2cm。
本场 Chat 您将学到如下内容：
双目摄像头：定标+矫正；
双目视觉：三维重建；
双目摄像头测距。
本场 Chat 不但分享方法、经验，还会分享源代码。
阅读全文: http://gitbook.cn/gitchat/activity/5d4825710b3c014c980e7f3a
您还可以下载 CSDN 旗下精品原创内容社区 GitChat App ，阅读更多 GitChat 专享技术内容哦。

虽然最近注意力已经不可遏制地被神经科学、大脑记忆机制和各种毕业活动吸引过去了，但是还是觉得有必要把这段时间双目视觉方面的进展总结一下。毕竟从上一篇博文发表之后，很多同仁发E-mail来与我讨论，很多原来的疑团，也在讨论和一步步的试验中逐渐解决了。  

开篇之前，首先要感谢maxwellsdemon和wobject，没有和你们的讨论，也就没有此篇的成文。




说到双摄像头测距，首先要复习一下测距原理，把Learning OpenCV翻到416和418页，可以看到下面两幅图。






图1. 双摄像头模型俯视图






图2, 双摄像头模型立体视图










图1解释了双摄像头测距的原理，书中Z的公式如下：



 

在OpenCV中，f的量纲是像素点，Tx的量纲由定标棋盘格的实际尺寸和用户输入值确定，一般总是设成毫米，当然为了精度提高也可以设置为0.1毫米量级，d=xl-xr的量纲也是像素点。因此分子分母约去，z的量纲与Tx相同

 

图2解释了双摄像头获取空间中某点三维坐标的原理。

 

可以看到，实际的坐标计算利用的都是相似三角形的原理，其表达式就如同Q矩阵所示。

 

 

空间中某点的三维坐标就是(X/W, Y/W, Z/W)。

 

因此，为了精确地求得某个点在三维空间里的距离，我们需要获得的参数有焦距f、视差d、摄像头中心距Tx。

如果还需要获得X坐标和Y坐标的话，那么还需要额外知道左右像平面的坐标系与立体坐标系中原点的偏移cx和cy。其中f, Tx, cx和cy可以通过立体标定获得初始值，并通过立体校准优化，使得两个摄像头在数学上完全平行放置，并且左右摄像头的cx, cy和f相同(也就是实现图2中左右视图完全平行对准的理想形式)。而立体匹配所做的工作，就是在之前的基础上，求取最后一个变量：视差d(这个d一般需要达到亚像素精度)。从而最终完成求一个点三维坐标所需要的准备工作。

 

在清楚了上述原理之后，我们也就知道了，所有的这几步：标定、校准和匹配，都是围绕着如何更精确地获得f, d, Tx, cx和cy而设计的。




 

双目测距的原理就说到这里，为了避免大家看到大段纯叙述性的文字头晕，下面的行文将会以FAQ的形式围绕着实现双摄像头测距过程中碰到的几点疑惑展开。当然，其中的解答也只是我的个人理解，如有不当，敬请指正。

Q1：标定时棋盘格的大小如何设定，对最后结果有没有影响？

A：当然有。在标定时，需要指定一个棋盘方格的长度，这个长度(一般以毫米为单位，如果需要更精确可以设为0.1毫米量级)与实际长度相同，标定得出的结果才能用于实际距离测量。一般如果尺寸设定准确的话，通过立体标定得出的Translation的向量的第一个分量Tx的绝对值就是左右摄像头的中心距。一般可以用这个来验证立体标定的准确度。比如我设定的棋盘格大小为270 (27mm)，最终得出的Tx大小就是602.8 (60.28mm)，相当精确。

 

Q2：通过立体标定得出的Tx符号为什么是负的？

A：这个其实我也不是很清楚。个人的解释是，立体标定得出的T向量指向是从右摄像头指向左摄像头(也就是Tx为负)，而在OpenCV坐标系中，坐标的原点是在左摄像头的。因此，用作校准的时候，要把这个向量的三个分量符号都要换一下，最后求出的距离才会是正的。

但是这里还有一个问题，就是Learning OpenCV中Q的表达式，第四行第三列元素是-1/Tx，而在具体实践中，求出来的实际值是1/Tx。这里我和maxwellsdemon讨论下来的结果是，估计书上Q表达式里的这个负号就是为了抵消T向量的反方向所设的，但在实际写OpenCV代码的过程中，那位朋友却没有把这个负号加进去。(一家之言，求更详细的解释)

 

Q3：cvFindStereoCorrespondenceBM的输出结果好像不是以像素点为单位的视差？

A：在OpenCV2.0中，BM函数得出的结果是以16位符号数的形式的存储的，出于精度需要，所有的视差在输出时都扩大了16倍(2^4)。其具体代码表示如下：

dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4);

可以看到，原始视差在左移8位(256)并且加上一个修正值之后又右移了4位，最终的结果就是左移4位

因此，在实际求距离时，cvReprojectTo3D出来的X/W,Y/W,Z/W都要乘以16 (也就是W除以16)，才能得到正确的三维坐标信息




Q4：利用双摄像头进行测距的时候世界坐标的原点究竟在哪里？ 

A：世界坐标系的原点是左摄像头凸透镜的光心。

说起这个，就不得不提到针孔模型。如图3所示，针孔模型是凸透镜成像的一种简化模型。当物距足够远时(远大于两倍焦距)，凸透镜成像可以看作是在焦距处的小孔成像。(ref: http://bak1.beareyes.com.cn/2/lib/200110/04/20011004006.htm)






图3. 针孔模型




在实际计算过程中，为了计算方便，我们将像平面翻转平移到针孔前，从而得到一种数学上更为简单的等价形式（方便相似三角形的计算），如图4所示。



图4. 针孔模型的数学等价形式

 

 

因此，对应图2就可以知道，世界坐标系原点就是左摄像头针孔模型的针孔，也就是左摄像头凸透镜的光心

 

Q5：f和d的单位是像素，那这个像素到底表示什么，它与毫米之间又是怎样换算的？

A：这个问题也与针孔模型相关。在针孔模型中，光线穿过针孔（也就是凸透镜中心）在焦距处上成像，因此，图3的像平面就是摄像头的CCD传感器的表面。每个CCD传感器都有一定的尺寸，也有一定的分辨率，这个就确定了毫米与像素点之间的转换关系。举个例子，CCD的尺寸是8mm X 6mm，分辨率是640X480，那么毫米与像素点之间的转换关系就是80pixel/mm。

在实际运用中，我们在数学上将这个像平面等效到小孔前（图4），这样就相当于将在透镜中心点之前假设了一块虚拟的CCD传感器。

 

Q6：为什么cvStereoRectify求出的Q矩阵cx, cy, f都与原来的不同？

A：这个在前文有提到过。在实际测量中，由于摄像头摆放的关系，左右摄像头的f, cx, cy都是不相同的。而为了使左右视图达到完全平行对准的理想形式从而达到数学上运算的方便，立体 校准所做的工作事实上就是在左右像重合区域最大的情况下，让两个摄像头光轴的前向平行，并且让左右摄像头的f, cx, cy相同。因此，Q矩阵中的值与两个instrinsic矩阵的值不一样就可以理解了。

 

实验结果：

实验下来，虽然Block Matching算法本身对精度有所限制，但测距基本能达到能让人接受的精度，结果如下图5所示

 



图5. OpenCV双摄像头测距结果




上图中，中、左、右三个物体分别被放在离摄像头50cm, 75cm和90cm的位置。可以看出测距的结果相当不错。当然，上面这幅图是比较好的结果。由于BM算法的限制，同一点云中相同距离的点一般会有正负2厘米之内的误差。




图6是利用双目摄像头测物体长宽的结果，可以看出结果似乎不太准确。。。



图6. OpenCV双摄像头测边长结果




其中，物体宽为117-88=29mm，但实际宽度为5.2cm，物体高位71-13=58mm，但实际高度为13cm。这方面的误差还是比较难以理解


此外，还有一个问题至今尚未完全理解，就是双目摄像头的中心距，为什么采用Tx而不是T向量的长度。因为如果要左右视图重合区域最大化的话两个摄像头的光轴都要与T垂直才是(如图7)，这样的话，校正后两个摄像头的中心距应该是T才对。不知道我这样的理解对不对？






图7. 双摄像头立体校准俯视图

AI
人工智能
使用OpenCV/python进行双目测距
            
在做SLAM时，希望用到深度图来辅助生成场景，所以要构建立体视觉，在这里使用OpenCV的Stereo库和python来进行双目立体视觉的图像处理。
立体标定
应用标定数据
转换成深度图
标定
在开始之前，需要准备的当然是两个摄相头，根据你的需求将两个摄像头进行相对位置的固定，我是按平行来进行固定的(如果为了追求两个双目图像更高的生命度，也可以将其按一定钝角固定，这样做又限制了场景深度的扩展，根据实际需求选择)
由于摄像头目前是我们手动进行定位的，我们现在还不知道两张图像与世界坐标之间的耦合关系，所以下一步要进行的是标定，用来确定分别获取两个摄像头的内部参数，并且根据两个摄像头在同一个世界坐标下的标定参数来获取立体参数。注：不要使用OpenCV自带的自动calbration，其对棋盘的识别率极低，使用Matlab的Camera Calibration Toolbox更为有效，具体细节请看：摄像机标定和立体标定
同时从两个摄像头获取图片
import cv2
import time
AUTO = True  # 自动拍照，或手动按s键拍照
INTERVAL = 2 # 自动拍照间隔
cv2.namedWindow("left")
cv2.namedWindow("right")
cv2.moveWindow("left", 0, 0)
cv2.moveWindow("right", 400, 0)
left_camera = cv2.VideoCapture(0)
right_camera = cv2.VideoCapture(1)
counter = 0
utc = time.time()
pattern = (12, 8) # 棋盘格尺寸
folder = "./snapshot/" # 拍照文件目录
def shot(pos, frame):
global counter
path = folder + pos + "_" + str(counter) + ".jpg"
cv2.imwrite(path, frame)
print("snapshot saved into: " + path)
while True:
ret, left_frame = left_camera.read()
ret, right_frame = right_camera.read()
cv2.imshow("left", left_frame)
cv2.imshow("right", right_frame)
now = time.time()
if AUTO and now - utc >= INTERVAL:
shot("left", left_frame)
shot("right", right_frame)
counter += 1
utc = now
key = cv2.waitKey(1)
if key == ord("q"):
break
elif key == ord("s"):
shot("left", left_frame)
shot("right", right_frame)
counter += 1
left_camera.release()
right_camera.release()
cv2.destroyWindow("left")
cv2.destroyWindow("right")
下面是我拍摄的样本之一，可以肉眼看出来这两个摄像头成像都不是水平的，这更是需要标定的存在的意义
在进行标定的过程中，要注意的是在上面标定方法中没有提到的是，单个标定后，要对标定的数据进行错误分析(Analyse Error)，如左图，是我对左摄像头的标定结果分析。图中天蓝色点明显与大部分点不聚敛，所以有可能是标定时对这个图片标定出现的错误，要重新标定，在该点上点击并获取其图片名称索引，对其重新标定后，右图的结果看起来还是比较满意的
在进行完立体标定后，我们将得到如下的数据：
Stereo calibration parameters after optimization:
Intrinsic parameters of left camera:
Focal Length:          fc_left = [ 824.93564   825.93598 ]  [ 8.21112   8.53492 ]
Principal point:       cc_left = [ 251.64723   286.58058 ]  [ 13.92642   9.11583 ]
Skew:             alpha_c_left = [ 0.00000 ]  [ 0.00000  ]   => angle of pixel axes = 90.00000  0.00000 degrees
Distortion:            kc_left = [ 0.23233   -0.99375   0.00160   0.00145  0.00000 ]  [ 0.05659   0.30408   0.00472   0.00925  0.00000 ]
Intrinsic parameters of right camera:
Focal Length:          fc_right = [ 853.66485   852.95574 ]  [ 8.76773   9.19051 ]
Principal point:       cc_right = [ 217.00856   269.37140 ]  [ 10.40940   9.47786 ]
Skew:             alpha_c_right = [ 0.00000 ]  [ 0.00000  ]   => angle of pixel axes = 90.00000  0.00000 degrees
Distortion:            kc_right = [ 0.30829   -1.61541   0.01495   -0.00758  0.00000 ]  [ 0.06567   0.55294   0.00547   0.00641  0.00000 ]
Extrinsic parameters (position of right camera wrt left camera):
Rotation vector:             om = [ 0.01911   0.03125  -0.00960 ]  [ 0.01261   0.01739  0.00112 ]
Translation vector:           T = [ -70.59612   -2.60704  18.87635 ]  [ 0.95533   0.79030  5.25024 ]
应用标定数据
我们使用如下的代码来将其配置到python中，上面的参数都是手动填写至下面的内容中的，这样免去保存成文件再去读取，在托运填写的时候要注意数据的对应位置。
# filename: camera_configs.py
import cv2
import numpy as np
left_camera_matrix = np.array([[824.93564, 0., 251.64723],
[0., 825.93598, 286.58058],
[0., 0., 1.]])
left_distortion = np.array([[0.23233, -0.99375, 0.00160, 0.00145, 0.00000]])
right_camera_matrix = np.array([[853.66485, 0., 217.00856],
[0., 852.95574, 269.37140],
[0., 0., 1.]])
right_distortion = np.array([[0.30829, -1.61541, 0.01495, -0.00758, 0.00000]])
om = np.array([0.01911, 0.03125, -0.00960]) # 旋转关系向量
R = cv2.Rodrigues(om)[0]  # 使用Rodrigues变换将om变换为R
T = np.array([-70.59612, -2.60704, 18.87635]) # 平移关系向量
size = (640, 480) # 图像尺寸
# 进行立体更正
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(left_camera_matrix, left_distortion,
right_camera_matrix, right_distortion, size, R,
T)
# 计算更正map
left_map1, left_map2 = cv2.initUndistortRectifyMap(left_camera_matrix, left_distortion, R1, P1, size, cv2.CV_16SC2)
right_map1, right_map2 = cv2.initUndistortRectifyMap(right_camera_matrix, right_distortion, R2, P2, size, cv2.CV_16SC2)
这样，我们得到了左右摄像头的两个map，并得到了立体的Q，这些参数都将应用于下面的转换成深度图中
转换成深度图
import numpy as np
import cv2
import camera_configs
cv2.namedWindow("left")
cv2.namedWindow("right")
cv2.namedWindow("depth")
cv2.moveWindow("left", 0, 0)
cv2.moveWindow("right", 600, 0)
cv2.createTrackbar("num", "depth", 0, 10, lambda x: None)
cv2.createTrackbar("blockSize", "depth", 5, 255, lambda x: None)
camera1 = cv2.VideoCapture(0)
camera2 = cv2.VideoCapture(1)
# 添加点击事件，打印当前点的距离
def callbackFunc(e, x, y, f, p):
if e == cv2.EVENT_LBUTTONDOWN:
print threeD[y][x]
cv2.setMouseCallback("depth", callbackFunc, None)
while True:
ret1, frame1 = camera1.read()
ret2, frame2 = camera2.read()
if not ret1 or not ret2:
break
# 根据更正map对图片进行重构
img1_rectified = cv2.remap(frame1, camera_configs.left_map1, camera_configs.left_map2, cv2.INTER_LINEAR)
img2_rectified = cv2.remap(frame2, camera_configs.right_map1, camera_configs.right_map2, cv2.INTER_LINEAR)
# 将图片置为灰度图，为StereoBM作准备
imgL = cv2.cvtColor(img1_rectified, cv2.COLOR_BGR2GRAY)
imgR = cv2.cvtColor(img2_rectified, cv2.COLOR_BGR2GRAY)
# 两个trackbar用来调节不同的参数查看效果
num = cv2.getTrackbarPos("num", "depth")
blockSize = cv2.getTrackbarPos("blockSize", "depth")
if blockSize % 2 == 0:
blockSize += 1
if blockSize < 5:
blockSize = 5
# 根据Block Maching方法生成差异图(opencv里也提供了SGBM/Semi-Global Block Matching算法，有兴趣可以试试)
stereo = cv2.StereoBM_create(numDisparities=16*num, blockSize=blockSize)
disparity = stereo.compute(imgL, imgR)
disp = cv2.normalize(disparity, disparity, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)
# 将图片扩展至3d空间中，其z方向的值则为当前的距离
threeD = cv2.reprojectImageTo3D(disparity.astype(np.float32)/16., camera_configs.Q)
cv2.imshow("left", img1_rectified)
cv2.imshow("right", img2_rectified)
cv2.imshow("depth", disp)
key = cv2.waitKey(1)
if key == ord("q"):
break
elif key == ord("s"):
cv2.imwrite("./snapshot/BM_left.jpg", imgL)
cv2.imwrite("./snapshot/BM_right.jpg", imgR)
cv2.imwrite("./snapshot/BM_depth.jpg", disp)
camera1.release()
camera2.release()
cv2.destroyAllWindows()
下面则是一附成像图，最右侧的为生成的disparity图，按照上面的代码，在图上点击则可以读取到该点的距离
Have fun.
内容来源于网络，如有侵权请联系客服删除