双目摄像头立体成像（二）畸变矫正与立体校正

一、立体校正的原因

**原因一：**当畸变系数和内外参数矩阵标定完成后，就应该进行畸变矫正，以达到消除畸变的目的。

**原因二：**在立体成像原理中提到，要通过两幅图像估计物点的深度信息，就必须在两幅图像中准确的匹配到同一物点，这样才能根据该物点在两幅图像中的位置关系，计算物体深度。
为了降低匹配的计算量，两个摄像头的成像平面应处于同一平面。但是，单单依靠严格的摆放摄像头来达到这个目的显然有些困难。立体校正就是利用几何图形变换(Geometric Image Transformation)关系，使得原先不满足上述位置关系的两幅图像满足该条件。

二、畸变校正（compensate lens distortion）和立体校正（stereo rectify）的数学原理
（一）畸变校正（compensate lens distortion）
畸变矫正的方法就是用上一篇博文给出的公式对像素位置进行重新映射。这里重新写出重新映射的公式。
先矫正径向畸变

再矫正切向畸变

（二）立体校正（stereo rectify）
立体矫正能够有效降低立体匹配的计算量，立体矫正的具体作用见下图，

立体矫正前，

立体矫正后，

立体矫正的算法原理没有详细了解，此处从略。

OpenCV相关函数说明

一、畸变矫正函数 undistort( )

undistort() 是独立的一个畸变矫正函数，一次性可以完成映射矩阵的求解和重新映射。下面我们还会看到把这两步分开来做的函数。
调用方法：略
参数介绍：
1. src-输入未经过矫正的图像
2. dst-经过矫正后输出的图像
3. cameraMatrix-标定而得到的摄像机矩阵
4. distCoeffs-标定得到的摄像机畸变矩阵
5. newCameraMatrix-输入矫正后的摄像机矩阵（可以省略）

二、立体标定函数 stereoCalibrate()

stereoCalibrate( ) 是用来标定一个立体摄像头的，也就是同时标定两个摄像头。标定的结果除了能够求出两个摄像头的内外参数矩阵，跟能够得出两个摄像头的位置关系R,T。

调用方法：

double stereoCalibrate(
 InputArrayOfArrays objectPoints,
 InputArrayOfArrays imagePoints1, 
 InputArrayOfArrays imagePoints2, 
 InputOutputArray cameraMatrix1,
 InputOutputArray distCoeffs1, 
 InputOutputArray cameraMatrix2, 
 InputOutputArray distCoeffs2, 
 Size imageSize, OutputArray R,OutputArray T, OutputArray E, OutputArray F, 
TermCriteria criteria=TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6), int flags=CALIB_FIX_INTRINSIC )

参数介绍：

