SLAM学习笔记一基础知识：双目标定，去畸变，立体匹配，视差图

1，摄像头

使用的是自己用FPGA做的双目，1280*720p@60hz,9轴IMU，以太网接口，硬件帧同步,优于1us，全局快门AR0134,输出彩色，灰度图像，可配640*480@80hz,1280*960@50hz,自动曝光触发，外部曝光触发。

2，matlab工具箱标定双目

matlab工具箱的标定方法要比opencv精确，可以人工挑选匹配图片。

拍摄100张左右标定板图片，两个摄像头对应的图片名要相同，分别放到两个文件夹。

注意事项： 
1. 采集棋盘图的时候要注意，尽量让棋盘占据尽可能多的画面，这样可以得到更多有关摄像头畸变方面的信息。 
2. 要让棋盘左右旋转，计算两个相机之间的变换矩阵。 
3. 两个摄像头的焦距应该保持一致，因为在后续的视差图转换为三维图时的Q矩阵只有一个f值。所以必须要求至少焦距相近。而且立体成像的三角测量（Learning OpenCV书中提到）的前提假设就是fl=fr。（调整两个摄像头的焦距相同的方法：离两个相机相同远处放置标定板，分别调节两个相机的焦距，使得两个画面的清晰度相似。）

 

file:///home/teemos/%E4%B8%8B%E8%BD%BD/webwxgetmsgimg.jpeg

其中，可能会出现很多误匹配的图片，把左面两两对应的图片检查一下，XY方向是否相同，角点是否检测完，（0，0）点是否一样。有误差的，右键移除。

 

8，标定参数的使用

一共有6个参数供opencv调用，摄像头1内参K,摄像头1畸变，摄像头2内参K,摄像头2畸变，摄像头2相对于摄像头1的旋转矩阵R,平移向量T,

R,T如下:其中R需要转置一下才能给opencv用

摄像头内参K，畸变如下，摄像头的内参也需要转置。 RadialDistortion为径向畸变K1，K2，K3，TangentialDistortion为切向畸变P1，P2，opencv调用时，写成K1，K2，P1，P2，K3的形式。

3，双目校正

双目校正的作用就是要把消除畸变后的两幅图像严格地行对应，使得两幅图像的对极线恰好在同一水平线上，这样一幅图像上任意一点与其在另一幅图像上的对应点就必然具有相同的行号，只需在该行进行一维搜索即可匹配到对应点。

 

1，使用matlab标定的参数

2，调用stereoRectify

stereoRectify(cameraMatrixL, distCoeffL,
	cameraMatrixR, distCoeffR,
	imageSize, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, -1, imageSize, &validRoiL, &validRoiR);

其中cameraMatrixL, distCoeffL,  cameraMatrixR, distCoeffR, R, T皆是输入的相机参数，通过stereoRectify得出校正旋转矩阵R、投影矩阵P、重投影矩阵Q，其中参数-1会输出完整的去畸变图像，输入0会自动裁剪图像。

调用initUndistortRectifyMap得出校准映射参数

initUndistortRectifyMap(cameraMatrixL, distCoeffL, R1, P1, imageSize, CV_32FC1, rmap[0][0], rmap[0][1]);

调用remap校准输入的左右图像

remap(Image1, rectifyImageL, rmap[0][0], rmap[0][1], INTER_LINEAR);

双目矫正程序：

#include 
#include "opencv2/calib3d.hpp"
#include "opencv2/imgcodecs.hpp"
#include 
#include"opencv2/imgproc.hpp"
#include"opencv2/highgui.hpp"
//#include"opencv2/core.hpp"
using namespace cv;
using namespace std;

