bool CamAugmentation::EstimateHomographyRT(){ // Search for first existing homography: for( int c = 0; c < (int)v_homography.size(); c++ ){ // If homography and corresponding camera exists, yipiee: if( v_homography[c]->m_homography && (int)s_optimal.v_camera_c.size() > c ){ return CreateHomographyRotationTranslationMatrix( c ); } } return false; }
int ComputeHomographyFromPointCorrespondanceOpenCV ( struct FeatureList * source, struct CameraCalibrationData * calibration, struct TransformationMatrix * rotation_matrix, struct TransformationMatrix * translation_matrix, struct TransformationMatrix * rotation_and_translation_matrix, struct TransformationMatrix * homography_matrix , unsigned int render_warped_image ) { if ( source->current_features == 0 ) { return 0; } if ( source->current_features < 4) { return 0; } int i=0 , res = 1; unsigned int points_limit = source->current_features; //points_limit = 4; // If we want just a perspective Transform and not a Homography CvMat* srcPoints = cvCreateMat(2,points_limit,CV_32FC1); if ( srcPoints != 0 ) { for ( i=0; i<points_limit; i++ ) { cvmSet(srcPoints,0,i,(float) source->list[i].last_x); cvmSet(srcPoints,1,i,(float) source->list[i].last_y); } } CvMat* dstPoints = cvCreateMat(2,points_limit,CV_32FC1); if ( dstPoints != 0 ) { for ( i=0; i<points_limit; i++ ) { cvmSet(dstPoints,0,i,(float) source->list[i].x); cvmSet(dstPoints,1,i,(float) source->list[i].y); } } CvMat* status=0; CvMat* H = cvCreateMat(3,3,CV_64F); cvZero(H); res = cvFindHomography(srcPoints,dstPoints,H,CV_RANSAC,2.5,status); //cvPerspectiveTransform(srcPoints,dstPoints, H); i=0; int mem=0,j=0; homography_matrix->rows=3; homography_matrix->columns=3; for(i=0; i<3; i++) { for(j=0; j<3; j++) { homography_matrix->item[mem++]=cvmGet(H,i,j); } } // THIS OVERLAYS WARPED IMAGE OF LAST VIEW if (render_warped_image) { IplImage * image = cvCreateImage( cvSize(320,240), IPL_DEPTH_8U, 3 ); memcpy(image->imageData , video_register[CALIBRATED_LEFT_EYE].pixels , metrics[RESOLUTION_MEMORY_LIMIT_3BYTE]); IplImage * dstImg = cvCloneImage(image); cvWarpPerspective(image, dstImg, H , CV_INTER_CUBIC | CV_WARP_FILL_OUTLIERS, cvScalarAll(0) ); memcpy( video_register[LAST_RIGHT_OPERATION].pixels , dstImg->imageData , metrics[RESOLUTION_MEMORY_LIMIT_3BYTE]); video_register[LAST_RIGHT_OPERATION].time = video_register[CALIBRATED_LEFT_EYE].time; cvReleaseImage( &image ); cvReleaseImage( &dstImg ); } // transformed output image CvMat* intriMat=cvCreateMat(3,3,CV_64F); //cvMat(3,3,CV_64F,calibration->intrinsic_parameters_array); cvmSet(intriMat,0,0,calibration->intrinsic_parameters_array[0]); cvmSet(intriMat,0,1,calibration->intrinsic_parameters_array[1]); cvmSet(intriMat,0,2,calibration->intrinsic_parameters_array[2]); cvmSet(intriMat,1,0,calibration->intrinsic_parameters_array[3]); cvmSet(intriMat,1,1,calibration->intrinsic_parameters_array[4]); cvmSet(intriMat,1,2,calibration->intrinsic_parameters_array[5]); cvmSet(intriMat,2,0,calibration->intrinsic_parameters_array[6]); cvmSet(intriMat,2,1,calibration->intrinsic_parameters_array[7]); cvmSet(intriMat,2,2,calibration->intrinsic_parameters_array[8]); CvMat* homography_decomposition_to_translation_and_rotation = CreateHomographyRotationTranslationMatrix(H,intriMat); if ( homography_decomposition_to_translation_and_rotation != 0 ) { ConvertMatrices( rotation_matrix,translation_matrix,rotation_and_translation_matrix , homography_decomposition_to_translation_and_rotation ); } if ( srcPoints != 0 ) { cvReleaseMat(&srcPoints); } if ( dstPoints != 0 ) { cvReleaseMat(&dstPoints); } if ( H != 0 ) { cvReleaseMat(&H); } if ( homography_decomposition_to_translation_and_rotation != 0 ) { cvReleaseMat(&homography_decomposition_to_translation_and_rotation); } if ( intriMat != 0 ) { cvReleaseMat(&intriMat); } return res; }