modules/legacy/src/corrimages.cpp

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