OpenCV相機標定和姿態更新

原帖地址：

http://blog.csdn.net/aptx704610875/article/details/48914043

http://blog.csdn.net/aptx704610875/article/details/48915149

這一節咱們首先介紹下計算機視覺領域中常見的三個座標系：圖像座標系，相機座標系，世界座標系以及他們之間的關係，而後介紹如何使用張正友相機標定法標定相機。

圖像座標系：

理想的圖像座標系原點O₁和真實的O₀有必定的誤差，由此咱們創建了等式（1）和（2），能夠用矩陣形式（3）表示。

相機座標系（C）和世界座標系（W）：

經過相機與圖像的投影關係，咱們獲得了等式（4）和等式（5），能夠用矩陣形式（6）表示。

咱們又知道相機座標系和世界座標的關係能夠用等式（7）表示：

由等式（3），等式（6）和等式（7）咱們能夠推導出圖像座標系和世界座標系的關係：

其中M1稱爲相機的內參矩陣，包含內參（fx，fy，u0，v0）。M2稱爲相機的外參矩陣，包含外參（R:旋轉矩陣，T:平移矩陣）。

衆所周知，相機鏡頭存在一些畸變，主要是徑向畸變（下圖dr），也包括切向畸變（下圖dt）等。

上圖右側等式中，k1,k2,k3,k4,k5,k6爲徑向畸變，p1,p2爲切向畸變。在OpenCV中咱們使用張正友相機標定法經過10幅不一樣角度的棋盤圖像來標定相機得到相機內參和畸變係數。函數爲calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs，flag)

objectPoints: 一組世界座標系中的3D

imagePoints: 超過10張圖片的角點集合

imageSize: 每張圖片的大小

cameraMatrix: 內參矩陣

distCoeffs: 畸變矩陣(默認得到5個即使參數k1,k2,p1,p2,k3，可修改)

rvecs: 外參：旋轉向量

tvecs: 外參：平移向量

flag: 標定時的一些選項：

CV_CALIB_USE_INTRINSIC_GUESS：使用該參數時，在cameraMatrix矩陣中應該有fx,fy,u0,v0的估計值。不然的話，將初始化(u0,v0）圖像的中心點，使用最小二乘法估算出fx，fy。

CV_CALIB_FIX_PRINCIPAL_POINT：在進行優化時會固定光軸點。當CV_CALIB_USE_INTRINSIC_GUESS參數被設置，光軸點將保持在中心或者某個輸入的值。

CV_CALIB_FIX_ASPECT_RATIO：固定fx/fy的比值，只將fy做爲可變量，進行優化計算。當CV_CALIB_USE_INTRINSIC_GUESS沒有被設置，fx和fy將會被忽略。只有fx/fy的比值在計算中會被用到。

CV_CALIB_ZERO_TANGENT_DIST：設定切向畸變參數（p1,p2）爲零。

CV_CALIB_FIX_K1,...,CV_CALIB_FIX_K6：對應的徑向畸變在優化中保持不變。

CV_CALIB_RATIONAL_MODEL：計算k4，k5，k6三個畸變參數。若是沒有設置，則只計算其它5個畸變參數。

首先咱們打開攝像頭並按下'g'鍵開始標定：

VideoCapture cap(1);  
cap.set(CV_CAP_PROP_FRAME_WIDTH,640);  
cap.set(CV_CAP_PROP_FRAME_HEIGHT,480);  
if(!cap.isOpened()){  
    std::cout<<"打開攝像頭失敗，退出";  
    exit(-1);  
}  
namedWindow("Calibration");  
std::cout<<"Press 'g' to start capturing images!"<<endl;   
if( cap.isOpened() && key == 'g' )  
{  
     mode = CAPTURING;  
}

按下空格鍵(SPACE)後使用findChessboardCorners函數在當前幀尋找是否存在可用於標定的角點，若是存在將其提取出來後亞像素化並壓入角點集合，保存當前圖像：

if( (key & 255) == 32 )  
{  
    image_size = frame.size();  
    /* 提取角點 */     
    Mat imageGray;  
    cvtColor(frame, imageGray , CV_RGB2GRAY);  
    bool patternfound = findChessboardCorners(frame, board_size, corners,CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK );  
    if (patternfound)     
    {      
        n++;  
        tempname<<n;  
        tempname>>filename;  
        filename+=".jpg";  
        /* 亞像素精確化 */  
        cornerSubPix(imageGray, corners, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));  
        count += corners.size();  
        corners_Seq.push_back(corners);  
        imwrite(filename,frame);  
        tempname.clear();  
        filename.clear();  
    }  
    else  
    {  
        std::cout<<"Detect Failed.\n";  
    }  
}

