--- /dev/null
+/*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
projects / opencv / blobdiff