git.maemo.org Git - opencv/blob - cvaux/src/cvcorrimages.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 "_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 */
  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 */
 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 /*-------------------------------------------------------------------------------------*/
 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 /*-------------------------------------------------------------------------------------*/
 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 /*-------------------------------------------------------------------------------------*/
 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
 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 /*-------------------------------------------------------------------------------------*/
 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