1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
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.
8 //
9 //
10 //                        Intel License Agreement
11 //                For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
15 //
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
18 //
19 //   * Redistribution's of source code must retain the above copyright notice,
20 //     this list of conditions and the following disclaimer.
21 //
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.
25 //
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.
28 //
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.
39 //
40 //M*/
41 
42 #include "_cvaux.h"
43 //#include "cvtypes.h"
44 //#include <float.h>
45 //#include <limits.h>
46 //#include "cv.h"
47 //#include "highgui.h"
48 
49 #include <stdio.h>
50 
51 /* Valery Mosyagin */
52 
53 /* ===== Function for find corresponding between images ===== */
54 
55 /* Create feature points on image and return number of them. Array points fills by found points */
icvCreateFeaturePoints(IplImage * image,CvMat * points,CvMat * status)56 int icvCreateFeaturePoints(IplImage *image, CvMat *points, CvMat *status)
57 {
58     int foundFeaturePoints = 0;
59     IplImage *grayImage = 0;
60     IplImage *eigImage = 0;
61     IplImage *tmpImage = 0;
62     CvPoint2D32f *cornerPoints = 0;
63 
64     CV_FUNCNAME( "icvFeatureCreatePoints" );
65     __BEGIN__;
66 
67     /* Test for errors */
68     if( image == 0 || points == 0 )
69     {
70         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
71     }
72 
73     /* Test image size */
74     int w,h;
75     w = image->width;
76     h = image->height;
77 
78     if( w <= 0 || h <= 0)
79     {
80         CV_ERROR( CV_StsOutOfRange, "Size of image must be > 0" );
81     }
82 
83     /* Test for matrices */
84     if( !CV_IS_MAT(points) )
85     {
86         CV_ERROR( CV_StsUnsupportedFormat, "Input parameter points must be a matrix" );
87     }
88 
89     int needNumPoints;
90     needNumPoints = points->cols;
91     if( needNumPoints <= 0 )
92     {
93         CV_ERROR( CV_StsOutOfRange, "Number of need points must be > 0" );
94     }
95 
96     if( points->rows != 2 )
97     {
98         CV_ERROR( CV_StsOutOfRange, "Number of point coordinates must be == 2" );
99     }
100 
101     if( status != 0 )
102     {
103         /* If status matrix exist test it for correct */
104         if( !CV_IS_MASK_ARR(status) )
105         {
106             CV_ERROR( CV_StsUnsupportedFormat, "Statuses must be a mask arrays" );
107         }
108 
109         if( status->cols != needNumPoints )
110         {
111             CV_ERROR( CV_StsUnmatchedSizes, "Size of points and statuses must be the same" );
112         }
113 
114         if( status->rows !=1 )
115         {
116             CV_ERROR( CV_StsUnsupportedFormat, "Number of rows of status must be 1" );
117         }
118     }
119 
120     /* Create temporary images */
121     CV_CALL( grayImage = cvCreateImage(cvSize(w,h), 8,1) );
122     CV_CALL( eigImage   = cvCreateImage(cvSize(w,h),32,1) );
123     CV_CALL( tmpImage   = cvCreateImage(cvSize(w,h),32,1) );
124 
125     /* Create points */
126     CV_CALL( cornerPoints = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f) * needNumPoints) );
127 
128     int foundNum;
129     double quality;
130     double minDist;
131 
132     cvCvtColor(image,grayImage, CV_BGR2GRAY);
133 
134     foundNum = needNumPoints;
135     quality = 0.01;
136     minDist = 5;
137     cvGoodFeaturesToTrack(grayImage, eigImage, tmpImage, cornerPoints, &foundNum, quality, minDist);
138 
139     /* Copy found points to result */
140     int i;
141     for( i = 0; i < foundNum; i++ )
142     {
143         cvmSet(points,0,i,cornerPoints[i].x);
144         cvmSet(points,1,i,cornerPoints[i].y);
145     }
146 
147     /* Set status if need */
148     if( status )
149     {
150         for( i = 0; i < foundNum; i++ )
151         {
152             status->data.ptr[i] = 1;
153         }
154 
155         for( i = foundNum; i < needNumPoints; i++ )
156         {
157             status->data.ptr[i] = 0;
158         }
159     }
160 
161     foundFeaturePoints = foundNum;
162 
163     __END__;
164 
165     /* Free allocated memory */
166     cvReleaseImage(&grayImage);
167     cvReleaseImage(&eigImage);
168     cvReleaseImage(&tmpImage);
169     cvFree(&cornerPoints);
170 
171     return foundFeaturePoints;
172 }
173 
174 /*-------------------------------------------------------------------------------------*/
175 
176 /* For given points1 (with pntStatus) on image1 finds corresponding points2 on image2 and set pntStatus2 for them */
177 /* Returns number of corresponding points */
icvFindCorrForGivenPoints(IplImage * image1,IplImage * image2,CvMat * points1,CvMat * pntStatus1,CvMat * points2,CvMat * pntStatus2,int useFilter,double threshold)178 int icvFindCorrForGivenPoints( IplImage *image1,/* Image 1 */
179                                 IplImage *image2,/* Image 2 */
180                                 CvMat *points1,
181                                 CvMat *pntStatus1,
182                                 CvMat *points2,
183                                 CvMat *pntStatus2,
184                                 int useFilter,/*Use fundamental matrix to filter points */
185                                 double threshold)/* Threshold for good points in filter */
186 {
187     int resNumCorrPoints = 0;
188     CvPoint2D32f* cornerPoints1 = 0;
189     CvPoint2D32f* cornerPoints2 = 0;
190     char*  status = 0;
191     float* errors = 0;
192     CvMat* tmpPoints1 = 0;
193     CvMat* tmpPoints2 = 0;
194     CvMat* pStatus = 0;
195     IplImage *grayImage1 = 0;
196     IplImage *grayImage2 = 0;
197     IplImage *pyrImage1 = 0;
198     IplImage *pyrImage2 = 0;
199 
200     CV_FUNCNAME( "icvFindCorrForGivenPoints" );
201     __BEGIN__;
202 
203     /* Test input data for errors */
204 
205     /* Test for null pointers */
206     if( image1     == 0 || image2     == 0 ||
207         points1    == 0 || points2    == 0 ||
208         pntStatus1 == 0 || pntStatus2 == 0)
209     {
210         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
211     }
212 
213     /* Test image size */
214     int w,h;
215     w = image1->width;
216     h = image1->height;
217 
218     if( w <= 0 || h <= 0)
219     {
220         CV_ERROR( CV_StsOutOfRange, "Size of image1 must be > 0" );
221     }
222 
223     if( image2->width != w || image2->height != h )
224     {
225         CV_ERROR( CV_StsUnmatchedSizes, "Size of images must be the same" );
226     }
227 
228     /* Test for matrices */
229     if( !CV_IS_MAT(points1)    || !CV_IS_MAT(points2) ||
230         !CV_IS_MAT(pntStatus1) || !CV_IS_MAT(pntStatus2) )
231     {
232         CV_ERROR( CV_StsUnsupportedFormat, "Input parameters (points and status) must be a matrices" );
233     }
234 
235     /* Test type of status matrices */
236     if( !CV_IS_MASK_ARR(pntStatus1) || !CV_IS_MASK_ARR(pntStatus2) )
237     {
238         CV_ERROR( CV_StsUnsupportedFormat, "Statuses must be a mask arrays" );
239     }
240 
241     /* Test number of points */
242     int numPoints;
243     numPoints = points1->cols;
244 
245     if( numPoints <= 0 )
246     {
247         CV_ERROR( CV_StsOutOfRange, "Number of points1 must be > 0" );
248     }
249 
250     if( points2->cols != numPoints || pntStatus1->cols != numPoints || pntStatus2->cols != numPoints )
251     {
252         CV_ERROR( CV_StsUnmatchedSizes, "Number of points and statuses must be the same" );
253     }
254 
255     if( points1->rows != 2 || points2->rows != 2 )
256     {
257         CV_ERROR( CV_StsOutOfRange, "Number of points coordinates must be 2" );
258     }
259 
260     if( pntStatus1->rows != 1 || pntStatus2->rows != 1 )
261     {
262         CV_ERROR( CV_StsOutOfRange, "Status must be a matrix 1xN" );
263     }
264     /* ----- End test ----- */
265 
266 
267     /* Compute number of visible points on image1 */
268     int numVisPoints;
269     numVisPoints = cvCountNonZero(pntStatus1);
270 
271     if( numVisPoints > 0 )
272     {
273         /* Create temporary images */
274         /* We must use iplImage againts hughgui images */
275 
276 /*
277         CvvImage grayImage1;
278         CvvImage grayImage2;
279         CvvImage pyrImage1;
280         CvvImage pyrImage2;
281 */
282 
283         /* Create Ipl images */
284         CV_CALL( grayImage1 = cvCreateImage(cvSize(w,h),8,1) );
285         CV_CALL( grayImage2 = cvCreateImage(cvSize(w,h),8,1) );
286         CV_CALL( pyrImage1  = cvCreateImage(cvSize(w,h),8,1) );
287         CV_CALL( pyrImage2  = cvCreateImage(cvSize(w,h),8,1) );
288 
289         CV_CALL( cornerPoints1 = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f)*numVisPoints) );
290         CV_CALL( cornerPoints2 = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f)*numVisPoints) );
291         CV_CALL( status = (char*)cvAlloc( sizeof(char)*numVisPoints) );
292         CV_CALL( errors = (float*)cvAlloc( 2 * sizeof(float)*numVisPoints) );
293 
294         int i;
295         for( i = 0; i < numVisPoints; i++ )
296         {
297             status[i] = 1;
298         }
299 
300         /* !!! Need test creation errors */
301         /*
302         if( !grayImage1.Create(w,h,8)) EXIT;
303         if( !grayImage2.Create(w,h,8)) EXIT;
304         if( !pyrImage1. Create(w,h,8)) EXIT;
305         if( !pyrImage2. Create(w,h,8)) EXIT;
306         */
307 
308         cvCvtColor(image1,grayImage1,CV_BGR2GRAY);
309         cvCvtColor(image2,grayImage2,CV_BGR2GRAY);
310 
311         /*
312         grayImage1.CopyOf(image1,0);
313         grayImage2.CopyOf(image2,0);
314         */
315 
316         /* Copy points good points from input data */
317         uchar *stat1 = pntStatus1->data.ptr;
318         uchar *stat2 = pntStatus2->data.ptr;
319 
320         int curr = 0;
321         for( i = 0; i < numPoints; i++ )
322         {
323             if( stat1[i] )
324             {
325                 cornerPoints1[curr].x = (float)cvmGet(points1,0,i);
326                 cornerPoints1[curr].y = (float)cvmGet(points1,1,i);
327                 curr++;
328             }
329         }
330 
331         /* Define number of levels of pyramid */
332         cvCalcOpticalFlowPyrLK( grayImage1, grayImage2,
333                                 pyrImage1, pyrImage2,
334                                 cornerPoints1, cornerPoints2,
335                                 numVisPoints, cvSize(10,10), 3,
336                                 status, errors,
337                                 cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03),
338                                 0/*CV_LKFLOW_PYR_A_READY*/ );
339 
340 
341         memset(stat2,0,sizeof(uchar)*numPoints);
342 
343         int currVis = 0;
344         int totalCorns = 0;
345 
346         /* Copy new points and set status */
347         /* stat1 may not be the same as stat2 */
348         for( i = 0; i < numPoints; i++ )
349         {
350             if( stat1[i] )
351             {
352                 if( status[currVis] && errors[currVis] < 1000 )
353                 {
354                     stat2[i] = 1;
355                     cvmSet(points2,0,i,cornerPoints2[currVis].x);
356                     cvmSet(points2,1,i,cornerPoints2[currVis].y);
357                     totalCorns++;
358                 }
359                 currVis++;
360             }
361         }
362 
363         resNumCorrPoints = totalCorns;
364 
365         /* Filter points using RANSAC */
366         if( useFilter )
367         {
368             resNumCorrPoints = 0;
369             /* Use RANSAC filter for found points */
370             if( totalCorns > 7 )
371             {
372                 /* Create array with good points only */
373                 CV_CALL( tmpPoints1 = cvCreateMat(2,totalCorns,CV_64F) );
374                 CV_CALL( tmpPoints2 = cvCreateMat(2,totalCorns,CV_64F) );
375 
376                 /* Copy just good points */
377                 int currPoint = 0;
378                 for( i = 0; i < numPoints; i++ )
379                 {
380                     if( stat2[i] )
381                     {
382                         cvmSet(tmpPoints1,0,currPoint,cvmGet(points1,0,i));
383                         cvmSet(tmpPoints1,1,currPoint,cvmGet(points1,1,i));
384 
385                         cvmSet(tmpPoints2,0,currPoint,cvmGet(points2,0,i));
386                         cvmSet(tmpPoints2,1,currPoint,cvmGet(points2,1,i));
387 
388                         currPoint++;
389                     }
390                 }
391 
392                 /* Compute fundamental matrix */
393                 CvMat fundMatr;
394                 double fundMatr_dat[9];
395                 fundMatr = cvMat(3,3,CV_64F,fundMatr_dat);
396 
397                 CV_CALL( pStatus = cvCreateMat(1,totalCorns,CV_32F) );
398 
399                 int num = cvFindFundamentalMat(tmpPoints1,tmpPoints2,&fundMatr,CV_FM_RANSAC,threshold,0.99,pStatus);
400                 if( num > 0 )
401                 {
402                     int curr = 0;
403                     /* Set final status for points2 */
404                     for( i = 0; i < numPoints; i++ )
405                     {
406                         if( stat2[i] )
407                         {
408                             if( cvmGet(pStatus,0,curr) == 0 )
409                             {
410                                 stat2[i] = 0;
411                             }
412                             curr++;
413                         }
414                     }
415                     resNumCorrPoints = curr;
416                 }
417             }
418         }
419     }
420 
421     __END__;
422 
423     /* Free allocated memory */
424     cvFree(&cornerPoints1);
425     cvFree(&cornerPoints2);
426     cvFree(&status);
427     cvFree(&errors);
428     cvFree(&tmpPoints1);
429     cvFree(&tmpPoints2);
430     cvReleaseMat( &pStatus );
431     cvReleaseImage( &grayImage1 );
432     cvReleaseImage( &grayImage2 );
433     cvReleaseImage( &pyrImage1 );
434     cvReleaseImage( &pyrImage2 );
435 
436     return resNumCorrPoints;
437 }
438 /*-------------------------------------------------------------------------------------*/
icvGrowPointsAndStatus(CvMat ** oldPoints,CvMat ** oldStatus,CvMat * addPoints,CvMat * addStatus,int addCreateNum)439 int icvGrowPointsAndStatus(CvMat **oldPoints,CvMat **oldStatus,CvMat *addPoints,CvMat *addStatus,int addCreateNum)
440 {
441     /* Add to existing points and status arrays new points or just grow */
442     CvMat *newOldPoint  = 0;
443     CvMat *newOldStatus = 0;
444     int newTotalNumber = 0;
445 
446     CV_FUNCNAME( "icvGrowPointsAndStatus" );
447     __BEGIN__;
448 
449     /* Test for errors */
450     if( oldPoints == 0 || oldStatus == 0 )
451     {
452         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
453     }
454 
455     if( *oldPoints == 0 || *oldStatus == 0 )
456     {
457         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
458     }
459 
460     if( !CV_IS_MAT(*oldPoints))
461     {
462         CV_ERROR( CV_StsUnsupportedFormat, "oldPoints must be a pointer to a matrix" );
463     }
464 
465     if( !CV_IS_MASK_ARR(*oldStatus))
466     {
467         CV_ERROR( CV_StsUnsupportedFormat, "oldStatus must be a pointer to a mask array" );
468     }
469 
470     int oldNum;
471     oldNum = (*oldPoints)->cols;
472     if( oldNum < 1 )
473     {
474         CV_ERROR( CV_StsOutOfRange, "Number of old points must be > 0" );
475     }
476 
477     /* Define if need number of add points */
478     int addNum;
479     addNum = 0;
480     if( addPoints != 0 && addStatus != 0 )
481     {/* We have aditional points */
482         if( CV_IS_MAT(addPoints) && CV_IS_MASK_ARR(addStatus) )
483         {
484             addNum = addPoints->cols;
485             if( addStatus->cols != addNum )
486             {
487                 CV_ERROR( CV_StsOutOfRange, "Number of add points and statuses must be the same" );
488             }
489         }
490     }
491 
492     /*  */
493 
494     int numCoord;
495     numCoord = (*oldPoints)->rows;
496     newTotalNumber = oldNum + addNum + addCreateNum;
497 
498     if( newTotalNumber )
499     {
500         /* Free allocated memory */
501         newOldPoint  = cvCreateMat(numCoord,newTotalNumber,CV_64F);
502         newOldStatus = cvCreateMat(1,newTotalNumber,CV_8S);
503 
504         /* Copy old values to  */
505         int i;
506 
507         /* Clear all values */
508         cvZero(newOldPoint);
509         cvZero(newOldStatus);
510 
511         for( i = 0; i < oldNum; i++ )
512         {
513             int currCoord;
514             for( currCoord = 0; currCoord < numCoord; currCoord++ )
515             {
516                 cvmSet(newOldPoint,currCoord,i,cvmGet(*oldPoints,currCoord,i));
517             }
518             newOldStatus->data.ptr[i] = (*oldStatus)->data.ptr[i];
519         }
520 
521         /* Copy additional points and statuses */
522         if( addNum )
523         {
524             for( i = 0; i < addNum; i++ )
525             {
526                 int currCoord;
527                 for( currCoord = 0; currCoord < numCoord; currCoord++ )
528                 {
529                     cvmSet(newOldPoint,currCoord,i+oldNum,cvmGet(addPoints,currCoord,i));
530                 }
531                 newOldStatus->data.ptr[i+oldNum] = addStatus->data.ptr[i];
532                 //cvmSet(newOldStatus,0,i,cvmGet(addStatus,0,i));
533             }
534         }
535 
536         /* Delete previous data */
537         cvReleaseMat(oldPoints);
538         cvReleaseMat(oldStatus);
539 
540         /* copy pointers */
541         *oldPoints  = newOldPoint;
542         *oldStatus = newOldStatus;
543 
544     }
545     __END__;
546 
547     return newTotalNumber;
548 }
549 /*-------------------------------------------------------------------------------------*/
icvRemoveDoublePoins(CvMat * oldPoints,CvMat * newPoints,CvMat * oldStatus,CvMat * newStatus,CvMat * origStatus,float threshold)550 int icvRemoveDoublePoins(   CvMat *oldPoints,/* Points on prev image */
551                             CvMat *newPoints,/* New points */
552                             CvMat *oldStatus,/* Status for old points */
553                             CvMat *newStatus,
554                             CvMat *origStatus,
555                             float threshold)/* Status for new points */
556 {
557 
558     CvMemStorage* storage = 0;
559     CvSubdiv2D* subdiv = 0;
560     CvSeq* seq = 0;
561 
562     int originalPoints = 0;
563 
564     CV_FUNCNAME( "icvRemoveDoublePoins" );
565     __BEGIN__;
566 
567     /* Test input data */
568     if( oldPoints == 0 || newPoints == 0 ||
569         oldStatus == 0 || newStatus == 0 || origStatus == 0 )
570     {
571         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
572     }
573 
574     if( !CV_IS_MAT(oldPoints) || !CV_IS_MAT(newPoints) )
575     {
576         CV_ERROR( CV_StsUnsupportedFormat, "Input parameters points must be a matrices" );
577     }
578 
579     if( !CV_IS_MASK_ARR(oldStatus) || !CV_IS_MASK_ARR(newStatus) || !CV_IS_MASK_ARR(origStatus) )
580     {
581         CV_ERROR( CV_StsUnsupportedFormat, "Input parameters statuses must be a mask array" );
582     }
583 
584     int oldNumPoints;
585     oldNumPoints = oldPoints->cols;
586     if( oldNumPoints < 0 )
587     {
588         CV_ERROR( CV_StsOutOfRange, "Number of oldPoints must be >= 0" );
589     }
590 
591     if( oldStatus->cols != oldNumPoints )
592     {
593         CV_ERROR( CV_StsUnmatchedSizes, "Number of old Points and old Statuses must be the same" );
594     }
595 
596     int newNumPoints;
597     newNumPoints = newPoints->cols;
598     if( newNumPoints < 0 )
599     {
600         CV_ERROR( CV_StsOutOfRange, "Number of newPoints must be >= 0" );
601     }
602 
603     if( newStatus->cols != newNumPoints )
604     {
605         CV_ERROR( CV_StsUnmatchedSizes, "Number of new Points and new Statuses must be the same" );
606     }
607 
608     if( origStatus->cols != newNumPoints )
609     {
610         CV_ERROR( CV_StsUnmatchedSizes, "Number of new Points and new original Status must be the same" );
611     }
612 
613     if( oldPoints->rows != 2)
614     {
615         CV_ERROR( CV_StsOutOfRange, "OldPoints must have 2 coordinates >= 0" );
616     }
617 
618     if( newPoints->rows != 2)
619     {
620         CV_ERROR( CV_StsOutOfRange, "NewPoints must have 2 coordinates >= 0" );
621     }
622 
623     if( oldStatus->rows != 1 || newStatus->rows != 1 || origStatus->rows != 1 )
624     {
625         CV_ERROR( CV_StsOutOfRange, "Statuses must have 1 row" );
626     }
627 
628     /* we have points on image and wants add new points */
629     /* use subdivision for find nearest points */
630 
631     /* Define maximum and minimum X and Y */
632     float minX,minY;
633     float maxX,maxY;
634 
635     minX = minY = FLT_MAX;
636     maxX = maxY = FLT_MIN;
637 
638     int i;
639 
640     for( i = 0; i < oldNumPoints; i++ )
641     {
642         if( oldStatus->data.ptr[i] )
643         {
644             float x = (float)cvmGet(oldPoints,0,i);
645             float y = (float)cvmGet(oldPoints,1,i);
646 
647             if( x < minX )
648                 minX = x;
649 
650             if( x > maxX )
651                 maxX = x;
652 
653             if( y < minY )
654                 minY = y;
655 
656             if( y > maxY )
657                 maxY = y;
658         }
659     }
660 
661     for( i = 0; i < newNumPoints; i++ )
662     {
663         if( newStatus->data.ptr[i] )
664         {
665             float x = (float)cvmGet(newPoints,0,i);
666             float y = (float)cvmGet(newPoints,1,i);
667 
668             if( x < minX )
669                 minX = x;
670 
671             if( x > maxX )
672                 maxX = x;
673 
674             if( y < minY )
675                 minY = y;
676 
677             if( y > maxY )
678                 maxY = y;
679         }
680     }
681 
682 
683     /* Creare subdivision for old image */
684     storage = cvCreateMemStorage(0);
685 //    subdiv = cvCreateSubdivDelaunay2D( cvRect( 0, 0, size.width, size.height ), storage );
686     subdiv = cvCreateSubdivDelaunay2D( cvRect( cvRound(minX)-5, cvRound(minY)-5, cvRound(maxX-minX)+10, cvRound(maxY-minY)+10 ), storage );
687     seq = cvCreateSeq( 0, sizeof(*seq), sizeof(CvPoint2D32f), storage );
688 
689     /* Insert each point from first image */
690     for( i = 0; i < oldNumPoints; i++ )
691     {
692         /* Add just exist points */
693         if( oldStatus->data.ptr[i] )
694         {
695             CvPoint2D32f pt;
696             pt.x = (float)cvmGet(oldPoints,0,i);
697             pt.y = (float)cvmGet(oldPoints,1,i);
698 
699             CvSubdiv2DPoint* point;
700             point = cvSubdivDelaunay2DInsert( subdiv, pt );
701         }
702     }
703 
704 
705     /* Find nearest points */
706     /* for each new point */
707     int flag;
708     for( i = 0; i < newNumPoints; i++ )
709     {
710         flag = 0;
711         /* Test just exist points */
712         if( newStatus->data.ptr[i] )
713         {
714             flag = 1;
715             /* Let this is a good point */
716             //originalPoints++;
717 
718             CvPoint2D32f pt;
719 
720             pt.x = (float)cvmGet(newPoints,0,i);
721             pt.y = (float)cvmGet(newPoints,1,i);
722 
723             CvSubdiv2DPoint* point = cvFindNearestPoint2D( subdiv, pt );
724 
725             if( point )
726             {
727                 /* Test distance of found nearest point */
728                 double minDistance = icvSqDist2D32f( pt, point->pt );
729 
730                 if( minDistance < threshold*threshold )
731                 {
732                     /* Point is double. Turn it off */
733                     /* Set status */
734                     //newStatus->data.ptr[i] = 0;
735 
736                     /* No this is a double point */
737                     //originalPoints--;
738                     flag = 0;
739                 }
740             }
741         }
742         originalPoints += flag;
743         origStatus->data .ptr[i] = (uchar)flag;
744     }
745 
746     __END__;
747 
748     cvReleaseMemStorage( &storage );
749 
750 
751     return originalPoints;
752 
753 
754 }
755 
756 void icvComputeProjectMatrix(CvMat* objPoints,CvMat* projPoints,CvMat* projMatr);
757 
758 /*-------------------------------------------------------------------------------------*/
icvComputeProjectMatrixStatus(CvMat * objPoints4D,CvMat * points2,CvMat * status,CvMat * projMatr)759 void icvComputeProjectMatrixStatus(CvMat *objPoints4D,CvMat *points2,CvMat *status, CvMat *projMatr)
760 {
761     /* Compute number of good points */
762     int num = cvCountNonZero(status);
763 
764     /* Create arrays */
765     CvMat *objPoints = 0;
766     objPoints = cvCreateMat(4,num,CV_64F);
767 
768     CvMat *points2D = 0;
769     points2D = cvCreateMat(2,num,CV_64F);
770 
771     int currVis = 0;
772     int i;
773 #if 1
774     FILE *file;
775     file = fopen("d:\\test\\projStatus.txt","w");
776 #endif
777     int totalNum = objPoints4D->cols;
778     for( i = 0; i < totalNum; i++ )
779     {
780         fprintf(file,"%d (%d) ",i,status->data.ptr[i]);
781         if( status->data.ptr[i] )
782         {
783 
784 #if 1
785             double X,Y,Z,W;
786             double x,y;
787             X = cvmGet(objPoints4D,0,i);
788             Y = cvmGet(objPoints4D,1,i);
789             Z = cvmGet(objPoints4D,2,i);
790             W = cvmGet(objPoints4D,3,i);
791 
792             x = cvmGet(points2,0,i);
793             y = cvmGet(points2,1,i);
794             fprintf(file,"%d (%lf %lf %lf %lf) - (%lf %lf)",i,X,Y,Z,W,x,y );
795 #endif
796             cvmSet(objPoints,0,currVis,cvmGet(objPoints4D,0,i));
797             cvmSet(objPoints,1,currVis,cvmGet(objPoints4D,1,i));
798             cvmSet(objPoints,2,currVis,cvmGet(objPoints4D,2,i));
799             cvmSet(objPoints,3,currVis,cvmGet(objPoints4D,3,i));
800 
801             cvmSet(points2D,0,currVis,cvmGet(points2,0,i));
802             cvmSet(points2D,1,currVis,cvmGet(points2,1,i));
803 
804             currVis++;
805         }
806 
807         fprintf(file,"\n");
808     }
809 
810 #if 1
811     fclose(file);
812 #endif
813 
814     icvComputeProjectMatrix(objPoints,points2D,projMatr);
815 
816     /* Free allocated memory */
817     cvReleaseMat(&objPoints);
818     cvReleaseMat(&points2D);
819 }
820 
821 
822 
823 /*-------------------------------------------------------------------------------------*/
824 /* For given N images
825  we have corresponding points on N images
826  computed projection matrices
827  reconstructed 4D points
828 
829   we must to compute
830 
831 
832 */
833 
icvAddNewImageToPrevious____(IplImage * newImage,IplImage * oldImage,CvMat * oldPoints,CvMat * oldPntStatus,CvMat * objPoints4D,CvMat * newPoints,CvMat * newPntStatus,CvMat * newFPoints2D1,CvMat * newFPoints2D2,CvMat * newFPointsStatus,CvMat * newProjMatr,int useFilter,double threshold)834 void icvAddNewImageToPrevious____(
835                                     IplImage *newImage,//Image to add
836                                     IplImage *oldImage,//Previous image
837                                     CvMat *oldPoints,// previous 2D points on prev image (some points may be not visible)
838                                     CvMat *oldPntStatus,//Status for each point on prev image
839                                     CvMat *objPoints4D,//prev 4D points
840                                     CvMat *newPoints,  //Points on new image corr for prev
841                                     CvMat *newPntStatus,// New point status for new image
842                                     CvMat *newFPoints2D1,//new feature points on prev image
843                                     CvMat *newFPoints2D2,//new feature points on new image
844                                     CvMat *newFPointsStatus,
845                                     CvMat *newProjMatr,
846                                     int useFilter,
847                                     double threshold)//New projection matrix
848 {
849     CvMat *points2 = 0;
850     CvMat *status = 0;
851     CvMat *newFPointsStatusTmp = 0;
852 
853     //CV_FUNCNAME( "icvAddNewImageToPrevious____" );
854     __BEGIN__;
855 
856     /* First found correspondence points for images */
857 
858     /* Test input params */
859 
860     int numPoints;
861     numPoints = oldPoints->cols;
862 
863     /* Allocate memory */
864 
865     points2 = cvCreateMat(2,numPoints,CV_64F);
866     status = cvCreateMat(1,numPoints,CV_8S);
867     newFPointsStatusTmp = cvCreateMat(1, newFPoints2D1->cols,CV_8S);
868 
869     int corrNum;
870     corrNum = icvFindCorrForGivenPoints(    oldImage,/* Image 1 */
871                                             newImage,/* Image 2 */
872                                             oldPoints,
873                                             oldPntStatus,
874                                             points2,
875                                             status,
876                                             useFilter,/*Use fundamental matrix to filter points */
877                                             threshold);/* Threshold for good points in filter */
878 
879     cvCopy(status,newPntStatus);
880     cvCopy(points2,newPoints);
881 
882     CvMat projMatr;
883     double projMatr_dat[12];
884     projMatr = cvMat(3,4,CV_64F,projMatr_dat);
885 
886     if( corrNum >= 6 )
887     {/* We can compute projection matrix */
888 //        icvComputeProjectMatrix(objPoints4D,points2,&projMatr);
889         icvComputeProjectMatrixStatus(objPoints4D,points2,status,&projMatr);
890         cvCopy(&projMatr,newProjMatr);
891 
892         /* Create new points and find correspondence */
893         icvCreateFeaturePoints(newImage, newFPoints2D2,newFPointsStatus);
894 
895         /* Good if we test new points before find corr points */
896 
897         /* Find correspondence for new found points */
898         icvFindCorrForGivenPoints( newImage,/* Image 1 */
899                                    oldImage,/* Image 2 */
900                                    newFPoints2D2,
901                                    newFPointsStatus,//prev status
902                                    newFPoints2D1,
903                                    newFPointsStatusTmp,//new status
904                                    useFilter,/*Use fundamental matrix to filter points */
905                                    threshold);/* Threshold for good points in filter */
906 
907         /* We generated new points on image test for exist points */
908 
909         /* Remove all new double points */
910 
911         int origNum;
912         /* Find point of old image */
913         origNum = icvRemoveDoublePoins( oldPoints,/* Points on prev image */
914                                         newFPoints2D1,/* New points */
915                                         oldPntStatus,/* Status for old points */
916                                         newFPointsStatusTmp,
917                                         newFPointsStatusTmp,//orig status
918                                         20);/* Status for new points */
919 
920         /* Find double points on new image */
921         origNum = icvRemoveDoublePoins( newPoints,/* Points on prev image */
922                                         newFPoints2D2,/* New points */
923                                         newPntStatus,/* Status for old points */
924                                         newFPointsStatusTmp,
925                                         newFPointsStatusTmp,//orig status
926                                         20);/* Status for new points */
927 
928 
929 
930         /* Add all new good points to result */
931 
932 
933         /* Copy new status to old */
934         cvCopy(newFPointsStatusTmp,newFPointsStatus);
935 
936 
937     }
938 
939 
940 
941     __END__;
942 
943     /* Free allocated memory */
944 
945     return;
946 }
947 /*-------------------------------------------------------------------------------------*/
948 //int icvDelete//
949 //CreateGood
950 
951 /*-------------------------------------------------------------------------------------*/
icvDeleteSparsInPoints(int numImages,CvMat ** points,CvMat ** status,CvMat * wasStatus)952 int icvDeleteSparsInPoints(  int numImages,
953                              CvMat **points,
954                              CvMat **status,
955                              CvMat *wasStatus)/* status of previous configuration */
956 {
957     /* Delete points which no exist on any of images */
958     /* numImages - number of images */
959     /* points - arrays of points for each image. Changing */
960     /* status - arrays of status for each image. Changing */
961     /* Function returns number of common points */
962 
963     int comNumber = 0;
964     CV_FUNCNAME( "icvDeleteSparsInPoints" );
965     __BEGIN__;
966 
967     /* Test for errors */
968     if( numImages < 1 )
969     {
970         CV_ERROR( CV_StsOutOfRange, "Number of images must be more than 0" );
971     }
972 
973     if( points == 0 || status == 0 )
974     {
975         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
976     }
977     int numPoints;
978 
979     numPoints = points[0]->cols;
980     ////////// TESTS //////////
981 
982     int numCoord;
983     numCoord = points[0]->rows;// !!! may be number of coordinates is not correct !!!
984 
985     int i;
986     int currExistPoint;
987     currExistPoint = 0;
988 
989     if( wasStatus )
990     {
991         cvZero(wasStatus);
992     }
993 
994     int currImage;
995     for( i = 0; i < numPoints; i++ )
996     {
997         int flag = 0;
998         for( currImage = 0; currImage < numImages; currImage++ )
999         {
1000             flag |= status[currImage]->data.ptr[i];
1001         }
1002 
1003         if( flag )
1004         {
1005             /* Current point exists */
1006             /* Copy points and status */
1007             if( currExistPoint != i )/* Copy just if different */
1008             {
1009                 for( currImage = 0; currImage < numImages; currImage++ )
1010                 {
1011                     /* Copy points */
1012                     for( int currCoord = 0; currCoord < numCoord; currCoord++ )
1013                     {
1014                         cvmSet(points[currImage],currCoord,currExistPoint, cvmGet(points[currImage],currCoord,i) );
1015                     }
1016 
1017                     /* Copy status */
1018                     status[currImage]->data.ptr[currExistPoint] = status[currImage]->data.ptr[i];
1019                 }
1020             }
1021             if( wasStatus )
1022             {
1023                 wasStatus->data.ptr[i] = 1;
1024             }
1025 
1026             currExistPoint++;
1027 
1028         }
1029     }
1030 
1031     /* Rest of final status of points must be set to 0  */
1032     for( i = currExistPoint; i < numPoints; i++ )
1033     {
1034         for( currImage = 0; currImage < numImages; currImage++ )
1035         {
1036             status[currImage]->data.ptr[i] = 0;
1037         }
1038     }
1039 
1040     comNumber = currExistPoint;
1041 
1042     __END__;
1043     return comNumber;
1044 }
1045 
1046 #if 0
1047 /*-------------------------------------------------------------------------------------*/
1048 void icvGrowPointsArray(CvMat **points)
1049 {
1050 
1051 
1052 }
1053 
1054 /*-------------------------------------------------------------------------------------*/
1055 void icvAddNewArrayPoints()
1056 {
1057 
1058 }
1059 
1060 /*-------------------------------------------------------------------------------------*/
1061 #endif
1062 
1063 //////////////////////////////////////////////////////////////////////////////////////////
1064 //////////////////////////////////////////////////////////////////////////////////////////
1065 //////////////////////////////////////////////////////////////////////////////////////////
1066 //////////////////////////////////////////////////////////////////////////////////////////
1067 
1068 /* Add image to existing images and corr points */
1069 #if 0
1070 /* Returns: 1 if new image was added good */
1071 /*          0 image was not added. Not enought corr points */
1072 int AddImageToStruct(  IplImage *newImage,//Image to add
1073                         IplImage *oldImage,//Previous image
1074                         CvMat *oldPoints,// previous 2D points on prev image (some points may be not visible)
1075                         CvMat *oldPntStatus,//Status for each point on prev image
1076                         CvMat *objPoints4D,//prev 4D points
1077                         CvMat *newPntStatus,// New point status for new image
1078                         CvMat *newPoints,//New corresponding points on new image
1079                         CvMat *newPoints2D1,//new points on prev image
1080                         CvMat *newPoints2D2,//new points on new image
1081                         CvMat *newProjMatr);//New projection matrix
1082 {
1083 
1084     /* Add new image. Create new corr points */
1085     /* Track exist points from oldImage to newImage */
1086     /* Create new vector status */
1087     CvMat *status;
1088     int numPoints = oldPoints->cols;
1089     status = cvCreateMat(1,numPoints,CV_64F);
1090     /* Copy status */
1091     cvConvert(pntStatus,status);
1092 
1093     int corrNum = FindCorrForGivenPoints(oldImage,newImage,oldPoints,newPoints,status);
1094 
1095     /* Status has new status of points */
1096 
1097     CvMat projMatr;
1098     double projMatr_dat[12];
1099     projMatr = cvMat(3,4,CV_64F,projMatr_dat);
1100 
1101     /* If number of corr points is 6 or more can compute projection matrix */
1102     if( corrNum >= 6)
1103     {
1104         /* Compute projection matrix for new image using corresponding points */
1105         icvComputeProjectMatrix(objPoints4D,newPoints,&projMatr);
1106 
1107         CvMat *tmpPoints;
1108         /* Create new points and find correspondence */
1109         int num = FindFeaturePoints(newImage, &tmpPoints);
1110         if( num > 0 )
1111         {
1112             CvMat *newPoints;
1113             newPoints = cvCreateMat(2,num,CV_64F);
1114             CvMat *status;
1115             status = cvCreateMat(1,num,CV_64F);
1116             /* Set status for all points */
1117             int i;
1118             for( i = 0; i < num; i++ )
1119             {
1120                 cvmSet(status,0,i,1.0);
1121             }
1122 
1123             int corrNum2 = FindCorrForGivenPoints(oldImage,newImage,tmpPoints,newPoints,status);
1124 
1125             /* !!! Filter points using projection matrices or not ??? */
1126 
1127             /* !!! Need to filter nearest points */
1128 
1129             /* Add new found points to exist points and optimize again */
1130             CvMat *new2DPoints;
1131             CvMat *newStatus;
1132 
1133             /* add new status to old status */
1134 
1135 
1136 
1137 
1138 
1139         }
1140         else
1141         {
1142             /* No new points were found */
1143         }
1144     }
1145     else
1146     {
1147         /* We can't compute projection matrix for new image */
1148         return 0;
1149     }
1150 
1151 }
1152 #endif
1153