1. objectPoints- vector 型的数据结构，存储标定角点在世界坐标系中的位置。
2. imagePoints1- vector<vector> 型的数据结构，存储标定角点在第一个摄像机下的投影后的亚像素坐标。
3. imagePoints2- vector<vector> 型的数据结构，存储标定角点在第二个摄像机下的投影后的亚像素坐标。
4. cameraMatrix1-输入/输出型的第一个摄像机的相机矩阵。如果CV_CALIB_USE_INTRINSIC_GUESS , CV_CALIB_FIX_ASPECT_RATIO ,CV_CALIB_FIX_INTRINSIC , or CV_CALIB_FIX_FOCAL_LENGTH其中的一个或多个标志被设置，该摄像机矩阵的一些或全部参数需要被初始化
5. distCoeffs1-第一个摄像机的输入/输出型畸变向量。根据矫正模型的不同，输出向量长度由标志决定
6. cameraMatrix2-输入/输出型的第二个摄像机的相机矩阵。参数意义同第一个相机矩阵相似
7. distCoeffs2-第一个摄像机的输入/输出型畸变向量。根据矫正模型的不同，输出向量长度由标志决定
8. imageSize-图像的大小
9. R-输出型，第一和第二个摄像机之间的旋转矩阵
10. T-输出型，第一和第二个摄像机之间的平移矩阵
11. E-输出型，基本矩阵
12. F-输出型，基础矩阵
13. term_crit-迭代优化的终止条件
14. flag-
    14.1 CV_CALIB_FIX_INTRINSIC 如果该标志被设置，那么就会固定输入的cameraMatrix和distCoeffs不变，只求解 $R,T,E,F
    $.
    14.2 CV_CALIB_USE_INTRINSIC_GUESS 根据用户提供的cameraMatrix和distCoeffs为初始值开始迭代
    14.3 CV_CALIB_FIX_PRINCIPAL_POINT 迭代过程中不会改变主点的位置
    14.4. CV_CALIB_FIX_FOCAL_LENGTH 迭代过程中不会改变焦距
    14.5 CV_CALIB_SAME_FOCAL_LENGTH 强制保持两个摄像机的焦距相同
    14.6 CV_CALIB_ZERO_TANGENT_DIST 切向畸变保持为零
    14.7 CV_CALIB_FIX_K1,…,CV_CALIB_FIX_K6 迭代过程中不改变相应的值。如果设置了 CV_CALIB_USE_INTRINSIC_GUESS 将会使用用户提供的初始值，否则设置为零
    14.8 CV_CALIB_RATIONAL_MODEL 畸变模型的选择，如果设置了该参数，将会使用更精确的畸变模型，distCoeffs的长度就会变成8

三、立体校正函数 stereoRectify()

stereoRectify() 的作用是为每个摄像头计算立体校正的映射矩阵。所以其运行结果并不是直接将图片进行立体矫正，而是得出进行立体矫正所需要的映射矩阵。

调用方法:

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(), Rect* validPixROI1=0, Rect* validPixROI2=0 )

参数介绍：

1. cameraMatrix1-第一个摄像机的摄像机矩阵
2. distCoeffs1-第一个摄像机的畸变向量
3. cameraMatrix2-第二个摄像机的摄像机矩阵
4. distCoeffs2-第二个摄像机的畸变向量
5. imageSize-图像大小
6. R- stereoCalibrate() 求得的R矩阵
7. T- stereoCalibrate() 求得的T矩阵
8. R1-输出矩阵，第一个摄像机的校正变换矩阵（旋转矩阵）
9. R2-输出矩阵，第二个摄像机的校正变换矩阵（旋转矩阵）
10. P1-输出矩阵，第一个摄像机在新坐标系下的投影矩阵
11. P2-输出矩阵，第二个摄像机在想坐标系下的投影矩阵
12. Q-4*4的深度差异映射矩阵
13. flags-可选的标志有两种零或者 CV_CALIB_ZERO_DISPARITY ,如果设置 CV_CALIB_ZERO_DISPARITY 的话，该函数会让两幅校正后的图像的主点有相同的像素坐标。否则该函数会水平或垂直的移动图像，以使得其有用的范围最大
14. alpha-拉伸参数。如果设置为负或忽略，将不进行拉伸。如果设置为0，那么校正后图像只有有效的部分会被显示（没有黑色的部分），如果设置为1，那么就会显示整个图像。设置为0~1之间的某个值，其效果也居于两者之间。
15. newImageSize-校正后的图像分辨率，默认为原分辨率大小。
16. validPixROI1-可选的输出参数，Rect型数据。其内部的所有像素都有效
17. validPixROI2-可选的输出参数，Rect型数据。其内部的所有像素都有效

四、映射变换计算函数 initUndistortRectifyMap()

该函数功能是计算畸变矫正和立体校正的映射变换。

调用方法：

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

调用方法：

1. cameraMatrix-摄像机参数矩阵
2. distCoeffs-畸变参数矩阵
3. R- stereoCalibrate() 求得的R矩阵
4. newCameraMatrix-矫正后的摄像机矩阵（可省略）
5. Size-没有矫正图像的分辨率
6. m1type-第一个输出映射的数据类型，可以为 CV_32FC1 或 CV_16SC2
7. map1-输出的第一个映射变换
8. map2-输出的第二个映射变换