角點提取完成後開始標定，首先初始化定標板上角點的三維座標：

for (int t=0;t<image_count;t++)   
{     
       vector<Point3f> tempPointSet;  
        for (int i=0;i<board_size.height;i++)   
    {     
        for (int j=0;j<board_size.width;j++)   
        {     
             /* 假設定標板放在世界座標系中z=0的平面上 */     
            Point3f tempPoint;  
            tempPoint.x = i*square_size.width;  
            tempPoint.y = j*square_size.height;  
            tempPoint.z = 0;  
            tempPointSet.push_back(tempPoint);  
           }     
        }  
    object_Points.push_back(tempPointSet);  
}

使用calibrateCamera函數開始標定：

calibrateCamera(object_Points, corners_Seq, image_size,  intrinsic_matrix  ,distortion_coeffs, rotation_vectors, translation_vectors);

完成定標後對標定進行評價，計算標定偏差並寫入文件：

std::cout<<"每幅圖像的定標偏差："<<endl;     
fout<<"每幅圖像的定標偏差："<<endl<<endl;     
for (int i=0;  i<image_count;  i++)   
{  
    vector<Point3f> tempPointSet = object_Points[i];  
        /****    經過獲得的攝像機內外參數，對空間的三維點進行從新投影計算，獲得新的投影點     ****/  
    projectPoints(tempPointSet, rotation_vectors[i], translation_vectors[i], intrinsic_matrix, distortion_coeffs, image_points2);  
        /* 計算新的投影點和舊的投影點之間的偏差*/    
    vector<Point2f> tempImagePoint = corners_Seq[i];  
    Mat tempImagePointMat = Mat(1,tempImagePoint.size(),CV_32FC2);  
    Mat image_points2Mat = Mat(1,image_points2.size(), CV_32FC2);  
    for (int j = 0 ; j < tempImagePoint.size(); j++)  
    {  
        image_points2Mat.at<Vec2f>(0,j) = Vec2f(image_points2[j].x, image_points2[j].y);  
        tempImagePointMat.at<Vec2f>(0,j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);  
    }  
    err = norm(image_points2Mat, tempImagePointMat, NORM_L2);  
        total_err += err/=  point_counts[i];     
        std::cout<<"第"<<i+1<<"幅圖像的平均偏差："<<err<<"像素"<<endl;     
        fout<<"第"<<i+1<<"幅圖像的平均偏差："<<err<<"像素"<<endl;     
}     
std::cout<<"整體平均偏差："<<total_err/image_count<<"像素"<<endl;     
fout<<"整體平均偏差："<<total_err/image_count<<"像素"<<endl<<endl;     
std::cout<<"評價完成！"<<endl;

顯示標定結果並寫入文件：

std::cout<<"開始保存定標結果………………"<<endl;         
Mat rotation_matrix = Mat(3,3,CV_32FC1, Scalar::all(0)); /* 保存每幅圖像的旋轉矩陣 */     
         
fout<<"相機內參數矩陣："<<endl;     
fout<<intrinsic_matrix<<endl<<endl;     
fout<<"畸變係數：\n";     
fout<<distortion_coeffs<<endl<<endl<<endl;     
for (int i=0; i<image_count; i++)   
{   
        fout<<"第"<<i+1<<"幅圖像的旋轉向量："<<endl;     
        fout<<rotation_vectors[i]<<endl;     
    
        /* 將旋轉向量轉換爲相對應的旋轉矩陣 */     
        Rodrigues(rotation_vectors[i],rotation_matrix);     
        fout<<"第"<<i+1<<"幅圖像的旋轉矩陣："<<endl;     
        fout<<rotation_matrix<<endl;     
        fout<<"第"<<i+1<<"幅圖像的平移向量："<<endl;     
        fout<<translation_vectors[i]<<endl<<endl;     
}     
std::cout<<"完成保存"<<endl;   
fout<<endl;

具體的代碼實現和工程詳見：Calibration

運行截圖：

