OpenCV相机标定和姿态更新

原帖地址:

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

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

这一节我们首先介绍下计算机视觉领域中常见的三个坐标系:图像坐标系,相机坐标系,世界坐标系以及他们之间的关系,然后介绍如何使用张正友相机标定法标定相机。

图像坐标系:

理想的图像坐标系原点O1和真实的O0有一定的偏差,由此我们建立了等式(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)algorithmPOSIT算法的原理--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解法(传统迭代,最小化重投影误差)。

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

posted on 2016-08-07 17:13  迈克老狼2012  阅读(27333)  评论(0编辑  收藏  举报

导航