五、像素重映射函数 remap()

调用方法：

void remap(InputArray src, OutputArray dst, InputArray map1, InputArray map2, int interpolation,int borderMode=BORDER_CONSTANT, 
const Scalar&borderValue=Scalar())

第五个参数：插值方式，有四中插值方式：

1. src-输入图像，即原图像，需要单通道8位或者浮点类型的图像。
2. dst-输出图像，即目标图像，需和原图形一样的尺寸和类型。
3. map1- x 映射表 ，它有两种可能表示的对象：（1）表示点（x,y）的第一个映射；（2）表示CV_16SC2，CV_32FC1等
4. map2- y 映射表 ，它有两种可能表示的对象：。（1）若map1表示点（x,y）时，这个参数不代表任何值；（2）表示 CV_16UC1，CV_32FC1类型的Y值。
5. interpolation-选择的插值方法，常见线性插值INTER_LINEAR，可选择立方等 ，但是不支持最近邻插值。
   （1）INTER_NEAREST——最近邻插值
   （2）INTER_LINEAR——双线性插值（默认）
   （3）INTER_CUBIC——双三样条插值（默认）
   （4）INTER_LANCZOS4——lanczos插值（默认）
6. 边界模式，默认BORDER_CONSTANT
7. 边界颜色，默认Scalar()黑色

基于OpenCV的仿真

仿真程序

int main()
{
    //initialize some parameters
    bool okcalib = false;
    Mat intrMatFirst, intrMatSec, distCoeffsFirst, distCoffesSec;
    Mat R, T, E, F, RFirst, RSec, PFirst, PSec, Q;
    vector<vector<Point2f>> imagePointsFirst, imagePointsSec;
    vector<vector<Point3f>> ObjectPoints(1);
    Rect validRoi[2];
    Size imageSize;
    int cameraIdFirst = 0, cameraIdSec = 1;
    double rms = 0;

    //get pictures and calibrate
    vector<string> imageList;
    string filename = "stereo_calib.xml";
    bool okread = readStringList(filename, imageList);
    if (!okread || imageList.empty())
    {
        cout << "can not open " << filename << " or the string list is empty" << endl;
        return false;
    }
    if (imageList.size() % 2 != 0)
    {
        cout << "Error: the image list contains odd (non-even) number of elements\n";
        return false;
    }

    //calibrate
    cout << "calibrate left camera..." << endl;
    okcalib = calibrate(intrMatFirst, distCoeffsFirst, imagePointsFirst, ObjectPoints,
        imageSize, cameraIdFirst, imageList);

    if (!okcalib)
    {
        cout << "fail to calibrate left camera" << endl;
        return -1;
    }
    else
    {
        cout << "calibrate the right camera..." << endl;
    }

    okcalib = calibrate(intrMatSec, distCoffesSec, imagePointsSec, ObjectPoints,
        imageSize, cameraIdSec, imageList);

    if (!okcalib)
    {
        cout << "fail to calibrate the right camera" << endl;
        return -1;
    }
    destroyAllWindows();

    //estimate position and orientation
    cout << "estimate position and orientation of the second camera" << endl
        << "relative to the first camera..." << endl;
    rms = stereoCalibrate(ObjectPoints, imagePointsFirst, imagePointsSec,
        intrMatFirst, distCoeffsFirst, intrMatSec, distCoffesSec,
        imageSize, R, T, E, F, CV_CALIB_FIX_INTRINSIC,
        TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, 1e-6));
    cout << "done with RMS error=" << rms << endl;

    //stereo rectify
    cout << "stereo rectify..." << endl;
    stereoRectify(intrMatFirst, distCoeffsFirst, intrMatSec, distCoffesSec, imageSize, R, T, RFirst,
        RSec, PFirst, PSec, Q, 0, 1, imageSize, &validRoi[0], &validRoi[1]);

    //read pictures for 3d-reconstruction
    namedWindow("canvas", 1);
    cout << "read the picture for 3d-reconstruction...";
    Mat canvas(imageSize.height, imageSize.width * 2, CV_8UC3), viewLeft, viewRight;
    Mat canLeft = canvas(Rect(0, 0, imageSize.width, imageSize.height));
    Mat canRight = canvas(Rect(imageSize.width, 0, imageSize.width, imageSize.height));
    viewLeft = imread(imageList[cameraIdFirst], 1);
    viewRight = imread(imageList[cameraIdSec], 1);
    viewLeft.copyTo(canLeft);
    viewRight.copyTo(canRight);
    cout << "done" << endl;
    imshow("canvas", canvas);
    waitKey(50);

    //stereoRectify
    Mat rmapFirst[2], rmapSec[2], rviewFirst, rviewSec;
    initUndistortRectifyMap(intrMatFirst, distCoeffsFirst, RFirst, PFirst,
        imageSize, CV_16SC2, rmapFirst[0], rmapFirst[1]);
    initUndistortRectifyMap(intrMatSec, distCoffesSec, RSec, PSec,
        imageSize, CV_16SC2, rmapSec[0], rmapSec[1]);
    remap(viewLeft, rviewFirst, rmapFirst[0], rmapFirst[1], INTER_LINEAR);
    remap(viewRight, rviewSec, rmapSec[0], rmapSec[1], INTER_LINEAR);
    rviewFirst.copyTo(canLeft);
    rviewSec.copyTo(canRight);

    rectangle(canLeft, validRoi[0], Scalar(255, 0, 0), 3, 8);
    rectangle(canRight, validRoi[1], Scalar(255, 0, 0), 3, 8);
    for (int j = 0; j <= canvas.rows; j += 16)
        line(canvas, Point(0, j), Point(canvas.cols, j), Scalar(0, 255, 0), 1, 8);
    cout << "stereo rectify done" << endl;
    imshow("canvas", canvas);
    waitKey(50);

