棋盘格的标定,标定棋盘格,不同角度下的棋盘格标定

#include <iostream> #include <cxcore.h> #include <stdio.h> #include "cv.h" #include "highgui.h" #define N 10 using namespace std; using namespace cv; double matmodul(Mat mat); void obinUVcoordinates(CvPoint2D32f*corners,Point2d *pointp,Point3d*pointP); void computeProjectionMatrix(Point2d *pointp,Point3d*pointP,Mat &projectMatrix); void decomposeProjectionMatrix1(Mat projectMatrix, Mat &intrinsicMatrix, Mat &R,Mat &t); void printMatrix(string matrixName,Mat inputMatrix); int main(int argc, char** argv) { Point3d pointP[N];Point2d pointp[N]; IplImage * imageCalibration=cvLoadImage("B.jpg"); //IplImage * imageCalibration=cvLoadImage("A.jpg"); int height=8,width=10; int amount=height*width; CvSize cornerSize; cornerSize.height=height; cornerSize.width=width; CvPoint2D32f corners[80]; int cornerNum; IplImage* grayImage=cvCreateImage(cvGetSize(imageCalibration),IPL_DEPTH_8U,1); cvCvtColor(imageCalibration,grayImage,CV_BGR2GRAY); cvFindChessboardCorners(grayImage,cornerSize,corners,&cornerNum,0); //寻找棋盘图内角点位置 cvFindCornerSubPix(grayImage,corners,cornerNum,cvSize(10,8),cvSize(-1,-1),cvTermCriteria(CV_TERMCRIT_EPS|CV_TERMCRIT_ITER,20,0.1));//检测亚像素级角点 cvDrawChessboardCorners(imageCalibration,cornerSize,corners,cornerNum,1);//画棋盘图的内角点位置 cvNamedWindow("calibrationImage",1); obinUVcoordinates(corners,pointp,pointP);//获得点对图像坐标点p和三维世界坐标P Mat projectMatrix=Mat::zeros(3,4,CV_64FC1); Mat intrinsicMatrix=Mat::zeros(3,3,CV_64FC1); Mat R=Mat::zeros(3,3,CV_64FC1); Mat t=Mat::zeros(3,1,CV_64FC1); computeProjectionMatrix(pointp,pointP,projectMatrix);//计算投影矩阵 decomposeProjectionMatrix1(projectMatrix,intrinsicMatrix,R,t);//分解投影矩阵 printMatrix("内参",intrinsicMatrix);//内参矩阵 printMatrix("外参",R);//外参矩阵 printMatrix("translation",t); cvShowImage("calibrationImage",imageCalibration); cvWaitKey(0);//可以返回键盘按键的键值 return 0; } void obinUVcoordinates(CvPoint2D32f*corners,Point2d *pointp,Point3d*pointP) { //世界坐标值 pointP[0].x=20;pointP[0].y=0;pointP[0].z=60;//53 pointP[1].x=0;pointP[1].y=0;pointP[1].z=60;//54 pointP[2].x=0;pointP[2].y=20;pointP[2].z=60;//55 pointP[3].x=20;pointP[3].y=0;pointP[3].z=40;//63 pointP[4].x=0;pointP[4].y=0;pointP[4].z=40;//64 pointP[5].x=0;pointP[5].y=20;pointP[5].z=40;//65 pointP[6].x=20;pointP[6].y=0;pointP[6].z=20;//73 pointP[7].x=0;pointP[7].y=0;pointP[7].z=20;//74 pointP[8].x=0;pointP[8].y=20;pointP[8].z=20;//75 //图像像素坐标 pointp[0]=corners[53]; pointp[1]=corners[54]; pointp[2]=corners[55]; pointp[3]=corners[63]; pointp[4]=corners[64]; pointp[5]=corners[65]; pointp[6]=corners[73]; pointp[7]=corners[74]; pointp[8]=corners[75]; } void computeProjectionMatrix(Point2d *pointp,Point3d*pointP,Mat &projectMatrix) { Mat A=Mat::zeros(2*N,12,CV_64FC1); //20*12 Mat LL=Mat::zeros(2*N-1,1,CV_64FC1);//19*1 Mat C=Mat::zeros(2*N,11,CV_64FC1);//20*11 Mat B=Mat::zeros(2*N,1,CV_64FC1);//20*1 for (int i=0;i<N;i++) { A.at<double>(2*i,0)=pointP[i].x; A.at<double>(2*i,1)=pointP[i].y; A.at<double>(2*i,2)=pointP[i].z; A.at<double>(2*i,3)=1; A.at<double>(2*i,8)=-pointp[i].x*pointP[i].x; A.at<double>(2*i,9)=-pointp[i].x*pointP[i].y; A.at<double>(2*i,10)=-pointp[i].x*pointP[i].z; A.at<double>(2*i,11)=-pointp[i].x; A.at<double>(2*i+1,4)=pointP[i].x; A.at<double>(2*i+1,5)=pointP[i].y; A.at<double>(2*i+1,6)=pointP[i].z; A.at<double>(2*i+1,7)=1; A.at<double>(2*i+1,8)=-pointp[i].y*pointP[i].x; A.at<double>(2*i+1,9)=-pointp[i].y*pointP[i].y; A.at<double>(2*i+1,10)=-pointp[i].y*pointP[i].z; A.at<double>(2*i+1,11)=-pointp[i].y; } C=A.colRange(0,11); //从第0行开始的11行 B=-A.col(11); SVD mySvd=SVD(C,0); //SVD分解 mySvd.backSubst(B,LL); projectMatrix.at<double>(0,0)=LL.at<double>(0,0); projectMatrix.at<double>(0,1)=LL.at<double>(1,0); projectMatrix.at<double>(0,2)=LL.at<double>(2,0); projectMatrix.at<double>(0,3)=LL.at<double>(3,0); projectMatrix.at<double>(1,0)=LL.at<double>(4,0); projectMatrix.at<double>(1,1)=LL.at<double>(5,0); projectMatrix.at<double>(1,2)=LL.at<double>(6,0); projectMatrix.at<double>(1,3)=LL.at<double>(7,0); projectMatrix.at<double>(2,0)=LL.at<double>(8,0); projectMatrix.at<double>(2,1)=LL.at<double>(9,0); projectMatrix.at<double>(2,2)=LL.at<double>(10,0); projectMatrix.at<double>(2,3)=1; } void decomposeProjectionMatrix1(Mat projectMatrix, Mat &intrinsicMatrix, Mat &R,Mat &t) { Mat m1=Mat::zeros(3,1,CV_64FC1); Mat m2=Mat::zeros(3,1,CV_64FC1); Mat m3=Mat::zeros(3,1,CV_64FC1); for (int i=0;i<3;i++) { m1.at<double>(i,0)=projectMatrix.at<double>(0,i); m2.at<double>(i,0)=projectMatrix.at<double>(1,i); m3.at<double>(i,0)=projectMatrix.at<double>(2,i); } double u,v,b,r; double m34=1/matmodul(m3); R.col(2)=m34*m3; r=m34*m34*matmodul(m1.cross(m3)); b=m34*m34*matmodul(m2.cross(m3)); Mat uMat=Mat::zeros(1,1,CV_64FC1); uMat=m34*m34*m1.t()*m3; u=uMat.at<double>(0,0); Mat vMat=Mat::zeros(1,1,CV_64FC1); vMat=m34*m34*m2.t()*m3; v=vMat.at<double>(0,0); R.col(0)=m34/r*(m1-u*m3); R.col(1)=m34/b*(m2-v*m3); t.at<double>(0,0)=m34/r*(projectMatrix.at<double>(0,3)-u); t.at<double>(1,0)=m34/b*(projectMatrix.at<double>(1,3)-v); t.at<double>(2,0)=m34; intrinsicMatrix.at<double>(0,0)=r; intrinsicMatrix.at<double>(0,2)=u; intrinsicMatrix.at<double>(1,1)=b; intrinsicMatrix.at<double>(1,2)=v; intrinsicMatrix.at<double>(2,2)=1; } double matmodul(Mat mat) { double distance2=0; for (int i=0;i<mat.rows;i++) { distance2+=pow(mat.at<double>(i,0),2); } return sqrt(distance2); } void printMatrix(string matrixName,Mat inputMatrix) { cout<<matrixName<<"="<<endl; int m=inputMatrix.rows; int n=inputMatrix.cols; for (int i=0;i<m;i++) { if(i==0) cout<<"["; for(int j=0;j<n;j++) { cout<<inputMatrix.at<double>(i,j)<<" "; } if(i<m-1) cout<<";"<<endl; else { cout<<"]"<<endl; } } cout<<endl; }