雙攝像頭立體成像(三)-畸變矯正與立體校正

畸變矯正是上一篇博文的遺留問題，當畸變系數和內外參數矩陣標定完成后，就應該進行畸變的矯正，以達到消除畸變的目的，此其一。

在該系列第一部分的博文中介紹的立體成像原理中提到，要通過兩幅圖像估計物點的深度信息，就必須在兩幅圖像中准確的匹配到同一物點，這樣才能根據該物點在兩幅圖像中的位置關系，計算物體深度。為了降低匹配的計算量，兩個攝像頭的成像平面應處於同一平面。但是，單單依靠嚴格的擺放攝像頭來達到這個目的顯然有些困難。立體校正就是利用幾何圖形變換(Geometric Image Transformation)關系，使得原先不滿足上述位置關系的兩幅圖像滿足該條件，此其二。

數學原理

• 畸變矯正(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<point3f> 型的數據結構，存儲標定角點在世界坐標系中的位置
2. imagePoints1- vector<vector<point2f>> 型的數據結構，存儲標定角點在第一個攝像機下的投影后的亞像素坐標
3. imagePoints2- vector<vector<point2f>> 型的數據結構，存儲標定角點在第二個攝像機下的投影后的亞像素坐標
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-

    •  CV_CALIB_FIX_INTRINSIC 如果該標志被設置，那么就會固定輸入的cameraMatrix和distCoeffs不變，只求解$$$R,T,E,F$$$.
    •  CV_CALIB_USE_INTRINSIC_GUESS 根據用戶提供的cameraMatrix和distCoeffs為初始值開始迭代
    •  CV_CALIB_FIX_PRINCIPAL_POINT 迭代過程中不會改變主點的位置
    •  CV_CALIB_FIX_FOCAL_LENGTH 迭代過程中不會改變焦距
    •  CV_CALIB_SAME_FOCAL_LENGTH 強制保持兩個攝像機的焦距相同
    •  CV_CALIB_ZERO_TANGENT_DIST 切向畸變保持為零
    •  CV_CALIB_FIX_K1,...,CV_CALIB_FIX_K6 迭代過程中不改變相應的值。如果設置了 CV_CALIB_USE_INTRINSIC_GUESS 將會使用用戶提供的初始值，否則設置為零
    •  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. distCoeffs1-第二個攝像機的畸變向量
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-原圖像
2. dst-幾何變換后的圖像
3. map1-第一個映射，無論是點(x,y)或者單純x的值都需要是CV_16SC2 ,CV_32FC1 , 或 CV_32FC2類型
4. map2-第二個映射，y需要是CV_16UC1 , CV_32FC1類型。或者當map1是點(x,y)時，map2為空。
5. interpolation-插值方法，但是不支持最近鄰插值
6. 剩下兩個我也沒看懂，但是一般示例程序中不會設置

基於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));
    }
}

 

• 仿真結果