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;
}