下一節咱們將使用RPP相機姿態算法獲得相機的外部參數：旋轉和平移。

==============================================================================================

2015/11/14補充：

全部分辨率下的畸變（k1,k2,p1,p2）相同，但內參不一樣（fx,fy,u0,v0），不一樣分辨率下須要從新標定相機內參。如下是羅技C920在1920*1080下的內參：

==============================================================================================

2016/08/20補充：

findChessboardCorners函數的第二個參數是定義棋盤格的橫縱內角點個數，要設置正確，否則函數找不到合適的角點，返回false。以下圖中的橫內角點是12，縱內角點是7，則Size board_size = Size(12, 7);

上一節咱們使用張正友相機標定法得到了相機內參，這一節咱們使用 PnP (Perspective-n-Point)算法估計相機初始姿態並更新之。

推薦3篇我學習的博客：【姿態估計】Pose estimation algorithm 之 Robust Planar Pose (RPP)algorithm，POSIT算法的原理--opencv 3D姿態估計，三維姿態：關於solvePnP與cvPOSIT。

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

2016/6/20

關於PnP問題我會從新寫一篇博客，講一下概念，最少須要幾組對應的3D/2D點，

3D點共面時怎麼處理，PnP有哪些主流解法，以及會更新一篇G2O的PnP解法。

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

注意點1：solvePnP裏有三種解法：P3P， EPnP，迭代法（默認）；opencv2裏參數分別爲CV_P3P，CV_EPNP，CV_ITERATIVE （opencv3裏多了DLS和UPnP解法）。

注意點2：solvePnP須要至少3組點：P3P只使用4組點，3組求出多個解，第四組肯定最優解；EPnP使用大於等於3組點；迭代法調用cvFindExtrinsicCameraParams2，進而使用SVD分解並調用cvFindHomography，而cvFindHomography須要至少4組點。

接下來咱們使用OpenCV實現相機姿態更新：

上一節獲得的相機內參和相機畸變：

double camD[9] = {618.526381968738, 0, 310.8963715614199,  
                    0, 619.4548980786033, 248.6374860176724,  
                    0, 0, 1};  
double distCoeffD[5] = {0.09367405350511771, -0.08731677320554751, 0.002823563134787144, -1.246739177460954e-005, -0.0469061739387372};  
Mat camera_matrix = Mat(3,3,CV_64FC1,camD);  
Mat distortion_coefficients = Mat(5,1,CV_64FC1,distCoeffD);

 

首先檢測ORB角點並亞像素化：

cap >> frame;  
if( frame.empty() )  
    break;  
  
frame.copyTo(image);  
if(needToGetgf)  
{  
    cvtColor(image, gray, COLOR_BGR2GRAY);  
  
    // automatic initialization  
    orb.detect(gray, keypoints);  
    goodfeatures.clear();  
    for( size_t i = 0; i < keypoints.size(); i++ ) {  
        goodfeatures.push_back(keypoints[i].pt);  
    }  
    cornerSubPix(gray, goodfeatures, subPixWinSize, Size(-1,-1), termcrit);  
    for(size_t i = 0; i < goodfeatures.size(); i++ )  
    {  
        circle( image, goodfeatures[i], 3, Scalar(0,255,0), -1, 8);  
    }  
}

 

使用鼠標選定4個2D點（按正方形左上頂點開始順時針），而後查找所選點附近的角點，若找到則壓入跟蹤點集合：

void on_mouse(int event,int x,int y,int flag, void *param)  
{  
    if(event==CV_EVENT_LBUTTONDOWN)  
    {  
        if(needtomap && points[1].size()<4)  
        {  
            for(size_t i = 0;i<goodfeatures.size();i++)  
            {  
                if(abs(goodfeatures[i].x-x)+abs(goodfeatures[i].y-y)<3)  
                {  
                    points[1].push_back(goodfeatures[i]);  
                    trackingpoints++;  
                    break;  
                }  
            }  
        }     
    }  
}

 

創建與2D跟蹤點集合相對應的3D空間點集合：

objP.push_back(Point3f(0,0,0));    //三維座標的單位是毫米  
objP.push_back(Point3f(5,0,0));  
objP.push_back(Point3f(5,5,0));  
objP.push_back(Point3f(0,5,0));  
Mat(objP).convertTo(objPM,CV_32F);

 

使用LK光流法跟蹤已選定角點：

