原帖地址：
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();
cap.set(CV_CAP_PROP_FRAME_WIDTH,);
cap.set(CV_CAP_PROP_FRAME_HEIGHT,);
if(!cap.isOpened()){
std::cout<<"打开摄像头失败，退出";
exit(-);
}
namedWindow("Calibration");
std::cout<<"Press 'g' to start capturing images!"<<endl;
if( cap.isOpened() && key == 'g' )
{
mode = CAPTURING;
}
按下空格键(SPACE)后使用findChessboardCorners函数在当前帧寻找是否存在可用于标定的角点，如果存在将其提取出来后亚像素化并压入角点集合，保存当前图像：
if( (key & ) == )
{
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(, ), Size(-, -), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, , 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=;t<image_count;t++)
{
vector<Point3f> tempPointSet;
for (int i=;i<board_size.height;i++)
{
for (int j=;j<board_size.width;j++)
{
/* 假设定标板放在世界坐标系中z=0的平面上 */
Point3f tempPoint;
tempPoint.x = i*square_size.width;
tempPoint.y = j*square_size.height;
tempPoint.z = ;
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=; 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(,tempImagePoint.size(),CV_32FC2);
Mat image_points2Mat = Mat(,image_points2.size(), CV_32FC2);
for (int j = ; j < tempImagePoint.size(); j++)
{
image_points2Mat.at<Vec2f>(,j) = Vec2f(image_points2[j].x, image_points2[j].y);
tempImagePointMat.at<Vec2f>(,j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
}
err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
total_err += err/= point_counts[i];
std::cout<<"第"<<i+<<"幅图像的平均误差："<<err<<"像素"<<endl;
fout<<"第"<<i+<<"幅图像的平均误差："<<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(,,CV_32FC1, Scalar::all()); /* 保存每幅图像的旋转矩阵 */ fout<<"相机内参数矩阵："<<endl;
fout<<intrinsic_matrix<<endl<<endl;
fout<<"畸变系数：\n";
fout<<distortion_coeffs<<endl<<endl<<endl;
for (int i=; i<image_count; i++)
{
fout<<"第"<<i+<<"幅图像的旋转向量："<<endl;
fout<<rotation_vectors[i]<<endl; /* 将旋转向量转换为相对应的旋转矩阵 */
Rodrigues(rotation_vectors[i],rotation_matrix);
fout<<"第"<<i+<<"幅图像的旋转矩阵："<<endl;
fout<<rotation_matrix<<endl;
fout<<"第"<<i+<<"幅图像的平移向量："<<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[] = {618.526381968738, , 310.8963715614199,
, 619.4548980786033, 248.6374860176724,
, , };
double distCoeffD[] = {0.09367405350511771, -0.08731677320554751, 0.002823563134787144, -1.246739177460954e-005, -0.0469061739387372};
Mat camera_matrix = Mat(,,CV_64FC1,camD);
Mat distortion_coefficients = Mat(,,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 = ; i < keypoints.size(); i++ ) {
goodfeatures.push_back(keypoints[i].pt);
}
cornerSubPix(gray, goodfeatures, subPixWinSize, Size(-,-), termcrit);
for(size_t i = ; i < goodfeatures.size(); i++ )
{
circle( image, goodfeatures[i], , Scalar(,,), -, );
}
}
使用鼠标选定4个2D点（按正方形左上顶点开始顺时针），然后查找所选点附近的角点，若找到则压入跟踪点集合：
void on_mouse(int event,int x,int y,int flag, void *param)
{
if(event==CV_EVENT_LBUTTONDOWN)
{
if(needtomap && points[].size()<)
{
for(size_t i = ;i<goodfeatures.size();i++)
{
if(abs(goodfeatures[i].x-x)+abs(goodfeatures[i].y-y)<)
{
points[].push_back(goodfeatures[i]);
trackingpoints++;
break;
}
}
}
}
}
建立与2D跟踪点集合相对应的3D空间点集合：
objP.push_back(Point3f(,,)); //三维坐标的单位是毫米
objP.push_back(Point3f(,,));
objP.push_back(Point3f(,,));
objP.push_back(Point3f(,,));
Mat(objP).convertTo(objPM,CV_32F);
使用LK光流法跟踪已选定角点：
vector<uchar> status;
vector<float> err;
if(prevGray.empty())
gray.copyTo(prevGray);
calcOpticalFlowPyrLK(prevGray, gray, points[], points[], status, err);
size_t i,k;
for(i = k = ; i < points[].size(); i++ )
{
if( !status[i] )
continue;
points[][k++] = points[][i];
circle( image, points[][i], , Scalar(,,), -, );
}
若4个点均跟踪成功，使用solvePnP计算相机姿态，并使用计算出的相机姿态重画3D空间点到2D平面查看是否匹配：
if(k == )
getPlanarSurface(points[]);
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[]<<" "<<tv[]<<" "<<tv[]<<endl; projectedPoints.clear();
projectPoints(objPM, rvec, tvec, camera_matrix, distortion_coefficients, projectedPoints); for(unsigned int i = ; i < projectedPoints.size(); ++i)
{
circle( image, projectedPoints[i], , Scalar(,,), -, );
}
}
通过查看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解法（传统迭代，最小化重投影误差）。
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////