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