X-Git-Url: https://vcs.maemo.org/git/?a=blobdiff_plain;f=cvaux%2Fsrc%2Fcvcorrimages.cpp;fp=cvaux%2Fsrc%2Fcvcorrimages.cpp;h=0000000000000000000000000000000000000000;hb=e4c14cdbdf2fe805e79cd96ded236f57e7b89060;hp=12261e9bc18cca0cd395336373aac4aa9f7c38df;hpb=454138ff8a20f6edb9b65a910101403d8b520643;p=opencv diff --git a/cvaux/src/cvcorrimages.cpp b/cvaux/src/cvcorrimages.cpp deleted file mode 100644 index 12261e9..0000000 --- a/cvaux/src/cvcorrimages.cpp +++ /dev/null @@ -1,1152 +0,0 @@ -/*M/////////////////////////////////////////////////////////////////////////////////////// -// -// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. -// -// By downloading, copying, installing or using the software you agree to this license. -// If you do not agree to this license, do not download, install, -// copy or use the software. -// -// -// Intel License Agreement -// For Open Source Computer Vision Library -// -// Copyright (C) 2000, Intel Corporation, all rights reserved. -// Third party copyrights are property of their respective owners. -// -// Redistribution and use in source and binary forms, with or without modification, -// are permitted provided that the following conditions are met: -// -// * Redistribution's of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// -// * Redistribution's in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// -// * The name of Intel Corporation may not be used to endorse or promote products -// derived from this software without specific prior written permission. -// -// This software is provided by the copyright holders and contributors "as is" and -// any express or implied warranties, including, but not limited to, the implied -// warranties of merchantability and fitness for a particular purpose are disclaimed. -// In no event shall the Intel Corporation or contributors be liable for any direct, -// indirect, incidental, special, exemplary, or consequential damages -// (including, but not limited to, procurement of substitute goods or services; -// loss of use, data, or profits; or business interruption) however caused -// and on any theory of liability, whether in contract, strict liability, -// or tort (including negligence or otherwise) arising in any way out of -// the use of this software, even if advised of the possibility of such damage. -// -//M*/ - -#include "_cvaux.h" -//#include "cvtypes.h" -//#include -//#include -//#include "cv.h" -//#include "highgui.h" - -#include - -/* Valery Mosyagin */ - -/* ===== Function for find corresponding between images ===== */ - -/* Create feature points on image and return number of them. Array points fills by found points */ -int icvCreateFeaturePoints(IplImage *image, CvMat *points, CvMat *status) -{ - int foundFeaturePoints = 0; - IplImage *grayImage = 0; - IplImage *eigImage = 0; - IplImage *tmpImage = 0; - CvPoint2D32f *cornerPoints = 0; - - CV_FUNCNAME( "icvFeatureCreatePoints" ); - __BEGIN__; - - /* Test for errors */ - if( image == 0 || points == 0 ) - { - CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); - } - - /* Test image size */ - int w,h; - w = image->width; - h = image->height; - - if( w <= 0 || h <= 0) - { - CV_ERROR( CV_StsOutOfRange, "Size of image must be > 0" ); - } - - /* Test for matrices */ - if( !CV_IS_MAT(points) ) - { - CV_ERROR( CV_StsUnsupportedFormat, "Input parameter points must be a matrix" ); - } - - int needNumPoints; - needNumPoints = points->cols; - if( needNumPoints <= 0 ) - { - CV_ERROR( CV_StsOutOfRange, "Number of need points must be > 0" ); - } - - if( points->rows != 2 ) - { - CV_ERROR( CV_StsOutOfRange, "Number of point coordinates must be == 2" ); - } - - if( status != 0 ) - { - /* If status matrix exist test it for correct */ - if( !CV_IS_MASK_ARR(status) ) - { - CV_ERROR( CV_StsUnsupportedFormat, "Statuses must be a mask arrays" ); - } - - if( status->cols != needNumPoints ) - { - CV_ERROR( CV_StsUnmatchedSizes, "Size of points and statuses must be the same" ); - } - - if( status->rows !=1 ) - { - CV_ERROR( CV_StsUnsupportedFormat, "Number of rows of status must be 1" ); - } - } - - /* Create temporary images */ - CV_CALL( grayImage = cvCreateImage(cvSize(w,h), 8,1) ); - CV_CALL( eigImage = cvCreateImage(cvSize(w,h),32,1) ); - CV_CALL( tmpImage = cvCreateImage(cvSize(w,h),32,1) ); - - /* Create points */ - CV_CALL( cornerPoints = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f) * needNumPoints) ); - - int foundNum; - double quality; - double minDist; - - cvCvtColor(image,grayImage, CV_BGR2GRAY); - - foundNum = needNumPoints; - quality = 0.01; - minDist = 5; - cvGoodFeaturesToTrack(grayImage, eigImage, tmpImage, cornerPoints, &foundNum, quality, minDist); - - /* Copy found points to result */ - int i; - for( i = 0; i < foundNum; i++ ) - { - cvmSet(points,0,i,cornerPoints[i].x); - cvmSet(points,1,i,cornerPoints[i].y); - } - - /* Set status if need */ - if( status ) - { - for( i = 0; i < foundNum; i++ ) - { - status->data.ptr[i] = 1; - } - - for( i = foundNum; i < needNumPoints; i++ ) - { - status->data.ptr[i] = 0; - } - } - - foundFeaturePoints = foundNum; - - __END__; - - /* Free allocated memory */ - cvReleaseImage(&grayImage); - cvReleaseImage(&eigImage); - cvReleaseImage(&tmpImage); - cvFree(&cornerPoints); - - return foundFeaturePoints; -} - -/*-------------------------------------------------------------------------------------*/ - -/* For given points1 (with pntStatus) on image1 finds corresponding points2 on image2 and set pntStatus2 for them */ -/* Returns number of corresponding points */ -int icvFindCorrForGivenPoints( IplImage *image1,/* Image 1 */ - IplImage *image2,/* Image 2 */ - CvMat *points1, - CvMat *pntStatus1, - CvMat *points2, - CvMat *pntStatus2, - int useFilter,/*Use fundamental matrix to filter points */ - double threshold)/* Threshold for good points in filter */ -{ - int resNumCorrPoints = 0; - CvPoint2D32f* cornerPoints1 = 0; - CvPoint2D32f* cornerPoints2 = 0; - char* status = 0; - float* errors = 0; - CvMat* tmpPoints1 = 0; - CvMat* tmpPoints2 = 0; - CvMat* pStatus = 0; - IplImage *grayImage1 = 0; - IplImage *grayImage2 = 0; - IplImage *pyrImage1 = 0; - IplImage *pyrImage2 = 0; - - CV_FUNCNAME( "icvFindCorrForGivenPoints" ); - __BEGIN__; - - /* Test input data for errors */ - - /* Test for null pointers */ - if( image1 == 0 || image2 == 0 || - points1 == 0 || points2 == 0 || - pntStatus1 == 0 || pntStatus2 == 0) - { - CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); - } - - /* Test image size */ - int w,h; - w = image1->width; - h = image1->height; - - if( w <= 0 || h <= 0) - { - CV_ERROR( CV_StsOutOfRange, "Size of image1 must be > 0" ); - } - - if( image2->width != w || image2->height != h ) - { - CV_ERROR( CV_StsUnmatchedSizes, "Size of images must be the same" ); - } - - /* Test for matrices */ - if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2) || - !CV_IS_MAT(pntStatus1) || !CV_IS_MAT(pntStatus2) ) - { - CV_ERROR( CV_StsUnsupportedFormat, "Input parameters (points and status) must be a matrices" ); - } - - /* Test type of status matrices */ - if( !CV_IS_MASK_ARR(pntStatus1) || !CV_IS_MASK_ARR(pntStatus2) ) - { - CV_ERROR( CV_StsUnsupportedFormat, "Statuses must be a mask arrays" ); - } - - /* Test number of points */ - int numPoints; - numPoints = points1->cols; - - if( numPoints <= 0 ) - { - CV_ERROR( CV_StsOutOfRange, "Number of points1 must be > 0" ); - } - - if( points2->cols != numPoints || pntStatus1->cols != numPoints || pntStatus2->cols != numPoints ) - { - CV_ERROR( CV_StsUnmatchedSizes, "Number of points and statuses must be the same" ); - } - - if( points1->rows != 2 || points2->rows != 2 ) - { - CV_ERROR( CV_StsOutOfRange, "Number of points coordinates must be 2" ); - } - - if( pntStatus1->rows != 1 || pntStatus2->rows != 1 ) - { - CV_ERROR( CV_StsOutOfRange, "Status must be a matrix 1xN" ); - } - /* ----- End test ----- */ - - - /* Compute number of visible points on image1 */ - int numVisPoints; - numVisPoints = cvCountNonZero(pntStatus1); - - if( numVisPoints > 0 ) - { - /* Create temporary images */ - /* We must use iplImage againts hughgui images */ - -/* - CvvImage grayImage1; - CvvImage grayImage2; - CvvImage pyrImage1; - CvvImage pyrImage2; -*/ - - /* Create Ipl images */ - CV_CALL( grayImage1 = cvCreateImage(cvSize(w,h),8,1) ); - CV_CALL( grayImage2 = cvCreateImage(cvSize(w,h),8,1) ); - CV_CALL( pyrImage1 = cvCreateImage(cvSize(w,h),8,1) ); - CV_CALL( pyrImage2 = cvCreateImage(cvSize(w,h),8,1) ); - - CV_CALL( cornerPoints1 = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f)*numVisPoints) ); - CV_CALL( cornerPoints2 = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f)*numVisPoints) ); - CV_CALL( status = (char*)cvAlloc( sizeof(char)*numVisPoints) ); - CV_CALL( errors = (float*)cvAlloc( 2 * sizeof(float)*numVisPoints) ); - - int i; - for( i = 0; i < numVisPoints; i++ ) - { - status[i] = 1; - } - - /* !!! Need test creation errors */ - /* - if( !grayImage1.Create(w,h,8)) EXIT; - if( !grayImage2.Create(w,h,8)) EXIT; - if( !pyrImage1. Create(w,h,8)) EXIT; - if( !pyrImage2. Create(w,h,8)) EXIT; - */ - - cvCvtColor(image1,grayImage1,CV_BGR2GRAY); - cvCvtColor(image2,grayImage2,CV_BGR2GRAY); - - /* - grayImage1.CopyOf(image1,0); - grayImage2.CopyOf(image2,0); - */ - - /* Copy points good points from input data */ - uchar *stat1 = pntStatus1->data.ptr; - uchar *stat2 = pntStatus2->data.ptr; - - int curr = 0; - for( i = 0; i < numPoints; i++ ) - { - if( stat1[i] ) - { - cornerPoints1[curr].x = (float)cvmGet(points1,0,i); - cornerPoints1[curr].y = (float)cvmGet(points1,1,i); - curr++; - } - } - - /* Define number of levels of pyramid */ - cvCalcOpticalFlowPyrLK( grayImage1, grayImage2, - pyrImage1, pyrImage2, - cornerPoints1, cornerPoints2, - numVisPoints, cvSize(10,10), 3, - status, errors, - cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), - 0/*CV_LKFLOW_PYR_A_READY*/ ); - - - memset(stat2,0,sizeof(uchar)*numPoints); - - int currVis = 0; - int totalCorns = 0; - - /* Copy new points and set status */ - /* stat1 may not be the same as stat2 */ - for( i = 0; i < numPoints; i++ ) - { - if( stat1[i] ) - { - if( status[currVis] && errors[currVis] < 1000 ) - { - stat2[i] = 1; - cvmSet(points2,0,i,cornerPoints2[currVis].x); - cvmSet(points2,1,i,cornerPoints2[currVis].y); - totalCorns++; - } - currVis++; - } - } - - resNumCorrPoints = totalCorns; - - /* Filter points using RANSAC */ - if( useFilter ) - { - resNumCorrPoints = 0; - /* Use RANSAC filter for found points */ - if( totalCorns > 7 ) - { - /* Create array with good points only */ - CV_CALL( tmpPoints1 = cvCreateMat(2,totalCorns,CV_64F) ); - CV_CALL( tmpPoints2 = cvCreateMat(2,totalCorns,CV_64F) ); - - /* Copy just good points */ - int currPoint = 0; - for( i = 0; i < numPoints; i++ ) - { - if( stat2[i] ) - { - cvmSet(tmpPoints1,0,currPoint,cvmGet(points1,0,i)); - cvmSet(tmpPoints1,1,currPoint,cvmGet(points1,1,i)); - - cvmSet(tmpPoints2,0,currPoint,cvmGet(points2,0,i)); - cvmSet(tmpPoints2,1,currPoint,cvmGet(points2,1,i)); - - currPoint++; - } - } - - /* Compute fundamental matrix */ - CvMat fundMatr; - double fundMatr_dat[9]; - fundMatr = cvMat(3,3,CV_64F,fundMatr_dat); - - CV_CALL( pStatus = cvCreateMat(1,totalCorns,CV_32F) ); - - int num = cvFindFundamentalMat(tmpPoints1,tmpPoints2,&fundMatr,CV_FM_RANSAC,threshold,0.99,pStatus); - if( num > 0 ) - { - int curr = 0; - /* Set final status for points2 */ - for( i = 0; i < numPoints; i++ ) - { - if( stat2[i] ) - { - if( cvmGet(pStatus,0,curr) == 0 ) - { - stat2[i] = 0; - } - curr++; - } - } - resNumCorrPoints = curr; - } - } - } - } - - __END__; - - /* Free allocated memory */ - cvFree(&cornerPoints1); - cvFree(&cornerPoints2); - cvFree(&status); - cvFree(&errors); - cvFree(&tmpPoints1); - cvFree(&tmpPoints2); - cvReleaseMat( &pStatus ); - cvReleaseImage( &grayImage1 ); - cvReleaseImage( &grayImage2 ); - cvReleaseImage( &pyrImage1 ); - cvReleaseImage( &pyrImage2 ); - - return resNumCorrPoints; -} -/*-------------------------------------------------------------------------------------*/ -int icvGrowPointsAndStatus(CvMat **oldPoints,CvMat **oldStatus,CvMat *addPoints,CvMat *addStatus,int addCreateNum) -{ - /* Add to existing points and status arrays new points or just grow */ - CvMat *newOldPoint = 0; - CvMat *newOldStatus = 0; - int newTotalNumber = 0; - - CV_FUNCNAME( "icvGrowPointsAndStatus" ); - __BEGIN__; - - /* Test for errors */ - if( oldPoints == 0 || oldStatus == 0 ) - { - CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); - } - - if( *oldPoints == 0 || *oldStatus == 0 ) - { - CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); - } - - if( !CV_IS_MAT(*oldPoints)) - { - CV_ERROR( CV_StsUnsupportedFormat, "oldPoints must be a pointer to a matrix" ); - } - - if( !CV_IS_MASK_ARR(*oldStatus)) - { - CV_ERROR( CV_StsUnsupportedFormat, "oldStatus must be a pointer to a mask array" ); - } - - int oldNum; - oldNum = (*oldPoints)->cols; - if( oldNum < 1 ) - { - CV_ERROR( CV_StsOutOfRange, "Number of old points must be > 0" ); - } - - /* Define if need number of add points */ - int addNum; - addNum = 0; - if( addPoints != 0 && addStatus != 0 ) - {/* We have aditional points */ - if( CV_IS_MAT(addPoints) && CV_IS_MASK_ARR(addStatus) ) - { - addNum = addPoints->cols; - if( addStatus->cols != addNum ) - { - CV_ERROR( CV_StsOutOfRange, "Number of add points and statuses must be the same" ); - } - } - } - - /* */ - - int numCoord; - numCoord = (*oldPoints)->rows; - newTotalNumber = oldNum + addNum + addCreateNum; - - if( newTotalNumber ) - { - /* Free allocated memory */ - newOldPoint = cvCreateMat(numCoord,newTotalNumber,CV_64F); - newOldStatus = cvCreateMat(1,newTotalNumber,CV_8S); - - /* Copy old values to */ - int i; - - /* Clear all values */ - cvZero(newOldPoint); - cvZero(newOldStatus); - - for( i = 0; i < oldNum; i++ ) - { - int currCoord; - for( currCoord = 0; currCoord < numCoord; currCoord++ ) - { - cvmSet(newOldPoint,currCoord,i,cvmGet(*oldPoints,currCoord,i)); - } - newOldStatus->data.ptr[i] = (*oldStatus)->data.ptr[i]; - } - - /* Copy additional points and statuses */ - if( addNum ) - { - for( i = 0; i < addNum; i++ ) - { - int currCoord; - for( currCoord = 0; currCoord < numCoord; currCoord++ ) - { - cvmSet(newOldPoint,currCoord,i+oldNum,cvmGet(addPoints,currCoord,i)); - } - newOldStatus->data.ptr[i+oldNum] = addStatus->data.ptr[i]; - //cvmSet(newOldStatus,0,i,cvmGet(addStatus,0,i)); - } - } - - /* Delete previous data */ - cvReleaseMat(oldPoints); - cvReleaseMat(oldStatus); - - /* copy pointers */ - *oldPoints = newOldPoint; - *oldStatus = newOldStatus; - - } - __END__; - - return newTotalNumber; -} -/*-------------------------------------------------------------------------------------*/ -int icvRemoveDoublePoins( CvMat *oldPoints,/* Points on prev image */ - CvMat *newPoints,/* New points */ - CvMat *oldStatus,/* Status for old points */ - CvMat *newStatus, - CvMat *origStatus, - float threshold)/* Status for new points */ -{ - - CvMemStorage* storage = 0; - CvSubdiv2D* subdiv = 0; - CvSeq* seq = 0; - - int originalPoints = 0; - - CV_FUNCNAME( "icvRemoveDoublePoins" ); - __BEGIN__; - - /* Test input data */ - if( oldPoints == 0 || newPoints == 0 || - oldStatus == 0 || newStatus == 0 || origStatus == 0 ) - { - CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); - } - - if( !CV_IS_MAT(oldPoints) || !CV_IS_MAT(newPoints) ) - { - CV_ERROR( CV_StsUnsupportedFormat, "Input parameters points must be a matrices" ); - } - - if( !CV_IS_MASK_ARR(oldStatus) || !CV_IS_MASK_ARR(newStatus) || !CV_IS_MASK_ARR(origStatus) ) - { - CV_ERROR( CV_StsUnsupportedFormat, "Input parameters statuses must be a mask array" ); - } - - int oldNumPoints; - oldNumPoints = oldPoints->cols; - if( oldNumPoints < 0 ) - { - CV_ERROR( CV_StsOutOfRange, "Number of oldPoints must be >= 0" ); - } - - if( oldStatus->cols != oldNumPoints ) - { - CV_ERROR( CV_StsUnmatchedSizes, "Number of old Points and old Statuses must be the same" ); - } - - int newNumPoints; - newNumPoints = newPoints->cols; - if( newNumPoints < 0 ) - { - CV_ERROR( CV_StsOutOfRange, "Number of newPoints must be >= 0" ); - } - - if( newStatus->cols != newNumPoints ) - { - CV_ERROR( CV_StsUnmatchedSizes, "Number of new Points and new Statuses must be the same" ); - } - - if( origStatus->cols != newNumPoints ) - { - CV_ERROR( CV_StsUnmatchedSizes, "Number of new Points and new original Status must be the same" ); - } - - if( oldPoints->rows != 2) - { - CV_ERROR( CV_StsOutOfRange, "OldPoints must have 2 coordinates >= 0" ); - } - - if( newPoints->rows != 2) - { - CV_ERROR( CV_StsOutOfRange, "NewPoints must have 2 coordinates >= 0" ); - } - - if( oldStatus->rows != 1 || newStatus->rows != 1 || origStatus->rows != 1 ) - { - CV_ERROR( CV_StsOutOfRange, "Statuses must have 1 row" ); - } - - /* we have points on image and wants add new points */ - /* use subdivision for find nearest points */ - - /* Define maximum and minimum X and Y */ - float minX,minY; - float maxX,maxY; - - minX = minY = FLT_MAX; - maxX = maxY = FLT_MIN; - - int i; - - for( i = 0; i < oldNumPoints; i++ ) - { - if( oldStatus->data.ptr[i] ) - { - float x = (float)cvmGet(oldPoints,0,i); - float y = (float)cvmGet(oldPoints,1,i); - - if( x < minX ) - minX = x; - - if( x > maxX ) - maxX = x; - - if( y < minY ) - minY = y; - - if( y > maxY ) - maxY = y; - } - } - - for( i = 0; i < newNumPoints; i++ ) - { - if( newStatus->data.ptr[i] ) - { - float x = (float)cvmGet(newPoints,0,i); - float y = (float)cvmGet(newPoints,1,i); - - if( x < minX ) - minX = x; - - if( x > maxX ) - maxX = x; - - if( y < minY ) - minY = y; - - if( y > maxY ) - maxY = y; - } - } - - - /* Creare subdivision for old image */ - storage = cvCreateMemStorage(0); -// subdiv = cvCreateSubdivDelaunay2D( cvRect( 0, 0, size.width, size.height ), storage ); - subdiv = cvCreateSubdivDelaunay2D( cvRect( cvRound(minX)-5, cvRound(minY)-5, cvRound(maxX-minX)+10, cvRound(maxY-minY)+10 ), storage ); - seq = cvCreateSeq( 0, sizeof(*seq), sizeof(CvPoint2D32f), storage ); - - /* Insert each point from first image */ - for( i = 0; i < oldNumPoints; i++ ) - { - /* Add just exist points */ - if( oldStatus->data.ptr[i] ) - { - CvPoint2D32f pt; - pt.x = (float)cvmGet(oldPoints,0,i); - pt.y = (float)cvmGet(oldPoints,1,i); - - CvSubdiv2DPoint* point; - point = cvSubdivDelaunay2DInsert( subdiv, pt ); - } - } - - - /* Find nearest points */ - /* for each new point */ - int flag; - for( i = 0; i < newNumPoints; i++ ) - { - flag = 0; - /* Test just exist points */ - if( newStatus->data.ptr[i] ) - { - flag = 1; - /* Let this is a good point */ - //originalPoints++; - - CvPoint2D32f pt; - - pt.x = (float)cvmGet(newPoints,0,i); - pt.y = (float)cvmGet(newPoints,1,i); - - CvSubdiv2DPoint* point = cvFindNearestPoint2D( subdiv, pt ); - - if( point ) - { - /* Test distance of found nearest point */ - double minDistance = icvSqDist2D32f( pt, point->pt ); - - if( minDistance < threshold*threshold ) - { - /* Point is double. Turn it off */ - /* Set status */ - //newStatus->data.ptr[i] = 0; - - /* No this is a double point */ - //originalPoints--; - flag = 0; - } - } - } - originalPoints += flag; - origStatus->data .ptr[i] = (uchar)flag; - } - - __END__; - - cvReleaseMemStorage( &storage ); - - - return originalPoints; - - -} - -void icvComputeProjectMatrix(CvMat* objPoints,CvMat* projPoints,CvMat* projMatr); - -/*-------------------------------------------------------------------------------------*/ -void icvComputeProjectMatrixStatus(CvMat *objPoints4D,CvMat *points2,CvMat *status, CvMat *projMatr) -{ - /* Compute number of good points */ - int num = cvCountNonZero(status); - - /* Create arrays */ - CvMat *objPoints = 0; - objPoints = cvCreateMat(4,num,CV_64F); - - CvMat *points2D = 0; - points2D = cvCreateMat(2,num,CV_64F); - - int currVis = 0; - int i; -#if 1 - FILE *file; - file = fopen("d:\\test\\projStatus.txt","w"); -#endif - int totalNum = objPoints4D->cols; - for( i = 0; i < totalNum; i++ ) - { - fprintf(file,"%d (%d) ",i,status->data.ptr[i]); - if( status->data.ptr[i] ) - { - -#if 1 - double X,Y,Z,W; - double x,y; - X = cvmGet(objPoints4D,0,i); - Y = cvmGet(objPoints4D,1,i); - Z = cvmGet(objPoints4D,2,i); - W = cvmGet(objPoints4D,3,i); - - x = cvmGet(points2,0,i); - y = cvmGet(points2,1,i); - fprintf(file,"%d (%lf %lf %lf %lf) - (%lf %lf)",i,X,Y,Z,W,x,y ); -#endif - cvmSet(objPoints,0,currVis,cvmGet(objPoints4D,0,i)); - cvmSet(objPoints,1,currVis,cvmGet(objPoints4D,1,i)); - cvmSet(objPoints,2,currVis,cvmGet(objPoints4D,2,i)); - cvmSet(objPoints,3,currVis,cvmGet(objPoints4D,3,i)); - - cvmSet(points2D,0,currVis,cvmGet(points2,0,i)); - cvmSet(points2D,1,currVis,cvmGet(points2,1,i)); - - currVis++; - } - - fprintf(file,"\n"); - } - -#if 1 - fclose(file); -#endif - - icvComputeProjectMatrix(objPoints,points2D,projMatr); - - /* Free allocated memory */ - cvReleaseMat(&objPoints); - cvReleaseMat(&points2D); -} - - - -/*-------------------------------------------------------------------------------------*/ -/* For given N images - we have corresponding points on N images - computed projection matrices - reconstructed 4D points - - we must to compute - - -*/ - -void icvAddNewImageToPrevious____( - IplImage *newImage,//Image to add - IplImage *oldImage,//Previous image - CvMat *oldPoints,// previous 2D points on prev image (some points may be not visible) - CvMat *oldPntStatus,//Status for each point on prev image - CvMat *objPoints4D,//prev 4D points - CvMat *newPoints, //Points on new image corr for prev - CvMat *newPntStatus,// New point status for new image - CvMat *newFPoints2D1,//new feature points on prev image - CvMat *newFPoints2D2,//new feature points on new image - CvMat *newFPointsStatus, - CvMat *newProjMatr, - int useFilter, - double threshold)//New projection matrix -{ - CvMat *points2 = 0; - CvMat *status = 0; - CvMat *newFPointsStatusTmp = 0; - - //CV_FUNCNAME( "icvAddNewImageToPrevious____" ); - __BEGIN__; - - /* First found correspondence points for images */ - - /* Test input params */ - - int numPoints; - numPoints = oldPoints->cols; - - /* Allocate memory */ - - points2 = cvCreateMat(2,numPoints,CV_64F); - status = cvCreateMat(1,numPoints,CV_8S); - newFPointsStatusTmp = cvCreateMat(1, newFPoints2D1->cols,CV_8S); - - int corrNum; - corrNum = icvFindCorrForGivenPoints( oldImage,/* Image 1 */ - newImage,/* Image 2 */ - oldPoints, - oldPntStatus, - points2, - status, - useFilter,/*Use fundamental matrix to filter points */ - threshold);/* Threshold for good points in filter */ - - cvCopy(status,newPntStatus); - cvCopy(points2,newPoints); - - CvMat projMatr; - double projMatr_dat[12]; - projMatr = cvMat(3,4,CV_64F,projMatr_dat); - - if( corrNum >= 6 ) - {/* We can compute projection matrix */ -// icvComputeProjectMatrix(objPoints4D,points2,&projMatr); - icvComputeProjectMatrixStatus(objPoints4D,points2,status,&projMatr); - cvCopy(&projMatr,newProjMatr); - - /* Create new points and find correspondence */ - icvCreateFeaturePoints(newImage, newFPoints2D2,newFPointsStatus); - - /* Good if we test new points before find corr points */ - - /* Find correspondence for new found points */ - icvFindCorrForGivenPoints( newImage,/* Image 1 */ - oldImage,/* Image 2 */ - newFPoints2D2, - newFPointsStatus,//prev status - newFPoints2D1, - newFPointsStatusTmp,//new status - useFilter,/*Use fundamental matrix to filter points */ - threshold);/* Threshold for good points in filter */ - - /* We generated new points on image test for exist points */ - - /* Remove all new double points */ - - int origNum; - /* Find point of old image */ - origNum = icvRemoveDoublePoins( oldPoints,/* Points on prev image */ - newFPoints2D1,/* New points */ - oldPntStatus,/* Status for old points */ - newFPointsStatusTmp, - newFPointsStatusTmp,//orig status - 20);/* Status for new points */ - - /* Find double points on new image */ - origNum = icvRemoveDoublePoins( newPoints,/* Points on prev image */ - newFPoints2D2,/* New points */ - newPntStatus,/* Status for old points */ - newFPointsStatusTmp, - newFPointsStatusTmp,//orig status - 20);/* Status for new points */ - - - - /* Add all new good points to result */ - - - /* Copy new status to old */ - cvCopy(newFPointsStatusTmp,newFPointsStatus); - - - } - - - - __END__; - - /* Free allocated memory */ - - return; -} -/*-------------------------------------------------------------------------------------*/ -//int icvDelete// -//CreateGood - -/*-------------------------------------------------------------------------------------*/ -int icvDeleteSparsInPoints( int numImages, - CvMat **points, - CvMat **status, - CvMat *wasStatus)/* status of previous configuration */ -{ - /* Delete points which no exist on any of images */ - /* numImages - number of images */ - /* points - arrays of points for each image. Changing */ - /* status - arrays of status for each image. Changing */ - /* Function returns number of common points */ - - int comNumber = 0; - CV_FUNCNAME( "icvDeleteSparsInPoints" ); - __BEGIN__; - - /* Test for errors */ - if( numImages < 1 ) - { - CV_ERROR( CV_StsOutOfRange, "Number of images must be more than 0" ); - } - - if( points == 0 || status == 0 ) - { - CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); - } - int numPoints; - - numPoints = points[0]->cols; - ////////// TESTS ////////// - - int numCoord; - numCoord = points[0]->rows;// !!! may be number of coordinates is not correct !!! - - int i; - int currExistPoint; - currExistPoint = 0; - - if( wasStatus ) - { - cvZero(wasStatus); - } - - int currImage; - for( i = 0; i < numPoints; i++ ) - { - int flag = 0; - for( currImage = 0; currImage < numImages; currImage++ ) - { - flag |= status[currImage]->data.ptr[i]; - } - - if( flag ) - { - /* Current point exists */ - /* Copy points and status */ - if( currExistPoint != i )/* Copy just if different */ - { - for( currImage = 0; currImage < numImages; currImage++ ) - { - /* Copy points */ - for( int currCoord = 0; currCoord < numCoord; currCoord++ ) - { - cvmSet(points[currImage],currCoord,currExistPoint, cvmGet(points[currImage],currCoord,i) ); - } - - /* Copy status */ - status[currImage]->data.ptr[currExistPoint] = status[currImage]->data.ptr[i]; - } - } - if( wasStatus ) - { - wasStatus->data.ptr[i] = 1; - } - - currExistPoint++; - - } - } - - /* Rest of final status of points must be set to 0 */ - for( i = currExistPoint; i < numPoints; i++ ) - { - for( currImage = 0; currImage < numImages; currImage++ ) - { - status[currImage]->data.ptr[i] = 0; - } - } - - comNumber = currExistPoint; - - __END__; - return comNumber; -} - -#if 0 -/*-------------------------------------------------------------------------------------*/ -void icvGrowPointsArray(CvMat **points) -{ - - -} - -/*-------------------------------------------------------------------------------------*/ -void icvAddNewArrayPoints() -{ - -} - -/*-------------------------------------------------------------------------------------*/ -#endif - -////////////////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////// - -/* Add image to existing images and corr points */ -#if 0 -/* Returns: 1 if new image was added good */ -/* 0 image was not added. Not enought corr points */ -int AddImageToStruct( IplImage *newImage,//Image to add - IplImage *oldImage,//Previous image - CvMat *oldPoints,// previous 2D points on prev image (some points may be not visible) - CvMat *oldPntStatus,//Status for each point on prev image - CvMat *objPoints4D,//prev 4D points - CvMat *newPntStatus,// New point status for new image - CvMat *newPoints,//New corresponding points on new image - CvMat *newPoints2D1,//new points on prev image - CvMat *newPoints2D2,//new points on new image - CvMat *newProjMatr);//New projection matrix -{ - - /* Add new image. Create new corr points */ - /* Track exist points from oldImage to newImage */ - /* Create new vector status */ - CvMat *status; - int numPoints = oldPoints->cols; - status = cvCreateMat(1,numPoints,CV_64F); - /* Copy status */ - cvConvert(pntStatus,status); - - int corrNum = FindCorrForGivenPoints(oldImage,newImage,oldPoints,newPoints,status); - - /* Status has new status of points */ - - CvMat projMatr; - double projMatr_dat[12]; - projMatr = cvMat(3,4,CV_64F,projMatr_dat); - - /* If number of corr points is 6 or more can compute projection matrix */ - if( corrNum >= 6) - { - /* Compute projection matrix for new image using corresponding points */ - icvComputeProjectMatrix(objPoints4D,newPoints,&projMatr); - - CvMat *tmpPoints; - /* Create new points and find correspondence */ - int num = FindFeaturePoints(newImage, &tmpPoints); - if( num > 0 ) - { - CvMat *newPoints; - newPoints = cvCreateMat(2,num,CV_64F); - CvMat *status; - status = cvCreateMat(1,num,CV_64F); - /* Set status for all points */ - int i; - for( i = 0; i < num; i++ ) - { - cvmSet(status,0,i,1.0); - } - - int corrNum2 = FindCorrForGivenPoints(oldImage,newImage,tmpPoints,newPoints,status); - - /* !!! Filter points using projection matrices or not ??? */ - - /* !!! Need to filter nearest points */ - - /* Add new found points to exist points and optimize again */ - CvMat *new2DPoints; - CvMat *newStatus; - - /* add new status to old status */ - - - - - - } - else - { - /* No new points were found */ - } - } - else - { - /* We can't compute projection matrix for new image */ - return 0; - } - -} -#endif