reprojection error를 계산하는 작은 프로그램

2976 단어 float
double reprojectionErrorCalculation(IplImage *rgbImage, CvMat *objectPoints, CvMat *imagePoints, CvMat *calibrationMatrix, CvMat *rotationMatrix, CvMat *translationVector){
	   IplImage* img=cvCreateImage(cvSize(rgbImage->width,rgbImage->height),rgbImage->depth,rgbImage->nChannels);
	   cvCopy(rgbImage,img);
	   int numPoints=objectPoints->height;
	   CvMat* translationMatrix=cvCreateMat(3,numPoints,CV_32FC1);
	   for(int i=0;i<3;i++){
		   float* ptr=(float*)(translationMatrix->data.ptr+i*translationMatrix->step);
		   float* ptrTranslation=(float*)(translationVector->data.ptr+i*translationVector->step);
		   for(int j=0;j<numPoints;j++){
		       *(ptr+j)=*ptrTranslation;
		   }
	   }

	   CvMat* objectPointsTranspose=cvCreateMat(3,numPoints,CV_32FC1);
	   cvTranspose(objectPoints,objectPointsTranspose);
	   CvMat* imagePointsHomogeneous=cvCreateMat(3,numPoints,CV_32FC1);
	   cvMatMulAdd(rotationMatrix,objectPointsTranspose,translationMatrix,imagePointsHomogeneous);

	   cvMatMul(calibrationMatrix,imagePointsHomogeneous,imagePointsHomogeneous);

	   CvMat* imagePointsReproject=cvCreateMat(numPoints,2,CV_32FC1);
	   cvConvertPointsHomogeneous(imagePointsHomogeneous,imagePointsReproject);

	   CvMat* reprojectionErrorMatrix=cvCreateMat(numPoints,2,CV_32FC1);
	   cvSub(imagePoints,imagePointsReproject,reprojectionErrorMatrix);
	   cvPow(reprojectionErrorMatrix,reprojectionErrorMatrix,2.0);
	   CvScalar reprojectionError=cvSum(reprojectionErrorMatrix);
	   reprojectionError.val[0]=sqrt(reprojectionError.val[0]/numPoints);

	   printf("The reprojection error is:\t%lf
",reprojectionError.val[0]); for(int i=0;i<numPoints;i++){ float* ptrOriginal=(float*)(imagePoints->data.ptr+i*imagePoints->step); float* ptrReproject=(float*)(imagePointsReproject->data.ptr+i*imagePointsReproject->step); cvLine(img,cvPoint(*(ptrOriginal)-3.0,*(ptrOriginal+1)),cvPoint(*(ptrOriginal)+3.0,*(ptrOriginal+1)),CV_RGB(255.0,0.0,0.0)); cvLine(img,cvPoint(*(ptrOriginal),*(ptrOriginal+1)-3.0),cvPoint(*(ptrOriginal),*(ptrOriginal+1)+3.0),CV_RGB(255.0,0.0,0.0)); cvLine(img,cvPoint(*(ptrReproject)-3.0,*(ptrReproject+1)),cvPoint(*(ptrReproject)+3.0,*(ptrReproject+1)),CV_RGB(0.0,255.0,0.0)); cvLine(img,cvPoint(*(ptrReproject),*(ptrReproject+1)-3.0),cvPoint(*(ptrReproject),*(ptrReproject+1)+3.0),CV_RGB(0.0,255.0,0.0)); } cvNamedWindow("reprojection error"); cvShowImage("reprojection error",img); while(1){ if(cvWaitKey(2)==27) break; } cvDestroyWindow("reprojection error"); cvReleaseImage(&img); cvReleaseMat(&translationMatrix); cvReleaseMat(&objectPointsTranspose); cvReleaseMat(&imagePointsHomogeneous); cvReleaseMat(&imagePointsReproject); cvReleaseMat(&reprojectionErrorMatrix); return reprojectionError.val[0]; }

좋은 웹페이지 즐겨찾기