vector<uchar> status;  
vector<float> err;  
if(prevGray.empty())  
    gray.copyTo(prevGray);  
calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err);  
size_t i,k;  
for(i = k = 0; i < points[1].size(); i++ )  
{  
    if( !status[i] )  
        continue;  
    points[1][k++] = points[1][i];  
    circle( image, points[1][i], 3, Scalar(0,0,255), -1, 8);  
}

 

若4個點均跟蹤成功，使用solvePnP計算相機姿態，並使用計算出的相機姿態重畫3D空間點到2D平面查看是否匹配：

if(k == 4)        
    getPlanarSurface(points[0]);
void getPlanarSurface(vector<Point2f>& imgP){  
  
    Rodrigues(rotM,rvec);  
    solvePnP(objPM, Mat(imgP), camera_matrix, distortion_coefficients, rvec, tvec);  
    Rodrigues(rvec,rotM);  
  
    cout<<"rotation matrix: "<<endl<<rotM<<endl;  
    cout<<"translation matrix: "<<endl<<tv[0]<<" "<<tv[1]<<" "<<tv[2]<<endl;  
      
    projectedPoints.clear();  
    projectPoints(objPM, rvec, tvec, camera_matrix, distortion_coefficients, projectedPoints);  
          
    for(unsigned int i = 0; i < projectedPoints.size(); ++i)  
    {  
        circle( image, projectedPoints[i], 3, Scalar(255,0,0), -1, 8);  
    }  
}

 

經過查看cmd中輸出的旋轉矩陣和平移向量以及重畫的2D點，咱們發現solvePnP運行良好。點這裏得到程序源碼

下一節咱們將結合相機外參使用OpenGL畫出AR物體。

============================================================================================================

2015/10/20號補充：

這幾天在作跟蹤恢復的時候須要用給定的2D點和R，T計算3D點，因而從新手算了一邊圖像2D點和空間3D點的關係。過程當中搞懂了爲何PnP計算rotation和translation的時候須要至少3組2D/3D點。

首先來看圖像2D點和空間3D點的關係：

對於R和T展開而且對矩陣相乘展開咱們獲得：

把（3）式帶入（1）式和（2）式，整理得：

Xw * ( fx * R11 + u0 * R31 - x * R31) + Yw * (fx * R12 + u0 * R32 - x * R32) + Zw * (fx * R13 + u0 * R33 - x * R33) = T3 * x - fx * T1 - u0 * T3

Xw * ( fy * R21 + v0 * R31 - y * R31) + Yw * (fy * R22 + v0 * R32 - y * R32) + Zw * (fy * R23 + v0 * R33 - y * R33) = T3 * y - fy * T2 - v0 * T3

咱們能夠看出，fx fy u0 v0是相機內參，上一節中已經求出，Xw Yw x y是一組3D/2D點的座標，因此未知數有R11 R12 R13 R21 R22 R23 R31 R32 R33 T1 T2 T3一共12個，因爲旋轉矩陣是正交矩陣，每行每列都是單位向量且兩兩正交，因此R的自由度爲3，秩也是3，好比知道R11 R12 R21就能求出剩下的Rxx。加上平移向量的3個未知數，一共6個未知數，而每一組2D/3D點提供的x y Xw Yw Zw能夠確立兩個方程，因此3組2D/3D點的座標能確立6個方程從而解出6個未知數。

故PnP須要知道至少3組2D/3D點。

============================================================================================================

2016/1/28號補充：

最近在用平均最小偏差求精準相機姿態的過程當中，須要搞清楚R和t的具體含義。

R的第i行 表示攝像機座標系中的第i個座標軸方向的單位向量在世界座標系裏的座標；
R的第i列 表示世界座標系中的第i個座標軸方向的單位向量在攝像機座標系裏的座標；
t 表示世界座標系的原點在攝像機座標系的座標；
-R的轉置 * t 表示攝像機座標系的原點在世界座標系的座標。（原理以下圖，t表示平移，T表示轉置）

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

2016/6/20

關於PnP問題我會從新寫一篇博客，講一下概念，最少須要幾組對應的3D/2D點，

3D點共面時怎麼處理，PnP有哪些主流解法(P3P, EPnP, DLS,  UPnP, 傳統迭代)，

以及會更新一篇G2O的PnP解法（傳統迭代，最小化重投影偏差）。

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////