| /*M/////////////////////////////////////////////////////////////////////////////////////// |
| // |
| // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. |
| // |
| // By downloading, copying, installing or using the software you agree to this license. |
| // If you do not agree to this license, do not download, install, |
| // copy or use the software. |
| // |
| // |
| // Intel License Agreement |
| // For Open Source Computer Vision Library |
| // |
| // Copyright (C) 2000, Intel Corporation, all rights reserved. |
| // Third party copyrights are property of their respective owners. |
| // |
| // Redistribution and use in source and binary forms, with or without modification, |
| // are permitted provided that the following conditions are met: |
| // |
| // * Redistribution's of source code must retain the above copyright notice, |
| // this list of conditions and the following disclaimer. |
| // |
| // * Redistribution's in binary form must reproduce the above copyright notice, |
| // this list of conditions and the following disclaimer in the documentation |
| // and/or other materials provided with the distribution. |
| // |
| // * The name of Intel Corporation may not be used to endorse or promote products |
| // derived from this software without specific prior written permission. |
| // |
| // This software is provided by the copyright holders and contributors "as is" and |
| // any express or implied warranties, including, but not limited to, the implied |
| // warranties of merchantability and fitness for a particular purpose are disclaimed. |
| // In no event shall the Intel Corporation or contributors be liable for any direct, |
| // indirect, incidental, special, exemplary, or consequential damages |
| // (including, but not limited to, procurement of substitute goods or services; |
| // loss of use, data, or profits; or business interruption) however caused |
| // and on any theory of liability, whether in contract, strict liability, |
| // or tort (including negligence or otherwise) arising in any way out of |
| // the use of this software, even if advised of the possibility of such damage. |
| // |
| //M*/ |
| |
| #include "_cvaux.h" |
| //#include "cvtypes.h" |
| //#include <float.h> |
| //#include <limits.h> |
| //#include "cv.h" |
| //#include "highgui.h" |
| |
| #include <stdio.h> |
| |
| /* Valery Mosyagin */ |
| |
| /* ===== Function for find corresponding between images ===== */ |
| |
| /* Create feature points on image and return number of them. Array points fills by found points */ |
| int icvCreateFeaturePoints(IplImage *image, CvMat *points, CvMat *status) |
| { |
| int foundFeaturePoints = 0; |
| IplImage *grayImage = 0; |
| IplImage *eigImage = 0; |
| IplImage *tmpImage = 0; |
| CvPoint2D32f *cornerPoints = 0; |
| |
| CV_FUNCNAME( "icvFeatureCreatePoints" ); |
| __BEGIN__; |
| |
| /* Test for errors */ |
| if( image == 0 || points == 0 ) |
| { |
| CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); |
| } |
| |
| /* Test image size */ |
| int w,h; |
| w = image->width; |
| h = image->height; |
| |
| if( w <= 0 || h <= 0) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Size of image must be > 0" ); |
| } |
| |
| /* Test for matrices */ |
| if( !CV_IS_MAT(points) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "Input parameter points must be a matrix" ); |
| } |
| |
| int needNumPoints; |
| needNumPoints = points->cols; |
| if( needNumPoints <= 0 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of need points must be > 0" ); |
| } |
| |
| if( points->rows != 2 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of point coordinates must be == 2" ); |
| } |
| |
| if( status != 0 ) |
| { |
| /* If status matrix exist test it for correct */ |
| if( !CV_IS_MASK_ARR(status) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "Statuses must be a mask arrays" ); |
| } |
| |
| if( status->cols != needNumPoints ) |
| { |
| CV_ERROR( CV_StsUnmatchedSizes, "Size of points and statuses must be the same" ); |
| } |
| |
| if( status->rows !=1 ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "Number of rows of status must be 1" ); |
| } |
| } |
| |
| /* Create temporary images */ |
| CV_CALL( grayImage = cvCreateImage(cvSize(w,h), 8,1) ); |
| CV_CALL( eigImage = cvCreateImage(cvSize(w,h),32,1) ); |
| CV_CALL( tmpImage = cvCreateImage(cvSize(w,h),32,1) ); |
| |
| /* Create points */ |
| CV_CALL( cornerPoints = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f) * needNumPoints) ); |
| |
| int foundNum; |
| double quality; |
| double minDist; |
| |
| cvCvtColor(image,grayImage, CV_BGR2GRAY); |
| |
| foundNum = needNumPoints; |
| quality = 0.01; |
| minDist = 5; |
| cvGoodFeaturesToTrack(grayImage, eigImage, tmpImage, cornerPoints, &foundNum, quality, minDist); |
| |
| /* Copy found points to result */ |
| int i; |
| for( i = 0; i < foundNum; i++ ) |
| { |
| cvmSet(points,0,i,cornerPoints[i].x); |
| cvmSet(points,1,i,cornerPoints[i].y); |
| } |
| |
| /* Set status if need */ |
| if( status ) |
| { |
| for( i = 0; i < foundNum; i++ ) |
| { |
| status->data.ptr[i] = 1; |
| } |
| |
| for( i = foundNum; i < needNumPoints; i++ ) |
| { |
| status->data.ptr[i] = 0; |
| } |
| } |
| |
| foundFeaturePoints = foundNum; |
| |
| __END__; |
| |
| /* Free allocated memory */ |
| cvReleaseImage(&grayImage); |
| cvReleaseImage(&eigImage); |
| cvReleaseImage(&tmpImage); |
| cvFree(&cornerPoints); |
| |
| return foundFeaturePoints; |
| } |
| |
| /*-------------------------------------------------------------------------------------*/ |
| |
| /* For given points1 (with pntStatus) on image1 finds corresponding points2 on image2 and set pntStatus2 for them */ |
| /* Returns number of corresponding points */ |
| int icvFindCorrForGivenPoints( IplImage *image1,/* Image 1 */ |
| IplImage *image2,/* Image 2 */ |
| CvMat *points1, |
| CvMat *pntStatus1, |
| CvMat *points2, |
| CvMat *pntStatus2, |
| int useFilter,/*Use fundamental matrix to filter points */ |
| double threshold)/* Threshold for good points in filter */ |
| { |
| int resNumCorrPoints = 0; |
| CvPoint2D32f* cornerPoints1 = 0; |
| CvPoint2D32f* cornerPoints2 = 0; |
| char* status = 0; |
| float* errors = 0; |
| CvMat* tmpPoints1 = 0; |
| CvMat* tmpPoints2 = 0; |
| CvMat* pStatus = 0; |
| IplImage *grayImage1 = 0; |
| IplImage *grayImage2 = 0; |
| IplImage *pyrImage1 = 0; |
| IplImage *pyrImage2 = 0; |
| |
| CV_FUNCNAME( "icvFindCorrForGivenPoints" ); |
| __BEGIN__; |
| |
| /* Test input data for errors */ |
| |
| /* Test for null pointers */ |
| if( image1 == 0 || image2 == 0 || |
| points1 == 0 || points2 == 0 || |
| pntStatus1 == 0 || pntStatus2 == 0) |
| { |
| CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); |
| } |
| |
| /* Test image size */ |
| int w,h; |
| w = image1->width; |
| h = image1->height; |
| |
| if( w <= 0 || h <= 0) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Size of image1 must be > 0" ); |
| } |
| |
| if( image2->width != w || image2->height != h ) |
| { |
| CV_ERROR( CV_StsUnmatchedSizes, "Size of images must be the same" ); |
| } |
| |
| /* Test for matrices */ |
| if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2) || |
| !CV_IS_MAT(pntStatus1) || !CV_IS_MAT(pntStatus2) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "Input parameters (points and status) must be a matrices" ); |
| } |
| |
| /* Test type of status matrices */ |
| if( !CV_IS_MASK_ARR(pntStatus1) || !CV_IS_MASK_ARR(pntStatus2) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "Statuses must be a mask arrays" ); |
| } |
| |
| /* Test number of points */ |
| int numPoints; |
| numPoints = points1->cols; |
| |
| if( numPoints <= 0 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of points1 must be > 0" ); |
| } |
| |
| if( points2->cols != numPoints || pntStatus1->cols != numPoints || pntStatus2->cols != numPoints ) |
| { |
| CV_ERROR( CV_StsUnmatchedSizes, "Number of points and statuses must be the same" ); |
| } |
| |
| if( points1->rows != 2 || points2->rows != 2 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of points coordinates must be 2" ); |
| } |
| |
| if( pntStatus1->rows != 1 || pntStatus2->rows != 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Status must be a matrix 1xN" ); |
| } |
| /* ----- End test ----- */ |
| |
| |
| /* Compute number of visible points on image1 */ |
| int numVisPoints; |
| numVisPoints = cvCountNonZero(pntStatus1); |
| |
| if( numVisPoints > 0 ) |
| { |
| /* Create temporary images */ |
| /* We must use iplImage againts hughgui images */ |
| |
| /* |
| CvvImage grayImage1; |
| CvvImage grayImage2; |
| CvvImage pyrImage1; |
| CvvImage pyrImage2; |
| */ |
| |
| /* Create Ipl images */ |
| CV_CALL( grayImage1 = cvCreateImage(cvSize(w,h),8,1) ); |
| CV_CALL( grayImage2 = cvCreateImage(cvSize(w,h),8,1) ); |
| CV_CALL( pyrImage1 = cvCreateImage(cvSize(w,h),8,1) ); |
| CV_CALL( pyrImage2 = cvCreateImage(cvSize(w,h),8,1) ); |
| |
| CV_CALL( cornerPoints1 = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f)*numVisPoints) ); |
| CV_CALL( cornerPoints2 = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f)*numVisPoints) ); |
| CV_CALL( status = (char*)cvAlloc( sizeof(char)*numVisPoints) ); |
| CV_CALL( errors = (float*)cvAlloc( 2 * sizeof(float)*numVisPoints) ); |
| |
| int i; |
| for( i = 0; i < numVisPoints; i++ ) |
| { |
| status[i] = 1; |
| } |
| |
| /* !!! Need test creation errors */ |
| /* |
| if( !grayImage1.Create(w,h,8)) EXIT; |
| if( !grayImage2.Create(w,h,8)) EXIT; |
| if( !pyrImage1. Create(w,h,8)) EXIT; |
| if( !pyrImage2. Create(w,h,8)) EXIT; |
| */ |
| |
| cvCvtColor(image1,grayImage1,CV_BGR2GRAY); |
| cvCvtColor(image2,grayImage2,CV_BGR2GRAY); |
| |
| /* |
| grayImage1.CopyOf(image1,0); |
| grayImage2.CopyOf(image2,0); |
| */ |
| |
| /* Copy points good points from input data */ |
| uchar *stat1 = pntStatus1->data.ptr; |
| uchar *stat2 = pntStatus2->data.ptr; |
| |
| int curr = 0; |
| for( i = 0; i < numPoints; i++ ) |
| { |
| if( stat1[i] ) |
| { |
| cornerPoints1[curr].x = (float)cvmGet(points1,0,i); |
| cornerPoints1[curr].y = (float)cvmGet(points1,1,i); |
| curr++; |
| } |
| } |
| |
| /* Define number of levels of pyramid */ |
| cvCalcOpticalFlowPyrLK( grayImage1, grayImage2, |
| pyrImage1, pyrImage2, |
| cornerPoints1, cornerPoints2, |
| numVisPoints, cvSize(10,10), 3, |
| status, errors, |
| cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), |
| 0/*CV_LKFLOW_PYR_A_READY*/ ); |
| |
| |
| memset(stat2,0,sizeof(uchar)*numPoints); |
| |
| int currVis = 0; |
| int totalCorns = 0; |
| |
| /* Copy new points and set status */ |
| /* stat1 may not be the same as stat2 */ |
| for( i = 0; i < numPoints; i++ ) |
| { |
| if( stat1[i] ) |
| { |
| if( status[currVis] && errors[currVis] < 1000 ) |
| { |
| stat2[i] = 1; |
| cvmSet(points2,0,i,cornerPoints2[currVis].x); |
| cvmSet(points2,1,i,cornerPoints2[currVis].y); |
| totalCorns++; |
| } |
| currVis++; |
| } |
| } |
| |
| resNumCorrPoints = totalCorns; |
| |
| /* Filter points using RANSAC */ |
| if( useFilter ) |
| { |
| resNumCorrPoints = 0; |
| /* Use RANSAC filter for found points */ |
| if( totalCorns > 7 ) |
| { |
| /* Create array with good points only */ |
| CV_CALL( tmpPoints1 = cvCreateMat(2,totalCorns,CV_64F) ); |
| CV_CALL( tmpPoints2 = cvCreateMat(2,totalCorns,CV_64F) ); |
| |
| /* Copy just good points */ |
| int currPoint = 0; |
| for( i = 0; i < numPoints; i++ ) |
| { |
| if( stat2[i] ) |
| { |
| cvmSet(tmpPoints1,0,currPoint,cvmGet(points1,0,i)); |
| cvmSet(tmpPoints1,1,currPoint,cvmGet(points1,1,i)); |
| |
| cvmSet(tmpPoints2,0,currPoint,cvmGet(points2,0,i)); |
| cvmSet(tmpPoints2,1,currPoint,cvmGet(points2,1,i)); |
| |
| currPoint++; |
| } |
| } |
| |
| /* Compute fundamental matrix */ |
| CvMat fundMatr; |
| double fundMatr_dat[9]; |
| fundMatr = cvMat(3,3,CV_64F,fundMatr_dat); |
| |
| CV_CALL( pStatus = cvCreateMat(1,totalCorns,CV_32F) ); |
| |
| int num = cvFindFundamentalMat(tmpPoints1,tmpPoints2,&fundMatr,CV_FM_RANSAC,threshold,0.99,pStatus); |
| if( num > 0 ) |
| { |
| int curr = 0; |
| /* Set final status for points2 */ |
| for( i = 0; i < numPoints; i++ ) |
| { |
| if( stat2[i] ) |
| { |
| if( cvmGet(pStatus,0,curr) == 0 ) |
| { |
| stat2[i] = 0; |
| } |
| curr++; |
| } |
| } |
| resNumCorrPoints = curr; |
| } |
| } |
| } |
| } |
| |
| __END__; |
| |
| /* Free allocated memory */ |
| cvFree(&cornerPoints1); |
| cvFree(&cornerPoints2); |
| cvFree(&status); |
| cvFree(&errors); |
| cvFree(&tmpPoints1); |
| cvFree(&tmpPoints2); |
| cvReleaseMat( &pStatus ); |
| cvReleaseImage( &grayImage1 ); |
| cvReleaseImage( &grayImage2 ); |
| cvReleaseImage( &pyrImage1 ); |
| cvReleaseImage( &pyrImage2 ); |
| |
| return resNumCorrPoints; |
| } |
| /*-------------------------------------------------------------------------------------*/ |
| int icvGrowPointsAndStatus(CvMat **oldPoints,CvMat **oldStatus,CvMat *addPoints,CvMat *addStatus,int addCreateNum) |
| { |
| /* Add to existing points and status arrays new points or just grow */ |
| CvMat *newOldPoint = 0; |
| CvMat *newOldStatus = 0; |
| int newTotalNumber = 0; |
| |
| CV_FUNCNAME( "icvGrowPointsAndStatus" ); |
| __BEGIN__; |
| |
| /* Test for errors */ |
| if( oldPoints == 0 || oldStatus == 0 ) |
| { |
| CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); |
| } |
| |
| if( *oldPoints == 0 || *oldStatus == 0 ) |
| { |
| CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); |
| } |
| |
| if( !CV_IS_MAT(*oldPoints)) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "oldPoints must be a pointer to a matrix" ); |
| } |
| |
| if( !CV_IS_MASK_ARR(*oldStatus)) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "oldStatus must be a pointer to a mask array" ); |
| } |
| |
| int oldNum; |
| oldNum = (*oldPoints)->cols; |
| if( oldNum < 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of old points must be > 0" ); |
| } |
| |
| /* Define if need number of add points */ |
| int addNum; |
| addNum = 0; |
| if( addPoints != 0 && addStatus != 0 ) |
| {/* We have aditional points */ |
| if( CV_IS_MAT(addPoints) && CV_IS_MASK_ARR(addStatus) ) |
| { |
| addNum = addPoints->cols; |
| if( addStatus->cols != addNum ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of add points and statuses must be the same" ); |
| } |
| } |
| } |
| |
| /* */ |
| |
| int numCoord; |
| numCoord = (*oldPoints)->rows; |
| newTotalNumber = oldNum + addNum + addCreateNum; |
| |
| if( newTotalNumber ) |
| { |
| /* Free allocated memory */ |
| newOldPoint = cvCreateMat(numCoord,newTotalNumber,CV_64F); |
| newOldStatus = cvCreateMat(1,newTotalNumber,CV_8S); |
| |
| /* Copy old values to */ |
| int i; |
| |
| /* Clear all values */ |
| cvZero(newOldPoint); |
| cvZero(newOldStatus); |
| |
| for( i = 0; i < oldNum; i++ ) |
| { |
| int currCoord; |
| for( currCoord = 0; currCoord < numCoord; currCoord++ ) |
| { |
| cvmSet(newOldPoint,currCoord,i,cvmGet(*oldPoints,currCoord,i)); |
| } |
| newOldStatus->data.ptr[i] = (*oldStatus)->data.ptr[i]; |
| } |
| |
| /* Copy additional points and statuses */ |
| if( addNum ) |
| { |
| for( i = 0; i < addNum; i++ ) |
| { |
| int currCoord; |
| for( currCoord = 0; currCoord < numCoord; currCoord++ ) |
| { |
| cvmSet(newOldPoint,currCoord,i+oldNum,cvmGet(addPoints,currCoord,i)); |
| } |
| newOldStatus->data.ptr[i+oldNum] = addStatus->data.ptr[i]; |
| //cvmSet(newOldStatus,0,i,cvmGet(addStatus,0,i)); |
| } |
| } |
| |
| /* Delete previous data */ |
| cvReleaseMat(oldPoints); |
| cvReleaseMat(oldStatus); |
| |
| /* copy pointers */ |
| *oldPoints = newOldPoint; |
| *oldStatus = newOldStatus; |
| |
| } |
| __END__; |
| |
| return newTotalNumber; |
| } |
| /*-------------------------------------------------------------------------------------*/ |
| int icvRemoveDoublePoins( CvMat *oldPoints,/* Points on prev image */ |
| CvMat *newPoints,/* New points */ |
| CvMat *oldStatus,/* Status for old points */ |
| CvMat *newStatus, |
| CvMat *origStatus, |
| float threshold)/* Status for new points */ |
| { |
| |
| CvMemStorage* storage = 0; |
| CvSubdiv2D* subdiv = 0; |
| CvSeq* seq = 0; |
| |
| int originalPoints = 0; |
| |
| CV_FUNCNAME( "icvRemoveDoublePoins" ); |
| __BEGIN__; |
| |
| /* Test input data */ |
| if( oldPoints == 0 || newPoints == 0 || |
| oldStatus == 0 || newStatus == 0 || origStatus == 0 ) |
| { |
| CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); |
| } |
| |
| if( !CV_IS_MAT(oldPoints) || !CV_IS_MAT(newPoints) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "Input parameters points must be a matrices" ); |
| } |
| |
| if( !CV_IS_MASK_ARR(oldStatus) || !CV_IS_MASK_ARR(newStatus) || !CV_IS_MASK_ARR(origStatus) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "Input parameters statuses must be a mask array" ); |
| } |
| |
| int oldNumPoints; |
| oldNumPoints = oldPoints->cols; |
| if( oldNumPoints < 0 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of oldPoints must be >= 0" ); |
| } |
| |
| if( oldStatus->cols != oldNumPoints ) |
| { |
| CV_ERROR( CV_StsUnmatchedSizes, "Number of old Points and old Statuses must be the same" ); |
| } |
| |
| int newNumPoints; |
| newNumPoints = newPoints->cols; |
| if( newNumPoints < 0 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of newPoints must be >= 0" ); |
| } |
| |
| if( newStatus->cols != newNumPoints ) |
| { |
| CV_ERROR( CV_StsUnmatchedSizes, "Number of new Points and new Statuses must be the same" ); |
| } |
| |
| if( origStatus->cols != newNumPoints ) |
| { |
| CV_ERROR( CV_StsUnmatchedSizes, "Number of new Points and new original Status must be the same" ); |
| } |
| |
| if( oldPoints->rows != 2) |
| { |
| CV_ERROR( CV_StsOutOfRange, "OldPoints must have 2 coordinates >= 0" ); |
| } |
| |
| if( newPoints->rows != 2) |
| { |
| CV_ERROR( CV_StsOutOfRange, "NewPoints must have 2 coordinates >= 0" ); |
| } |
| |
| if( oldStatus->rows != 1 || newStatus->rows != 1 || origStatus->rows != 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Statuses must have 1 row" ); |
| } |
| |
| /* we have points on image and wants add new points */ |
| /* use subdivision for find nearest points */ |
| |
| /* Define maximum and minimum X and Y */ |
| float minX,minY; |
| float maxX,maxY; |
| |
| minX = minY = FLT_MAX; |
| maxX = maxY = FLT_MIN; |
| |
| int i; |
| |
| for( i = 0; i < oldNumPoints; i++ ) |
| { |
| if( oldStatus->data.ptr[i] ) |
| { |
| float x = (float)cvmGet(oldPoints,0,i); |
| float y = (float)cvmGet(oldPoints,1,i); |
| |
| if( x < minX ) |
| minX = x; |
| |
| if( x > maxX ) |
| maxX = x; |
| |
| if( y < minY ) |
| minY = y; |
| |
| if( y > maxY ) |
| maxY = y; |
| } |
| } |
| |
| for( i = 0; i < newNumPoints; i++ ) |
| { |
| if( newStatus->data.ptr[i] ) |
| { |
| float x = (float)cvmGet(newPoints,0,i); |
| float y = (float)cvmGet(newPoints,1,i); |
| |
| if( x < minX ) |
| minX = x; |
| |
| if( x > maxX ) |
| maxX = x; |
| |
| if( y < minY ) |
| minY = y; |
| |
| if( y > maxY ) |
| maxY = y; |
| } |
| } |
| |
| |
| /* Creare subdivision for old image */ |
| storage = cvCreateMemStorage(0); |
| // subdiv = cvCreateSubdivDelaunay2D( cvRect( 0, 0, size.width, size.height ), storage ); |
| subdiv = cvCreateSubdivDelaunay2D( cvRect( cvRound(minX)-5, cvRound(minY)-5, cvRound(maxX-minX)+10, cvRound(maxY-minY)+10 ), storage ); |
| seq = cvCreateSeq( 0, sizeof(*seq), sizeof(CvPoint2D32f), storage ); |
| |
| /* Insert each point from first image */ |
| for( i = 0; i < oldNumPoints; i++ ) |
| { |
| /* Add just exist points */ |
| if( oldStatus->data.ptr[i] ) |
| { |
| CvPoint2D32f pt; |
| pt.x = (float)cvmGet(oldPoints,0,i); |
| pt.y = (float)cvmGet(oldPoints,1,i); |
| |
| CvSubdiv2DPoint* point; |
| point = cvSubdivDelaunay2DInsert( subdiv, pt ); |
| } |
| } |
| |
| |
| /* Find nearest points */ |
| /* for each new point */ |
| int flag; |
| for( i = 0; i < newNumPoints; i++ ) |
| { |
| flag = 0; |
| /* Test just exist points */ |
| if( newStatus->data.ptr[i] ) |
| { |
| flag = 1; |
| /* Let this is a good point */ |
| //originalPoints++; |
| |
| CvPoint2D32f pt; |
| |
| pt.x = (float)cvmGet(newPoints,0,i); |
| pt.y = (float)cvmGet(newPoints,1,i); |
| |
| CvSubdiv2DPoint* point = cvFindNearestPoint2D( subdiv, pt ); |
| |
| if( point ) |
| { |
| /* Test distance of found nearest point */ |
| double minDistance = icvSqDist2D32f( pt, point->pt ); |
| |
| if( minDistance < threshold*threshold ) |
| { |
| /* Point is double. Turn it off */ |
| /* Set status */ |
| //newStatus->data.ptr[i] = 0; |
| |
| /* No this is a double point */ |
| //originalPoints--; |
| flag = 0; |
| } |
| } |
| } |
| originalPoints += flag; |
| origStatus->data .ptr[i] = (uchar)flag; |
| } |
| |
| __END__; |
| |
| cvReleaseMemStorage( &storage ); |
| |
| |
| return originalPoints; |
| |
| |
| } |
| |
| void icvComputeProjectMatrix(CvMat* objPoints,CvMat* projPoints,CvMat* projMatr); |
| |
| /*-------------------------------------------------------------------------------------*/ |
| void icvComputeProjectMatrixStatus(CvMat *objPoints4D,CvMat *points2,CvMat *status, CvMat *projMatr) |
| { |
| /* Compute number of good points */ |
| int num = cvCountNonZero(status); |
| |
| /* Create arrays */ |
| CvMat *objPoints = 0; |
| objPoints = cvCreateMat(4,num,CV_64F); |
| |
| CvMat *points2D = 0; |
| points2D = cvCreateMat(2,num,CV_64F); |
| |
| int currVis = 0; |
| int i; |
| #if 1 |
| FILE *file; |
| file = fopen("d:\\test\\projStatus.txt","w"); |
| #endif |
| int totalNum = objPoints4D->cols; |
| for( i = 0; i < totalNum; i++ ) |
| { |
| fprintf(file,"%d (%d) ",i,status->data.ptr[i]); |
| if( status->data.ptr[i] ) |
| { |
| |
| #if 1 |
| double X,Y,Z,W; |
| double x,y; |
| X = cvmGet(objPoints4D,0,i); |
| Y = cvmGet(objPoints4D,1,i); |
| Z = cvmGet(objPoints4D,2,i); |
| W = cvmGet(objPoints4D,3,i); |
| |
| x = cvmGet(points2,0,i); |
| y = cvmGet(points2,1,i); |
| fprintf(file,"%d (%lf %lf %lf %lf) - (%lf %lf)",i,X,Y,Z,W,x,y ); |
| #endif |
| cvmSet(objPoints,0,currVis,cvmGet(objPoints4D,0,i)); |
| cvmSet(objPoints,1,currVis,cvmGet(objPoints4D,1,i)); |
| cvmSet(objPoints,2,currVis,cvmGet(objPoints4D,2,i)); |
| cvmSet(objPoints,3,currVis,cvmGet(objPoints4D,3,i)); |
| |
| cvmSet(points2D,0,currVis,cvmGet(points2,0,i)); |
| cvmSet(points2D,1,currVis,cvmGet(points2,1,i)); |
| |
| currVis++; |
| } |
| |
| fprintf(file,"\n"); |
| } |
| |
| #if 1 |
| fclose(file); |
| #endif |
| |
| icvComputeProjectMatrix(objPoints,points2D,projMatr); |
| |
| /* Free allocated memory */ |
| cvReleaseMat(&objPoints); |
| cvReleaseMat(&points2D); |
| } |
| |
| |
| |
| /*-------------------------------------------------------------------------------------*/ |
| /* For given N images |
| we have corresponding points on N images |
| computed projection matrices |
| reconstructed 4D points |
| |
| we must to compute |
| |
| |
| */ |
| |
| void icvAddNewImageToPrevious____( |
| IplImage *newImage,//Image to add |
| IplImage *oldImage,//Previous image |
| CvMat *oldPoints,// previous 2D points on prev image (some points may be not visible) |
| CvMat *oldPntStatus,//Status for each point on prev image |
| CvMat *objPoints4D,//prev 4D points |
| CvMat *newPoints, //Points on new image corr for prev |
| CvMat *newPntStatus,// New point status for new image |
| CvMat *newFPoints2D1,//new feature points on prev image |
| CvMat *newFPoints2D2,//new feature points on new image |
| CvMat *newFPointsStatus, |
| CvMat *newProjMatr, |
| int useFilter, |
| double threshold)//New projection matrix |
| { |
| CvMat *points2 = 0; |
| CvMat *status = 0; |
| CvMat *newFPointsStatusTmp = 0; |
| |
| //CV_FUNCNAME( "icvAddNewImageToPrevious____" ); |
| __BEGIN__; |
| |
| /* First found correspondence points for images */ |
| |
| /* Test input params */ |
| |
| int numPoints; |
| numPoints = oldPoints->cols; |
| |
| /* Allocate memory */ |
| |
| points2 = cvCreateMat(2,numPoints,CV_64F); |
| status = cvCreateMat(1,numPoints,CV_8S); |
| newFPointsStatusTmp = cvCreateMat(1, newFPoints2D1->cols,CV_8S); |
| |
| int corrNum; |
| corrNum = icvFindCorrForGivenPoints( oldImage,/* Image 1 */ |
| newImage,/* Image 2 */ |
| oldPoints, |
| oldPntStatus, |
| points2, |
| status, |
| useFilter,/*Use fundamental matrix to filter points */ |
| threshold);/* Threshold for good points in filter */ |
| |
| cvCopy(status,newPntStatus); |
| cvCopy(points2,newPoints); |
| |
| CvMat projMatr; |
| double projMatr_dat[12]; |
| projMatr = cvMat(3,4,CV_64F,projMatr_dat); |
| |
| if( corrNum >= 6 ) |
| {/* We can compute projection matrix */ |
| // icvComputeProjectMatrix(objPoints4D,points2,&projMatr); |
| icvComputeProjectMatrixStatus(objPoints4D,points2,status,&projMatr); |
| cvCopy(&projMatr,newProjMatr); |
| |
| /* Create new points and find correspondence */ |
| icvCreateFeaturePoints(newImage, newFPoints2D2,newFPointsStatus); |
| |
| /* Good if we test new points before find corr points */ |
| |
| /* Find correspondence for new found points */ |
| icvFindCorrForGivenPoints( newImage,/* Image 1 */ |
| oldImage,/* Image 2 */ |
| newFPoints2D2, |
| newFPointsStatus,//prev status |
| newFPoints2D1, |
| newFPointsStatusTmp,//new status |
| useFilter,/*Use fundamental matrix to filter points */ |
| threshold);/* Threshold for good points in filter */ |
| |
| /* We generated new points on image test for exist points */ |
| |
| /* Remove all new double points */ |
| |
| int origNum; |
| /* Find point of old image */ |
| origNum = icvRemoveDoublePoins( oldPoints,/* Points on prev image */ |
| newFPoints2D1,/* New points */ |
| oldPntStatus,/* Status for old points */ |
| newFPointsStatusTmp, |
| newFPointsStatusTmp,//orig status |
| 20);/* Status for new points */ |
| |
| /* Find double points on new image */ |
| origNum = icvRemoveDoublePoins( newPoints,/* Points on prev image */ |
| newFPoints2D2,/* New points */ |
| newPntStatus,/* Status for old points */ |
| newFPointsStatusTmp, |
| newFPointsStatusTmp,//orig status |
| 20);/* Status for new points */ |
| |
| |
| |
| /* Add all new good points to result */ |
| |
| |
| /* Copy new status to old */ |
| cvCopy(newFPointsStatusTmp,newFPointsStatus); |
| |
| |
| } |
| |
| |
| |
| __END__; |
| |
| /* Free allocated memory */ |
| |
| return; |
| } |
| /*-------------------------------------------------------------------------------------*/ |
| //int icvDelete// |
| //CreateGood |
| |
| /*-------------------------------------------------------------------------------------*/ |
| int icvDeleteSparsInPoints( int numImages, |
| CvMat **points, |
| CvMat **status, |
| CvMat *wasStatus)/* status of previous configuration */ |
| { |
| /* Delete points which no exist on any of images */ |
| /* numImages - number of images */ |
| /* points - arrays of points for each image. Changing */ |
| /* status - arrays of status for each image. Changing */ |
| /* Function returns number of common points */ |
| |
| int comNumber = 0; |
| CV_FUNCNAME( "icvDeleteSparsInPoints" ); |
| __BEGIN__; |
| |
| /* Test for errors */ |
| if( numImages < 1 ) |
| { |
| CV_ERROR( CV_StsOutOfRange, "Number of images must be more than 0" ); |
| } |
| |
| if( points == 0 || status == 0 ) |
| { |
| CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); |
| } |
| int numPoints; |
| |
| numPoints = points[0]->cols; |
| ////////// TESTS ////////// |
| |
| int numCoord; |
| numCoord = points[0]->rows;// !!! may be number of coordinates is not correct !!! |
| |
| int i; |
| int currExistPoint; |
| currExistPoint = 0; |
| |
| if( wasStatus ) |
| { |
| cvZero(wasStatus); |
| } |
| |
| int currImage; |
| for( i = 0; i < numPoints; i++ ) |
| { |
| int flag = 0; |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| flag |= status[currImage]->data.ptr[i]; |
| } |
| |
| if( flag ) |
| { |
| /* Current point exists */ |
| /* Copy points and status */ |
| if( currExistPoint != i )/* Copy just if different */ |
| { |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| /* Copy points */ |
| for( int currCoord = 0; currCoord < numCoord; currCoord++ ) |
| { |
| cvmSet(points[currImage],currCoord,currExistPoint, cvmGet(points[currImage],currCoord,i) ); |
| } |
| |
| /* Copy status */ |
| status[currImage]->data.ptr[currExistPoint] = status[currImage]->data.ptr[i]; |
| } |
| } |
| if( wasStatus ) |
| { |
| wasStatus->data.ptr[i] = 1; |
| } |
| |
| currExistPoint++; |
| |
| } |
| } |
| |
| /* Rest of final status of points must be set to 0 */ |
| for( i = currExistPoint; i < numPoints; i++ ) |
| { |
| for( currImage = 0; currImage < numImages; currImage++ ) |
| { |
| status[currImage]->data.ptr[i] = 0; |
| } |
| } |
| |
| comNumber = currExistPoint; |
| |
| __END__; |
| return comNumber; |
| } |
| |
| #if 0 |
| /*-------------------------------------------------------------------------------------*/ |
| void icvGrowPointsArray(CvMat **points) |
| { |
| |
| |
| } |
| |
| /*-------------------------------------------------------------------------------------*/ |
| void icvAddNewArrayPoints() |
| { |
| |
| } |
| |
| /*-------------------------------------------------------------------------------------*/ |
| #endif |
| |
| ////////////////////////////////////////////////////////////////////////////////////////// |
| ////////////////////////////////////////////////////////////////////////////////////////// |
| ////////////////////////////////////////////////////////////////////////////////////////// |
| ////////////////////////////////////////////////////////////////////////////////////////// |
| |
| /* Add image to existing images and corr points */ |
| #if 0 |
| /* Returns: 1 if new image was added good */ |
| /* 0 image was not added. Not enought corr points */ |
| int AddImageToStruct( IplImage *newImage,//Image to add |
| IplImage *oldImage,//Previous image |
| CvMat *oldPoints,// previous 2D points on prev image (some points may be not visible) |
| CvMat *oldPntStatus,//Status for each point on prev image |
| CvMat *objPoints4D,//prev 4D points |
| CvMat *newPntStatus,// New point status for new image |
| CvMat *newPoints,//New corresponding points on new image |
| CvMat *newPoints2D1,//new points on prev image |
| CvMat *newPoints2D2,//new points on new image |
| CvMat *newProjMatr);//New projection matrix |
| { |
| |
| /* Add new image. Create new corr points */ |
| /* Track exist points from oldImage to newImage */ |
| /* Create new vector status */ |
| CvMat *status; |
| int numPoints = oldPoints->cols; |
| status = cvCreateMat(1,numPoints,CV_64F); |
| /* Copy status */ |
| cvConvert(pntStatus,status); |
| |
| int corrNum = FindCorrForGivenPoints(oldImage,newImage,oldPoints,newPoints,status); |
| |
| /* Status has new status of points */ |
| |
| CvMat projMatr; |
| double projMatr_dat[12]; |
| projMatr = cvMat(3,4,CV_64F,projMatr_dat); |
| |
| /* If number of corr points is 6 or more can compute projection matrix */ |
| if( corrNum >= 6) |
| { |
| /* Compute projection matrix for new image using corresponding points */ |
| icvComputeProjectMatrix(objPoints4D,newPoints,&projMatr); |
| |
| CvMat *tmpPoints; |
| /* Create new points and find correspondence */ |
| int num = FindFeaturePoints(newImage, &tmpPoints); |
| if( num > 0 ) |
| { |
| CvMat *newPoints; |
| newPoints = cvCreateMat(2,num,CV_64F); |
| CvMat *status; |
| status = cvCreateMat(1,num,CV_64F); |
| /* Set status for all points */ |
| int i; |
| for( i = 0; i < num; i++ ) |
| { |
| cvmSet(status,0,i,1.0); |
| } |
| |
| int corrNum2 = FindCorrForGivenPoints(oldImage,newImage,tmpPoints,newPoints,status); |
| |
| /* !!! Filter points using projection matrices or not ??? */ |
| |
| /* !!! Need to filter nearest points */ |
| |
| /* Add new found points to exist points and optimize again */ |
| CvMat *new2DPoints; |
| CvMat *newStatus; |
| |
| /* add new status to old status */ |
| |
| |
| |
| |
| |
| } |
| else |
| { |
| /* No new points were found */ |
| } |
| } |
| else |
| { |
| /* We can't compute projection matrix for new image */ |
| return 0; |
| } |
| |
| } |
| #endif |
