原文链接如下:
http://blog.csdn.net/aptx704610875/article/details/48915149
上一节我们使用张正友相机标定法获得了相机内参,这一节我们使用Robust Planar Pose (RPP) 算法估计相机初始姿态并更新之。推荐3篇我学习的博客:【姿态估计】Pose estimation algorithm 之 Robust Planar Pose (RPP)algorithm,POSIT算法的原理--opencv 3D姿态估计,三维姿态:关于solvePnP与cvPOSIT。
注意点1:solvePnP调用的是cvFindExtrinsicCameraParams2通过已知的内参进行未知外参求解,是一个精确解;而cvPOSIT是用仿射投影模型近似透视投影模型下,不断迭代计算出来的估计值(在物体深度变化相对于物体到摄像机的距离比较大的时候,这种算法可能不收敛)。
注意点2:solvePnP使用至少4个点(共面不共线)或者6个点(不共面)。
注意点3:solvePnPRansac使用Ransac方法得到最优的姿态但是速度慢不适合实时系统。
接下来我们使用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);
- }
- }
- 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;
- }
- }
- }
- }
- }
- 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);
- 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);
- }
- 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);
- }
- }
下一节我们将结合相机外参使用OpenGL画出AR物体。
============================================================================================================
2015/10/20号补充:
这几天在做跟踪恢复的时候需要用给定的2D点和R,T计算3D点,于是重新手算了一边图像2D点和空间3D点的关系。过程中搞懂了为什么solvePnP计算rotation和translation的时候需要至少4组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个,由于旋转矩阵中每行每列的平方和都为1,所以R13可以用R11,R12表示, R23可以用R21,R22表示, R33可以用R31,R32表示(或者用R13,R23表示), R31可以用R11,R21表示, R32可以用R12,R22表示。所以未知数就只用7个了,而每一组2D/3D点提供的x y Xw Yw可以确立两个方程,所以4组2D/3D点的坐标能确立8个方程从而解出7个未知数。
故SolvePnP需要知道至少4组2D/3D点。