1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
42 #include "precomp.hpp"
48 /* ===== Function for find corresponding between images ===== */
50 /* Create feature points on image and return number of them. Array points fills by found points */
51 static int icvCreateFeaturePoints(IplImage *image, CvMat *points, CvMat *status)
53 int foundFeaturePoints = 0;
54 IplImage *grayImage = 0;
55 IplImage *eigImage = 0;
56 IplImage *tmpImage = 0;
57 CvPoint2D32f *cornerPoints = 0;
59 CV_FUNCNAME( "icvFeatureCreatePoints" );
63 if( image == 0 || points == 0 )
65 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
75 CV_ERROR( CV_StsOutOfRange, "Size of image must be > 0" );
78 /* Test for matrices */
79 if( !CV_IS_MAT(points) )
81 CV_ERROR( CV_StsUnsupportedFormat, "Input parameter points must be a matrix" );
85 needNumPoints = points->cols;
86 if( needNumPoints <= 0 )
88 CV_ERROR( CV_StsOutOfRange, "Number of need points must be > 0" );
91 if( points->rows != 2 )
93 CV_ERROR( CV_StsOutOfRange, "Number of point coordinates must be == 2" );
98 /* If status matrix exist test it for correct */
99 if( !CV_IS_MASK_ARR(status) )
101 CV_ERROR( CV_StsUnsupportedFormat, "Statuses must be a mask arrays" );
104 if( status->cols != needNumPoints )
106 CV_ERROR( CV_StsUnmatchedSizes, "Size of points and statuses must be the same" );
109 if( status->rows !=1 )
111 CV_ERROR( CV_StsUnsupportedFormat, "Number of rows of status must be 1" );
115 /* Create temporary images */
116 CV_CALL( grayImage = cvCreateImage(cvSize(w,h), 8,1) );
117 CV_CALL( eigImage = cvCreateImage(cvSize(w,h),32,1) );
118 CV_CALL( tmpImage = cvCreateImage(cvSize(w,h),32,1) );
121 CV_CALL( cornerPoints = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f) * needNumPoints) );
127 cvCvtColor(image,grayImage, CV_BGR2GRAY);
129 foundNum = needNumPoints;
132 cvGoodFeaturesToTrack(grayImage, eigImage, tmpImage, cornerPoints, &foundNum, quality, minDist);
134 /* Copy found points to result */
136 for( i = 0; i < foundNum; i++ )
138 cvmSet(points,0,i,cornerPoints[i].x);
139 cvmSet(points,1,i,cornerPoints[i].y);
142 /* Set status if need */
145 for( i = 0; i < foundNum; i++ )
147 status->data.ptr[i] = 1;
150 for( i = foundNum; i < needNumPoints; i++ )
152 status->data.ptr[i] = 0;
156 foundFeaturePoints = foundNum;
160 /* Free allocated memory */
161 cvReleaseImage(&grayImage);
162 cvReleaseImage(&eigImage);
163 cvReleaseImage(&tmpImage);
164 cvFree(&cornerPoints);
166 return foundFeaturePoints;
169 /*-------------------------------------------------------------------------------------*/
171 /* For given points1 (with pntStatus) on image1 finds corresponding points2 on image2 and set pntStatus2 for them */
172 /* Returns number of corresponding points */
173 static int icvFindCorrForGivenPoints( IplImage *image1,/* Image 1 */
174 IplImage *image2,/* Image 2 */
179 int useFilter,/*Use fundamental matrix to filter points */
180 double threshold)/* Threshold for good points in filter */
182 int resNumCorrPoints = 0;
183 CvPoint2D32f* cornerPoints1 = 0;
184 CvPoint2D32f* cornerPoints2 = 0;
187 CvMat* tmpPoints1 = 0;
188 CvMat* tmpPoints2 = 0;
190 IplImage *grayImage1 = 0;
191 IplImage *grayImage2 = 0;
192 IplImage *pyrImage1 = 0;
193 IplImage *pyrImage2 = 0;
195 CV_FUNCNAME( "icvFindCorrForGivenPoints" );
198 /* Test input data for errors */
200 /* Test for null pointers */
201 if( image1 == 0 || image2 == 0 ||
202 points1 == 0 || points2 == 0 ||
203 pntStatus1 == 0 || pntStatus2 == 0)
205 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
208 /* Test image size */
213 if( w <= 0 || h <= 0)
215 CV_ERROR( CV_StsOutOfRange, "Size of image1 must be > 0" );
218 if( image2->width != w || image2->height != h )
220 CV_ERROR( CV_StsUnmatchedSizes, "Size of images must be the same" );
223 /* Test for matrices */
224 if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2) ||
225 !CV_IS_MAT(pntStatus1) || !CV_IS_MAT(pntStatus2) )
227 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters (points and status) must be a matrices" );
230 /* Test type of status matrices */
231 if( !CV_IS_MASK_ARR(pntStatus1) || !CV_IS_MASK_ARR(pntStatus2) )
233 CV_ERROR( CV_StsUnsupportedFormat, "Statuses must be a mask arrays" );
236 /* Test number of points */
238 numPoints = points1->cols;
242 CV_ERROR( CV_StsOutOfRange, "Number of points1 must be > 0" );
245 if( points2->cols != numPoints || pntStatus1->cols != numPoints || pntStatus2->cols != numPoints )
247 CV_ERROR( CV_StsUnmatchedSizes, "Number of points and statuses must be the same" );
250 if( points1->rows != 2 || points2->rows != 2 )
252 CV_ERROR( CV_StsOutOfRange, "Number of points coordinates must be 2" );
255 if( pntStatus1->rows != 1 || pntStatus2->rows != 1 )
257 CV_ERROR( CV_StsOutOfRange, "Status must be a matrix 1xN" );
259 /* ----- End test ----- */
262 /* Compute number of visible points on image1 */
264 numVisPoints = cvCountNonZero(pntStatus1);
266 if( numVisPoints > 0 )
268 /* Create temporary images */
269 /* We must use iplImage againts hughgui images */
278 /* Create Ipl images */
279 CV_CALL( grayImage1 = cvCreateImage(cvSize(w,h),8,1) );
280 CV_CALL( grayImage2 = cvCreateImage(cvSize(w,h),8,1) );
281 CV_CALL( pyrImage1 = cvCreateImage(cvSize(w,h),8,1) );
282 CV_CALL( pyrImage2 = cvCreateImage(cvSize(w,h),8,1) );
284 CV_CALL( cornerPoints1 = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f)*numVisPoints) );
285 CV_CALL( cornerPoints2 = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f)*numVisPoints) );
286 CV_CALL( status = (char*)cvAlloc( sizeof(char)*numVisPoints) );
287 CV_CALL( errors = (float*)cvAlloc( 2 * sizeof(float)*numVisPoints) );
290 for( i = 0; i < numVisPoints; i++ )
295 /* !!! Need test creation errors */
297 if( !grayImage1.Create(w,h,8)) EXIT;
298 if( !grayImage2.Create(w,h,8)) EXIT;
299 if( !pyrImage1. Create(w,h,8)) EXIT;
300 if( !pyrImage2. Create(w,h,8)) EXIT;
303 cvCvtColor(image1,grayImage1,CV_BGR2GRAY);
304 cvCvtColor(image2,grayImage2,CV_BGR2GRAY);
307 grayImage1.CopyOf(image1,0);
308 grayImage2.CopyOf(image2,0);
311 /* Copy points good points from input data */
312 uchar *stat1 = pntStatus1->data.ptr;
313 uchar *stat2 = pntStatus2->data.ptr;
316 for( i = 0; i < numPoints; i++ )
320 cornerPoints1[curr].x = (float)cvmGet(points1,0,i);
321 cornerPoints1[curr].y = (float)cvmGet(points1,1,i);
326 /* Define number of levels of pyramid */
327 cvCalcOpticalFlowPyrLK( grayImage1, grayImage2,
328 pyrImage1, pyrImage2,
329 cornerPoints1, cornerPoints2,
330 numVisPoints, cvSize(10,10), 3,
332 cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03),
333 0/*CV_LKFLOW_PYR_A_READY*/ );
336 memset(stat2,0,sizeof(uchar)*numPoints);
341 /* Copy new points and set status */
342 /* stat1 may not be the same as stat2 */
343 for( i = 0; i < numPoints; i++ )
347 if( status[currVis] && errors[currVis] < 1000 )
350 cvmSet(points2,0,i,cornerPoints2[currVis].x);
351 cvmSet(points2,1,i,cornerPoints2[currVis].y);
358 resNumCorrPoints = totalCorns;
360 /* Filter points using RANSAC */
363 resNumCorrPoints = 0;
364 /* Use RANSAC filter for found points */
367 /* Create array with good points only */
368 CV_CALL( tmpPoints1 = cvCreateMat(2,totalCorns,CV_64F) );
369 CV_CALL( tmpPoints2 = cvCreateMat(2,totalCorns,CV_64F) );
371 /* Copy just good points */
373 for( i = 0; i < numPoints; i++ )
377 cvmSet(tmpPoints1,0,currPoint,cvmGet(points1,0,i));
378 cvmSet(tmpPoints1,1,currPoint,cvmGet(points1,1,i));
380 cvmSet(tmpPoints2,0,currPoint,cvmGet(points2,0,i));
381 cvmSet(tmpPoints2,1,currPoint,cvmGet(points2,1,i));
387 /* Compute fundamental matrix */
389 double fundMatr_dat[9];
390 fundMatr = cvMat(3,3,CV_64F,fundMatr_dat);
392 CV_CALL( pStatus = cvCreateMat(1,totalCorns,CV_32F) );
394 int num = cvFindFundamentalMat(tmpPoints1,tmpPoints2,&fundMatr,CV_FM_RANSAC,threshold,0.99,pStatus);
398 /* Set final status for points2 */
399 for( i = 0; i < numPoints; i++ )
403 if( cvmGet(pStatus,0,curr) == 0 )
410 resNumCorrPoints = curr;
418 /* Free allocated memory */
419 cvFree(&cornerPoints1);
420 cvFree(&cornerPoints2);
425 cvReleaseMat( &pStatus );
426 cvReleaseImage( &grayImage1 );
427 cvReleaseImage( &grayImage2 );
428 cvReleaseImage( &pyrImage1 );
429 cvReleaseImage( &pyrImage2 );
431 return resNumCorrPoints;
434 /*-------------------------------------------------------------------------------------*/
435 static int icvGrowPointsAndStatus(CvMat **oldPoints,CvMat **oldStatus,CvMat *addPoints,CvMat *addStatus,int addCreateNum)
437 /* Add to existing points and status arrays new points or just grow */
438 CvMat *newOldPoint = 0;
439 CvMat *newOldStatus = 0;
440 int newTotalNumber = 0;
442 CV_FUNCNAME( "icvGrowPointsAndStatus" );
445 /* Test for errors */
446 if( oldPoints == 0 || oldStatus == 0 )
448 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
451 if( *oldPoints == 0 || *oldStatus == 0 )
453 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
456 if( !CV_IS_MAT(*oldPoints))
458 CV_ERROR( CV_StsUnsupportedFormat, "oldPoints must be a pointer to a matrix" );
461 if( !CV_IS_MASK_ARR(*oldStatus))
463 CV_ERROR( CV_StsUnsupportedFormat, "oldStatus must be a pointer to a mask array" );
467 oldNum = (*oldPoints)->cols;
470 CV_ERROR( CV_StsOutOfRange, "Number of old points must be > 0" );
473 /* Define if need number of add points */
476 if( addPoints != 0 && addStatus != 0 )
477 {/* We have aditional points */
478 if( CV_IS_MAT(addPoints) && CV_IS_MASK_ARR(addStatus) )
480 addNum = addPoints->cols;
481 if( addStatus->cols != addNum )
483 CV_ERROR( CV_StsOutOfRange, "Number of add points and statuses must be the same" );
491 numCoord = (*oldPoints)->rows;
492 newTotalNumber = oldNum + addNum + addCreateNum;
496 /* Free allocated memory */
497 newOldPoint = cvCreateMat(numCoord,newTotalNumber,CV_64F);
498 newOldStatus = cvCreateMat(1,newTotalNumber,CV_8S);
500 /* Copy old values to */
503 /* Clear all values */
505 cvZero(newOldStatus);
507 for( i = 0; i < oldNum; i++ )
510 for( currCoord = 0; currCoord < numCoord; currCoord++ )
512 cvmSet(newOldPoint,currCoord,i,cvmGet(*oldPoints,currCoord,i));
514 newOldStatus->data.ptr[i] = (*oldStatus)->data.ptr[i];
517 /* Copy additional points and statuses */
520 for( i = 0; i < addNum; i++ )
523 for( currCoord = 0; currCoord < numCoord; currCoord++ )
525 cvmSet(newOldPoint,currCoord,i+oldNum,cvmGet(addPoints,currCoord,i));
527 newOldStatus->data.ptr[i+oldNum] = addStatus->data.ptr[i];
528 //cvmSet(newOldStatus,0,i,cvmGet(addStatus,0,i));
532 /* Delete previous data */
533 cvReleaseMat(oldPoints);
534 cvReleaseMat(oldStatus);
537 *oldPoints = newOldPoint;
538 *oldStatus = newOldStatus;
543 return newTotalNumber;
546 /*-------------------------------------------------------------------------------------*/
547 static int icvRemoveDoublePoins( CvMat *oldPoints,/* Points on prev image */
548 CvMat *newPoints,/* New points */
549 CvMat *oldStatus,/* Status for old points */
552 float threshold)/* Status for new points */
555 CvMemStorage* storage = 0;
556 CvSubdiv2D* subdiv = 0;
559 int originalPoints = 0;
561 CV_FUNCNAME( "icvRemoveDoublePoins" );
564 /* Test input data */
565 if( oldPoints == 0 || newPoints == 0 ||
566 oldStatus == 0 || newStatus == 0 || origStatus == 0 )
568 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
571 if( !CV_IS_MAT(oldPoints) || !CV_IS_MAT(newPoints) )
573 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters points must be a matrices" );
576 if( !CV_IS_MASK_ARR(oldStatus) || !CV_IS_MASK_ARR(newStatus) || !CV_IS_MASK_ARR(origStatus) )
578 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters statuses must be a mask array" );
582 oldNumPoints = oldPoints->cols;
583 if( oldNumPoints < 0 )
585 CV_ERROR( CV_StsOutOfRange, "Number of oldPoints must be >= 0" );
588 if( oldStatus->cols != oldNumPoints )
590 CV_ERROR( CV_StsUnmatchedSizes, "Number of old Points and old Statuses must be the same" );
594 newNumPoints = newPoints->cols;
595 if( newNumPoints < 0 )
597 CV_ERROR( CV_StsOutOfRange, "Number of newPoints must be >= 0" );
600 if( newStatus->cols != newNumPoints )
602 CV_ERROR( CV_StsUnmatchedSizes, "Number of new Points and new Statuses must be the same" );
605 if( origStatus->cols != newNumPoints )
607 CV_ERROR( CV_StsUnmatchedSizes, "Number of new Points and new original Status must be the same" );
610 if( oldPoints->rows != 2)
612 CV_ERROR( CV_StsOutOfRange, "OldPoints must have 2 coordinates >= 0" );
615 if( newPoints->rows != 2)
617 CV_ERROR( CV_StsOutOfRange, "NewPoints must have 2 coordinates >= 0" );
620 if( oldStatus->rows != 1 || newStatus->rows != 1 || origStatus->rows != 1 )
622 CV_ERROR( CV_StsOutOfRange, "Statuses must have 1 row" );
625 /* we have points on image and wants add new points */
626 /* use subdivision for find nearest points */
628 /* Define maximum and minimum X and Y */
632 minX = minY = FLT_MAX;
633 maxX = maxY = FLT_MIN;
637 for( i = 0; i < oldNumPoints; i++ )
639 if( oldStatus->data.ptr[i] )
641 float x = (float)cvmGet(oldPoints,0,i);
642 float y = (float)cvmGet(oldPoints,1,i);
658 for( i = 0; i < newNumPoints; i++ )
660 if( newStatus->data.ptr[i] )
662 float x = (float)cvmGet(newPoints,0,i);
663 float y = (float)cvmGet(newPoints,1,i);
680 /* Creare subdivision for old image */
681 storage = cvCreateMemStorage(0);
682 // subdiv = cvCreateSubdivDelaunay2D( cvRect( 0, 0, size.width, size.height ), storage );
683 subdiv = cvCreateSubdivDelaunay2D( cvRect( cvRound(minX)-5, cvRound(minY)-5, cvRound(maxX-minX)+10, cvRound(maxY-minY)+10 ), storage );
684 seq = cvCreateSeq( 0, sizeof(*seq), sizeof(CvPoint2D32f), storage );
686 /* Insert each point from first image */
687 for( i = 0; i < oldNumPoints; i++ )
689 /* Add just exist points */
690 if( oldStatus->data.ptr[i] )
693 pt.x = (float)cvmGet(oldPoints,0,i);
694 pt.y = (float)cvmGet(oldPoints,1,i);
696 cvSubdivDelaunay2DInsert( subdiv, pt );
701 /* Find nearest points */
702 /* for each new point */
704 for( i = 0; i < newNumPoints; i++ )
707 /* Test just exist points */
708 if( newStatus->data.ptr[i] )
711 /* Let this is a good point */
716 pt.x = (float)cvmGet(newPoints,0,i);
717 pt.y = (float)cvmGet(newPoints,1,i);
719 CvSubdiv2DPoint* point = cvFindNearestPoint2D( subdiv, pt );
723 /* Test distance of found nearest point */
724 double minDistance = icvSqDist2D32f( pt, point->pt );
726 if( minDistance < threshold*threshold )
728 /* Point is double. Turn it off */
730 //newStatus->data.ptr[i] = 0;
732 /* No this is a double point */
738 originalPoints += flag;
739 origStatus->data .ptr[i] = (uchar)flag;
744 cvReleaseMemStorage( &storage );
747 return originalPoints;
752 void icvComputeProjectMatrix(CvMat* objPoints,CvMat* projPoints,CvMat* projMatr);
754 /*-------------------------------------------------------------------------------------*/
755 static void icvComputeProjectMatrixStatus(CvMat *objPoints4D,CvMat *points2,CvMat *status, CvMat *projMatr)
757 /* Compute number of good points */
758 int num = cvCountNonZero(status);
761 CvMat *objPoints = 0;
762 objPoints = cvCreateMat(4,num,CV_64F);
765 points2D = cvCreateMat(2,num,CV_64F);
771 file = fopen("d:\\test\\projStatus.txt","w");
773 int totalNum = objPoints4D->cols;
774 for( i = 0; i < totalNum; i++ )
776 fprintf(file,"%d (%d) ",i,status->data.ptr[i]);
777 if( status->data.ptr[i] )
783 X = cvmGet(objPoints4D,0,i);
784 Y = cvmGet(objPoints4D,1,i);
785 Z = cvmGet(objPoints4D,2,i);
786 W = cvmGet(objPoints4D,3,i);
788 x = cvmGet(points2,0,i);
789 y = cvmGet(points2,1,i);
790 fprintf(file,"%d (%lf %lf %lf %lf) - (%lf %lf)",i,X,Y,Z,W,x,y );
792 cvmSet(objPoints,0,currVis,cvmGet(objPoints4D,0,i));
793 cvmSet(objPoints,1,currVis,cvmGet(objPoints4D,1,i));
794 cvmSet(objPoints,2,currVis,cvmGet(objPoints4D,2,i));
795 cvmSet(objPoints,3,currVis,cvmGet(objPoints4D,3,i));
797 cvmSet(points2D,0,currVis,cvmGet(points2,0,i));
798 cvmSet(points2D,1,currVis,cvmGet(points2,1,i));
810 icvComputeProjectMatrix(objPoints,points2D,projMatr);
812 /* Free allocated memory */
813 cvReleaseMat(&objPoints);
814 cvReleaseMat(&points2D);
819 /*-------------------------------------------------------------------------------------*/
820 /* For given N images
821 we have corresponding points on N images
822 computed projection matrices
823 reconstructed 4D points
829 static void icvAddNewImageToPrevious____(
830 IplImage *newImage,//Image to add
831 IplImage *oldImage,//Previous image
832 CvMat *oldPoints,// previous 2D points on prev image (some points may be not visible)
833 CvMat *oldPntStatus,//Status for each point on prev image
834 CvMat *objPoints4D,//prev 4D points
835 CvMat *newPoints, //Points on new image corr for prev
836 CvMat *newPntStatus,// New point status for new image
837 CvMat *newFPoints2D1,//new feature points on prev image
838 CvMat *newFPoints2D2,//new feature points on new image
839 CvMat *newFPointsStatus,
842 double threshold)//New projection matrix
846 CvMat *newFPointsStatusTmp = 0;
848 //CV_FUNCNAME( "icvAddNewImageToPrevious____" );
851 /* First found correspondence points for images */
853 /* Test input params */
856 numPoints = oldPoints->cols;
858 /* Allocate memory */
860 points2 = cvCreateMat(2,numPoints,CV_64F);
861 status = cvCreateMat(1,numPoints,CV_8S);
862 newFPointsStatusTmp = cvCreateMat(1, newFPoints2D1->cols,CV_8S);
865 corrNum = icvFindCorrForGivenPoints( oldImage,/* Image 1 */
866 newImage,/* Image 2 */
871 useFilter,/*Use fundamental matrix to filter points */
872 threshold);/* Threshold for good points in filter */
874 cvCopy(status,newPntStatus);
875 cvCopy(points2,newPoints);
878 double projMatr_dat[12];
879 projMatr = cvMat(3,4,CV_64F,projMatr_dat);
882 {/* We can compute projection matrix */
883 // icvComputeProjectMatrix(objPoints4D,points2,&projMatr);
884 icvComputeProjectMatrixStatus(objPoints4D,points2,status,&projMatr);
885 cvCopy(&projMatr,newProjMatr);
887 /* Create new points and find correspondence */
888 icvCreateFeaturePoints(newImage, newFPoints2D2,newFPointsStatus);
890 /* Good if we test new points before find corr points */
892 /* Find correspondence for new found points */
893 icvFindCorrForGivenPoints( newImage,/* Image 1 */
894 oldImage,/* Image 2 */
896 newFPointsStatus,//prev status
898 newFPointsStatusTmp,//new status
899 useFilter,/*Use fundamental matrix to filter points */
900 threshold);/* Threshold for good points in filter */
902 /* We generated new points on image test for exist points */
904 /* Remove all new double points */
906 /* Find point of old image */
907 icvRemoveDoublePoins( oldPoints,/* Points on prev image */
908 newFPoints2D1,/* New points */
909 oldPntStatus,/* Status for old points */
911 newFPointsStatusTmp,//orig status
912 20);/* Status for new points */
914 /* Find double points on new image */
915 icvRemoveDoublePoins( newPoints,/* Points on prev image */
916 newFPoints2D2,/* New points */
917 newPntStatus,/* Status for old points */
919 newFPointsStatusTmp,//orig status
920 20);/* Status for new points */
924 /* Add all new good points to result */
927 /* Copy new status to old */
928 cvCopy(newFPointsStatusTmp,newFPointsStatus);
937 /* Free allocated memory */
941 /*-------------------------------------------------------------------------------------*/
945 /*-------------------------------------------------------------------------------------*/
946 static int icvDeleteSparsInPoints( int numImages,
949 CvMat *wasStatus)/* status of previous configuration */
951 /* Delete points which no exist on any of images */
952 /* numImages - number of images */
953 /* points - arrays of points for each image. Changing */
954 /* status - arrays of status for each image. Changing */
955 /* Function returns number of common points */
958 CV_FUNCNAME( "icvDeleteSparsInPoints" );
961 /* Test for errors */
964 CV_ERROR( CV_StsOutOfRange, "Number of images must be more than 0" );
967 if( points == 0 || status == 0 )
969 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
973 numPoints = points[0]->cols;
974 ////////// TESTS //////////
977 numCoord = points[0]->rows;// !!! may be number of coordinates is not correct !!!
989 for( i = 0; i < numPoints; i++ )
992 for( currImage = 0; currImage < numImages; currImage++ )
994 flag |= status[currImage]->data.ptr[i];
999 /* Current point exists */
1000 /* Copy points and status */
1001 if( currExistPoint != i )/* Copy just if different */
1003 for( currImage = 0; currImage < numImages; currImage++ )
1006 for( int currCoord = 0; currCoord < numCoord; currCoord++ )
1008 cvmSet(points[currImage],currCoord,currExistPoint, cvmGet(points[currImage],currCoord,i) );
1012 status[currImage]->data.ptr[currExistPoint] = status[currImage]->data.ptr[i];
1017 wasStatus->data.ptr[i] = 1;
1025 /* Rest of final status of points must be set to 0 */
1026 for( i = currExistPoint; i < numPoints; i++ )
1028 for( currImage = 0; currImage < numImages; currImage++ )
1030 status[currImage]->data.ptr[i] = 0;
1034 comNumber = currExistPoint;
1041 /*-------------------------------------------------------------------------------------*/
1042 void icvGrowPointsArray(CvMat **points)
1048 /*-------------------------------------------------------------------------------------*/
1049 void icvAddNewArrayPoints()
1054 /*-------------------------------------------------------------------------------------*/
1057 //////////////////////////////////////////////////////////////////////////////////////////
1058 //////////////////////////////////////////////////////////////////////////////////////////
1059 //////////////////////////////////////////////////////////////////////////////////////////
1060 //////////////////////////////////////////////////////////////////////////////////////////
1062 /* Add image to existing images and corr points */
1064 /* Returns: 1 if new image was added good */
1065 /* 0 image was not added. Not enought corr points */
1066 int AddImageToStruct( IplImage *newImage,//Image to add
1067 IplImage *oldImage,//Previous image
1068 CvMat *oldPoints,// previous 2D points on prev image (some points may be not visible)
1069 CvMat *oldPntStatus,//Status for each point on prev image
1070 CvMat *objPoints4D,//prev 4D points
1071 CvMat *newPntStatus,// New point status for new image
1072 CvMat *newPoints,//New corresponding points on new image
1073 CvMat *newPoints2D1,//new points on prev image
1074 CvMat *newPoints2D2,//new points on new image
1075 CvMat *newProjMatr);//New projection matrix
1078 /* Add new image. Create new corr points */
1079 /* Track exist points from oldImage to newImage */
1080 /* Create new vector status */
1082 int numPoints = oldPoints->cols;
1083 status = cvCreateMat(1,numPoints,CV_64F);
1085 cvConvert(pntStatus,status);
1087 int corrNum = FindCorrForGivenPoints(oldImage,newImage,oldPoints,newPoints,status);
1089 /* Status has new status of points */
1092 double projMatr_dat[12];
1093 projMatr = cvMat(3,4,CV_64F,projMatr_dat);
1095 /* If number of corr points is 6 or more can compute projection matrix */
1098 /* Compute projection matrix for new image using corresponding points */
1099 icvComputeProjectMatrix(objPoints4D,newPoints,&projMatr);
1102 /* Create new points and find correspondence */
1103 int num = FindFeaturePoints(newImage, &tmpPoints);
1107 newPoints = cvCreateMat(2,num,CV_64F);
1109 status = cvCreateMat(1,num,CV_64F);
1110 /* Set status for all points */
1112 for( i = 0; i < num; i++ )
1114 cvmSet(status,0,i,1.0);
1117 int corrNum2 = FindCorrForGivenPoints(oldImage,newImage,tmpPoints,newPoints,status);
1119 /* !!! Filter points using projection matrices or not ??? */
1121 /* !!! Need to filter nearest points */
1123 /* Add new found points to exist points and optimize again */
1127 /* add new status to old status */
1136 /* No new points were found */
1141 /* We can't compute projection matrix for new image */