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\n",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];
}
一個計算reprojection error的小程序
發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.