子函数calibrate()和calcChessboardCorners()分别是用来表达相机和计算objectPoints的。函数体如下，

bool calibrate(Mat& intrMat, Mat& distCoeffs, vector<vector<Point2f>>& imagePoints, vector<vector<Point3f>>& ObjectPoints, Size& imageSize,const int cameraId , 
vector<string> imageList)
{
int w = 6;
 int h = 9;
double rms = 0;

Size boardSize;
boardSize.width = w;
boardSize.height = h;
vector<Point2f> pointBuf;
float squareSize = 1.f;
vector<Mat> rvecs, tvecs;
bool ok = false;

int nImages = (int)imageList.size() / 2;
namedWindow("View", 1);
for (int i = 0; i<nImages ; i++)
 {
Mat view, viewGray;
view = imread(imageList[i*2+cameraId], 1);
imageSize = view.size();
cvtColor(view, viewGray, COLOR_BGR2GRAY);

bool found = findChessboardCorners(view, boardSize, pointBuf,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);

if (found)
{
 cornerSubPix(viewGray, pointBuf, Size(11, 11),
Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
drawChessboardCorners(view, boardSize, Mat(pointBuf), found);
 bitwise_not(view, view);
imagePoints.push_back(pointBuf);
cout << '.';
}
imshow("View", view);
waitKey(50);
}
//calculate chessboardCorners
calcChessboardCorners(boardSize, squareSize, ObjectPoints[0]);
ObjectPoints.resize(imagePoints.size(), ObjectPoints[0]);

rms = calibrateCamera(ObjectPoints, imagePoints, imageSize, intrMat, distCoeffs,
rvecs, tvecs);
ok = checkRange(intrMat) && checkRange(distCoeffs);

if (ok)
 {
 cout << "done with RMS error=" << rms << endl;
return true;
 }
 else
 return false;
 }

static void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners)
{
 corners.resize(0);
for (int i = 0; i < boardSize.height; i++)        //height和width位置不能颠倒
   for (int j = 0; j < boardSize.width; j++)
    {
       corners.push_back(Point3f(j*squareSize, i*squareSize, 0));
    }
}

仿真结果

转载自：https://www.cnblogs.com/german-iris/p/5199203.html