【一起学opencv】双目测距、定标 + 矫正
书接上回。我们用相机拍完照之后就要进行相机标定了，双目测距只有在相机标定之后可以发挥比较好的效果。
我们要做一些准备工作，首先就是下载matlab并且去下载棋盘照片打印出来进行标定
棋盘图片下载网址：http://docs.opencv.org/2.4/_downloads/pattern.png
注：要注意棋盘一定要固定在一个平面上，不要弯曲卷折。
我所用到的是matlab里的标定工具箱，首先打开matlab，在命令行输入stereoCameraCalibrator进入工具箱，出现如下界面。
我们首先点击左上角的Add image添加图片
会出现如下界面
我们点击Browse 选取两个摄像头所拍的照片，我用的摄像头camera1是左画面，camera2是右画面，大家根据自己的摄像头自己选择图片路径。默认的棋盘格子大小是20mm*20mm。可以根据你下载的棋盘格子大小自由修改。修完完成之后点击确定，工具箱会自动匹配和选择合适的照片。
工具箱运行中
工具箱结果
工具箱会弹窗告诉你一共识别到多少组照片，多少组可以用，多少组被工具箱拒绝了。
我这里一共36组照片，20组照片可以使用，16组被拒绝。点击确定即可。
确定之后可以得到这样的画面，我们下一步需要点击工具栏右侧绿色的Calibrate按钮。
运行完成之后会在界面下方出现如下窗口：
从图中可以看到，平均的标定误差以及标定过程中误差较大的的图像对，以及图片的位置信息。针对左侧误差比较大的图片，我们可以选择误差大的照片进行删除，删除到误差满足要求为止。左键或右键点击柱状图会跳转到照片，右键remove选中照片即可。
之后点击工具栏的Export Camera Parameters导出摄像头参数，默认参数就可以 点击确定，即完成标定。之后回到matlab命令行界面，我们可以得到如下标定参数
CameraParameters1 与 CameraParameters2 为左右摄像头的内部参数，RotationOfCamera2 与 TranslationOfCamera2 为两个摄像头的旋转、平移参数。
平移参数可直接使用；但旋转参数需进行转置才能使用。
之后我们点击工作区
点击stereoParams查看参数，我们以左相机为例，查看所需的相机参数。
点击CameraParameters1，
IntrinsicMatrix 存放的是摄像头的内部参数
RadialDistortion 和 TangentialDistortion 中存放的是畸变参数（径向畸变和切向畸变）
双击IntrinsicMatrix 得到如下参数
这个和opencv中所使用的为转置关系。如何转置如下图所示，行和列进行互换即可。
RadialDistortion 为 径向畸变，摄像头由于光学透镜的特性使得成像存在着径向畸变，可由 K1、K2、K3 确定。TangentialDistortion 为 切向畸变，由于装配方面的误差，传感器与光学镜头之间并非完全平行，因此成像存在切向畸变，可由两个参数 P1、P2 确定。
在Opencv的使用中，我们的使用顺序是K1、K2、P1、P2、K3（K3的默认值是0）千万要注意顺序不要弄错！
即0.1287,-0.1407,0,0,0
右相机同理可得参数，这里就不再赘述了。
之后我们回到stereoParams界面读取：RotationOfCamera2 （为两个摄像头的旋转参数）
TranslationOfCamera2 （平移参数）。需注意！平移参数可以直接在Opencv中使用，而旋转参数需要进行转置之后使用，与上面转置同理。
到此为止我们摄像头的参数保存完毕，我们打开程序将程序写入即可。
按照批注填入上面标定所得的相机参数。
附源码：
/******************************/
/* 立体匹配和测距 */
/******************************/
#include
#include
#include
using namespace std;
using namespace cv;
const int imageWidth = 640; //摄像头的分辨率
const int imageHeight = 360;
Vec3f point3;
float d;
Size imageSize = Size(imageWidth, imageHeight);
Mat rgbImageL, grayImageL;
Mat rgbImageR, grayImageR;
Mat rectifyImageL, rectifyImageR;
Rect validROIL;//图像校正之后，会对图像进行裁剪，这里的validROI就是指裁剪之后的区域
Rect validROIR;
Mat mapLx, mapLy, mapRx, mapRy; //映射表
Mat Rl, Rr, Pl, Pr, Q; //校正旋转矩阵R，投影矩阵P 重投影矩阵Q
Mat xyz; //三维坐标
Point origin; //鼠标按下的起始点
Rect selection; //定义矩形选框
bool selectObject = false; //是否选择对象
int blockSize = 0, uniquenessRatio = 0, numDisparities = 0;
Ptr
/*事先标定好的左相机的内参矩阵
fx 0 cx
0 fy cy
0 0 1
*/
Mat cameraMatrixL = (Mat_
0, 485.273502858935, 214.517060356564,
0, 0, 1);
//获得的畸变参数 左相机畸变参数
/*418.523322187048 0 0
-1.26842201390676 421.222568242056 0
344.758267538961 243.318992284899 1 */ //2
Mat distCoeffL = (Mat_
//[0.006636837611004,0.050240447649195] [0.006681263320267,0.003130367429418] //左相机K1,K2,P1,P2,K3
/*事先标定好的右相机的内参矩阵
fx 0 cx
0 fy cy
0 0 1
*/
Mat cameraMatrixR = (Mat_
0, 489.272059352339, 212.644573601761,
0, 0, 1); //右相机畸变参数
/*
417.417985082506 0 0
0.498638151824367 419.795432389420 0
309.903372309072 236.256106972796 1
*/ //2
Mat distCoeffR = (Mat_
//[-0.038407383078874,0.236392800301615] [0.004121779274885,0.002296129959664]//右相机K1,K2,P1,P2,K3
Mat T = (Mat_
//[-1.210187345641146e+02,0.519235426836325,-0.425535566316217]
//对应Matlab所得T参数 TranslationOfCamera2
//Mat rec = (Mat_
Mat rec = (Mat_
-0.000388657271549596, 0.999999903041699, 0.000207031685706618,
-0.000835160728832328, -0.000207356220449212, 0.999999629754909); //rec旋转向量，对应matlab om参数 我
//RotationOfCamera2
/* 0.999341122700880 0.000660748031451783 -0.0362888948713456
-0.00206388651740061 0.999250989651683 -0.0386419468010579
0.0362361815232777 0.0386913826603732 0.998593969567432 */
//Mat T = (Mat_
//[-1.210187345641146e+02,0.519235426836325,-0.425535566316217]
//对应Matlab所得T参数
//Mat rec = (Mat_
Mat R;//R 旋转矩阵
/*****立体匹配*****/
void stereo_match(int, void*)
{
bm->setBlockSize(2 * blockSize + 5); //SAD窗口大小，5~21之间为宜
bm->setROI1(validROIL);
bm->setROI2(validROIR);
bm->setPreFilterCap(31);
bm->setMinDisparity(0); //最小视差，默认值为0, 可以是负值，int型
bm->setNumDisparities(numDisparities * 16 + 16);//视差窗口，即最大视差值与最小视差值之差,窗口大小必须是16的整数倍，int型
bm->setTextureThreshold(10);
bm->setUniquenessRatio(uniquenessRatio);//uniquenessRatio主要可以防止误匹配
bm->setSpeckleWindowSize(100);
bm->setSpeckleRange(32);
bm->setDisp12MaxDiff(-1);
Mat disp, disp8;
bm->compute(rectifyImageL, rectifyImageR, disp);//输入图像必须为灰度图
disp.convertTo(disp8, CV_8U, 255 / ((numDisparities * 16 + 16)*16.));//计算出的视差是CV_16S格式
reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
xyz = xyz * 16;
imshow("disparity", disp8);
}
/*****描述：鼠标操作回调*****/
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
point3 = xyz.at
point3[0];
//cout << "point3[0]:" << point3[0] << "point3[1]:" << point3[1] << "point3[2]:" << point3[2]< cout << "世界坐标：" << endl; cout << "x: " << point3[0] << " y: " << point3[1] << " z: " << point3[2] << endl; d = point3[0] * point3[0] + point3[1] * point3[1] + point3[2] * point3[2]; d = sqrt(d); //mm // cout << "距离是:" << d << "mm" << endl; d = d / 10.0; //cm cout << "距离是:" << d << "cm" << endl; // d = d/1000.0; //m // cout << "距离是:" << d << "m" << endl; break; case EVENT_LBUTTONUP: //鼠标左按钮释放的事件 selectObject = false; if (selection.width > 0 && selection.height > 0) break; } } /*****主函数*****/ int main() { /* 立体校正 */ Rodrigues(rec, R); //Rodrigues变换 stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY, 0, imageSize, &validROIL, &validROIR); initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_32FC1, mapLx, mapLy); initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy); /* 打开摄像头 */ VideoCapture cap; cap.open(0); //打开相机，电脑自带摄像头一般编号为0，外接摄像头编号为1，主要是在设备管理器中查看自己摄像头的编号。 cap.set(CV_CAP_PROP_FRAME_WIDTH, 2560); //设置捕获视频的宽度 cap.set(CV_CAP_PROP_FRAME_HEIGHT, 720); //设置捕获视频的高度 if (!cap.isOpened()) //判断是否成功打开相机 { cout << "摄像头打开失败!" << endl; return -1; } Mat frame, frame_L, frame_R; cap >> frame; //从相机捕获一帧图像 cout << "Painted ImageL" << endl; cout << "Painted ImageR" << endl; while (1) { double fScale = 0.5; //定义缩放系数，对2560*720图像进行缩放显示（2560*720图像过大，液晶屏分辨率较小时，需要缩放才可完整显示在屏幕） Size dsize = Size(frame.cols*fScale, frame.rows*fScale); Mat imagedst = Mat(dsize, CV_32S); resize(frame, imagedst, dsize); char image_left[200]; char image_right[200]; frame_L = imagedst(Rect(0, 0, 640, 360)); //获取缩放后左Camera的图像 // namedWindow("Video_L", 1); // imshow("Video_L", frame_L); frame_R = imagedst(Rect(640, 0, 640, 360)); //获取缩放后右Camera的图像 // namedWindow("Video_R", 2); // imshow("Video_R", frame_R); cap >> frame; /* 读取图片 */ //rgbImageL = imread("image_left_1.jpg", CV_LOAD_IMAGE_COLOR); cvtColor(frame_L, grayImageL, CV_BGR2GRAY); //rgbImageR = imread("image_right_1.jpg", CV_LOAD_IMAGE_COLOR); cvtColor(frame_R, grayImageR, CV_BGR2GRAY); // imshow("ImageL Before Rectify", grayImageL); // imshow("ImageR Before Rectify", grayImageR); /* 经过remap之后，左右相机的图像已经共面并且行对准了 */ remap(grayImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR); remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR); /* 把校正结果显示出来 */ Mat rgbRectifyImageL, rgbRectifyImageR; cvtColor(rectifyImageL, rgbRectifyImageL, CV_GRAY2BGR); //伪彩色图 cvtColor(rectifyImageR, rgbRectifyImageR, CV_GRAY2BGR); //单独显示 //rectangle(rgbRectifyImageL, validROIL, Scalar(0, 0, 255), 3, 8); //rectangle(rgbRectifyImageR, validROIR, Scalar(0, 0, 255), 3, 8); // imshow("ImageL After Rectify", rgbRectifyImageL); // imshow("ImageR After Rectify", rgbRectifyImageR); //显示在同一张图上 Mat canvas; double sf; int w, h; sf = 600. / MAX(imageSize.width, imageSize.height); w = cvRound(imageSize.width * sf); h = cvRound(imageSize.height * sf); canvas.create(h, w * 2, CV_8UC3); //注意通道 //左图像画到画布上 Mat canvasPart = canvas(Rect(w * 0, 0, w, h)); //得到画布的一部分 resize(rgbRectifyImageL, 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(rgbRectifyImageR, 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); /* 立体匹配 */ namedWindow("disparity", CV_WINDOW_AUTOSIZE); // 创建SAD窗口 Trackbar createTrackbar("BlockSize:\n", "disparity", &blockSize, 8, stereo_match); // 创建视差唯一性百分比窗口 Trackbar createTrackbar("UniquenessRatio:\n", "disparity", &uniquenessRatio, 50, stereo_match); // 创建视差窗口 Trackbar createTrackbar("NumDisparities:\n", "disparity", &numDisparities, 16, stereo_match); //鼠标响应函数setMouseCallback(窗口名称, 鼠标回调函数, 传给回调函数的参数，一般取0) setMouseCallback("disparity", onMouse, 0); stereo_match(0, 0); waitKey(10); } //wheil return 0; } 懒得附图了，就附上一个自己在B站写的专栏吧。还请大家多多提意见。 作者：秋雨又秋雨 https://www.bilibili.com/read/cv16114946 出处：bilibili
运行所得到的效果图，需要自行调节上方的三个参数，直到你想标定的物品轮廓与周边区分开来，左键点击窗口
在cmd窗口可以得到世界坐标距离等参数。