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