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