/*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