cvcorrimages.cpp revision 6acb9a7ea3d7564944e12cbc73a857b88c1301ee 
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 */
56int 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 */
178int 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/*-------------------------------------------------------------------------------------*/
439int 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/*-------------------------------------------------------------------------------------*/
550int 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
756void icvComputeProjectMatrix(CvMat* objPoints,CvMat* projPoints,CvMat* projMatr);
757
758/*-------------------------------------------------------------------------------------*/
759void 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
834void 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/*-------------------------------------------------------------------------------------*/
952int 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/*-------------------------------------------------------------------------------------*/
1048void icvGrowPointsArray(CvMat **points)
1049{
1050
1051
1052}
1053
1054/*-------------------------------------------------------------------------------------*/
1055void 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 */
1072int 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