int main()
{
	const int imageWidth = 1280;                             //摄像头的分辨率
	const int imageHeight = 720;
	Size imageSize = Size(imageWidth, imageHeight);
	Mat R1, R2, P1, P2, Q;
	Mat rectifyImageL, rectifyImageR;
	Rect validRoiL;
	Rect validRoiR;
	Mat cameraMatrixL = (Mat_(3, 3) << 815.1796, -28.6083, 660.3101,
		0, 821.6495, 413.9831,
		0, 0, 1);
	Mat distCoeffL = (Mat_(5, 1) << -0.3827, 0.1749, -0.0105, -0.0122, -0.0854);

	Mat cameraMatrixR = (Mat_(3, 3) << 812.8009, -28.3190, 653.3446,
		0, 820.1620, 386.9071,
		0, 0, 1);
	Mat distCoeffR = (Mat_(5, 1) << -0.3823, 0.1593, -0.0618, -0.0113, -0.0158);

	Mat T = (Mat_(3, 1) << -125.1593, 1.0114, 1.7302);//T平移向量
	Mat R = (Mat_(3, 3) << 0.9993, -0.0014, 0.0373,
		0.0012, 1.0000, 0.0056,
		-0.0373, -0.0056, 0.9993);//rec旋转向量

	stereoRectify(cameraMatrixL, distCoeffL,
	cameraMatrixR, distCoeffR,
	imageSize, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY,0 , imageSize, &validRoiL, &validRoiR);
	Mat rmap[2][2];
	initUndistortRectifyMap(cameraMatrixL, distCoeffL, R1, P1, imageSize, CV_32FC1, rmap[0][0], rmap[0][1]);
	initUndistortRectifyMap(cameraMatrixR, distCoeffR, R2, P2, imageSize, CV_32FC1, rmap[1][0], rmap[1][1]);
	Mat Image1 = imread("2.jpg", CV_LOAD_IMAGE_COLOR);
	Mat Image2 = imread("3.jpg", CV_LOAD_IMAGE_COLOR);
	//cvtColor(rgbImageL, grayImageL, CV_BGR2GRAY);
	remap(Image1, rectifyImageL, rmap[0][0], rmap[0][1], INTER_LINEAR);
	remap(Image2, rectifyImageR, rmap[1][0], rmap[1][1], INTER_LINEAR);
	imshow("IMAGE1", rectifyImageL);
	imshow("IMAGE2", rectifyImageR);
	waitKey(0);

}

3.立体匹配

由极线约束在两幅图片去找对应点，由对应点计算视差d,由视差d带入如下公式计算对应点的深度距离。由视差图-》深度图

BM算法 ：

采用Block Matching算法进行立体匹配，Block Matching用的是SAD方法，速度比较快，但效果一般。 
参数设置： 
MinDisparity设置为0，因为两个摄像头是前向平行放置，相同的物体在左图中一定比在右图中偏右。如果为了追求更大的双目重合区域而将两个摄像头向内偏转的话，这个参数是需要考虑的。 
UniquenessRatio主要可以防止误匹配，此参数对于最后的匹配结果是有很大的影响。立体匹配中，宁愿区域无法匹配，也不要误匹配。如果有误匹配的话，碰到障碍检测这种应用，就会很麻烦。该参数不能为负值，一般5-15左右的值比较合适，int型。 
BlockSize：SAD窗口大小，容许范围是[5,255]，一般应该在 5x5..21x21 之间，参数必须为奇数值, int型。 
NumDisparities：视差窗口，即最大视差值与最小视差值之差,窗口大小必须是 16的整数倍，int型。 
在BM算法的参数中，对视差生成效果影响较大的主要参数是BlockSize、NumDisparities和UniquenessRatio三个，一般只需对这三个参数进行调整，其余参数按默认设置即可。 

BM算法计算出的视差disp是CV_16S格式，通过disp.convertTo(disp8, CV_8U, 255/(numberOfDisparities*16.))变换才能得到真实的视差值。 
然后通过reprojectImageTo3D这个函数将视差矩阵转换成实际的物理坐标矩阵。在实际求距离时，reprojectImageTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。 

SGBM算法：

SGBM参数设置：（在下面的程序中已经标明） MinDisparity设置为0，因为两个摄像头是前向平行放置，相同的物体在左图中一定比在右图中偏右。如果为了追求更大的双目重合区域而将两个摄像头向内偏转的话，这个参数是需要考虑的。 UniquenessRatio主要可以防止误匹配，此参数对于最后的匹配结果是有很大的影响。立体匹配中，宁愿区域无法匹配，也不要误匹配。如果有误匹配的话，碰到障碍检测这种应用，就会很麻烦。该参数不能为负值，一般5-15左右的值比较合适，int型。 BlockSize：SAD窗口大小，容许范围是[5,255]，一般应该在 5x5..21x21 之间，参数必须为奇数值, int型。 NumDisparities：视差窗口，即最大视差值与最小视差值之差,窗口大小必须是 16的整数倍，int型。 在SGBM算法的参数中，对视差生成效果影响较大的主要参数是BlockSize、NumDisparities和UniquenessRatio三个，一般只需对这三个参数进行调整，其余参数按默认设置即可。 --------------------- 本文来自 小红ML 的CSDN 博客 ，全文地址请点击：https://blog.csdn.net/weixin_39449570/article/details/79033314?utm_source=copy

参考博客：

 

https://blog.csdn.net/wangchao7281/article/details/52506691?locationNum=7

https://blog.csdn.net/yanli33/article/details/60478023