16acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*M/////////////////////////////////////////////////////////////////////////////////////// 26acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 36acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 46acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 56acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// By downloading, copying, installing or using the software you agree to this license. 66acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// If you do not agree to this license, do not download, install, 76acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// copy or use the software. 86acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 96acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Intel License Agreement 116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// For Open Source Computer Vision Library 126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Copyright (C) 2000, Intel Corporation, all rights reserved. 146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Third party copyrights are property of their respective owners. 156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Redistribution and use in source and binary forms, with or without modification, 176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// are permitted provided that the following conditions are met: 186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's of source code must retain the above copyright notice, 206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer. 216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's in binary form must reproduce the above copyright notice, 236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer in the documentation 246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and/or other materials provided with the distribution. 256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * The name of Intel Corporation may not be used to endorse or promote products 276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// derived from this software without specific prior written permission. 286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// This software is provided by the copyright holders and contributors "as is" and 306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// any express or implied warranties, including, but not limited to, the implied 316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// warranties of merchantability and fitness for a particular purpose are disclaimed. 326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// In no event shall the Intel Corporation or contributors be liable for any direct, 336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// indirect, incidental, special, exemplary, or consequential damages 346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// (including, but not limited to, procurement of substitute goods or services; 356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// loss of use, data, or profits; or business interruption) however caused 366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and on any theory of liability, whether in contract, strict liability, 376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// or tort (including negligence or otherwise) arising in any way out of 386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// the use of this software, even if advised of the possibility of such damage. 396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//M*/ 416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "_cvaux.h" 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//#include "cvtypes.h" 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//#include <float.h> 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//#include <limits.h> 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//#include "cv.h" 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//#include "highgui.h" 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <stdio.h> 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Valery Mosyagin */ 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* ===== Function for find corresponding between images ===== */ 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Create feature points on image and return number of them. Array points fills by found points */ 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvCreateFeaturePoints(IplImage *image, CvMat *points, CvMat *status) 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int foundFeaturePoints = 0; 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IplImage *grayImage = 0; 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IplImage *eigImage = 0; 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IplImage *tmpImage = 0; 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f *cornerPoints = 0; 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvFeatureCreatePoints" ); 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for errors */ 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( image == 0 || points == 0 ) 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test image size */ 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int w,h; 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w = image->width; 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn h = image->height; 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( w <= 0 || h <= 0) 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Size of image must be > 0" ); 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for matrices */ 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(points) ) 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Input parameter points must be a matrix" ); 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int needNumPoints; 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn needNumPoints = points->cols; 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( needNumPoints <= 0 ) 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of need points must be > 0" ); 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points->rows != 2 ) 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of point coordinates must be == 2" ); 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( status != 0 ) 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* If status matrix exist test it for correct */ 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MASK_ARR(status) ) 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Statuses must be a mask arrays" ); 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( status->cols != needNumPoints ) 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of points and statuses must be the same" ); 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( status->rows !=1 ) 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Number of rows of status must be 1" ); 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create temporary images */ 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( grayImage = cvCreateImage(cvSize(w,h), 8,1) ); 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( eigImage = cvCreateImage(cvSize(w,h),32,1) ); 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( tmpImage = cvCreateImage(cvSize(w,h),32,1) ); 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create points */ 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cornerPoints = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f) * needNumPoints) ); 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int foundNum; 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double quality; 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double minDist; 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCvtColor(image,grayImage, CV_BGR2GRAY); 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn foundNum = needNumPoints; 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn quality = 0.01; 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minDist = 5; 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGoodFeaturesToTrack(grayImage, eigImage, tmpImage, cornerPoints, &foundNum, quality, minDist); 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy found points to result */ 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < foundNum; i++ ) 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points,0,i,cornerPoints[i].x); 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points,1,i,cornerPoints[i].y); 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Set status if need */ 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( status ) 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < foundNum; i++ ) 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status->data.ptr[i] = 1; 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = foundNum; i < needNumPoints; i++ ) 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status->data.ptr[i] = 0; 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn foundFeaturePoints = foundNum; 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Free allocated memory */ 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseImage(&grayImage); 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseImage(&eigImage); 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseImage(&tmpImage); 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&cornerPoints); 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return foundFeaturePoints; 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*-------------------------------------------------------------------------------------*/ 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* For given points1 (with pntStatus) on image1 finds corresponding points2 on image2 and set pntStatus2 for them */ 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Returns number of corresponding points */ 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvFindCorrForGivenPoints( IplImage *image1,/* Image 1 */ 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IplImage *image2,/* Image 2 */ 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *points1, 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *pntStatus1, 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *points2, 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *pntStatus2, 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int useFilter,/*Use fundamental matrix to filter points */ 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double threshold)/* Threshold for good points in filter */ 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int resNumCorrPoints = 0; 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f* cornerPoints1 = 0; 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f* cornerPoints2 = 0; 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn char* status = 0; 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* errors = 0; 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* tmpPoints1 = 0; 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* tmpPoints2 = 0; 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* pStatus = 0; 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IplImage *grayImage1 = 0; 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IplImage *grayImage2 = 0; 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IplImage *pyrImage1 = 0; 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IplImage *pyrImage2 = 0; 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvFindCorrForGivenPoints" ); 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test input data for errors */ 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for null pointers */ 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( image1 == 0 || image2 == 0 || 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points1 == 0 || points2 == 0 || 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pntStatus1 == 0 || pntStatus2 == 0) 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test image size */ 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int w,h; 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn w = image1->width; 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn h = image1->height; 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( w <= 0 || h <= 0) 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Size of image1 must be > 0" ); 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( image2->width != w || image2->height != h ) 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of images must be the same" ); 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for matrices */ 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2) || 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_IS_MAT(pntStatus1) || !CV_IS_MAT(pntStatus2) ) 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Input parameters (points and status) must be a matrices" ); 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test type of status matrices */ 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MASK_ARR(pntStatus1) || !CV_IS_MASK_ARR(pntStatus2) ) 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Statuses must be a mask arrays" ); 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test number of points */ 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = points1->cols; 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numPoints <= 0 ) 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of points1 must be > 0" ); 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points2->cols != numPoints || pntStatus1->cols != numPoints || pntStatus2->cols != numPoints ) 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of points and statuses must be the same" ); 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points1->rows != 2 || points2->rows != 2 ) 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of points coordinates must be 2" ); 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( pntStatus1->rows != 1 || pntStatus2->rows != 1 ) 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Status must be a matrix 1xN" ); 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ----- End test ----- */ 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute number of visible points on image1 */ 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numVisPoints; 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numVisPoints = cvCountNonZero(pntStatus1); 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numVisPoints > 0 ) 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create temporary images */ 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We must use iplImage againts hughgui images */ 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvvImage grayImage1; 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvvImage grayImage2; 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvvImage pyrImage1; 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvvImage pyrImage2; 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn*/ 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create Ipl images */ 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( grayImage1 = cvCreateImage(cvSize(w,h),8,1) ); 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( grayImage2 = cvCreateImage(cvSize(w,h),8,1) ); 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( pyrImage1 = cvCreateImage(cvSize(w,h),8,1) ); 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( pyrImage2 = cvCreateImage(cvSize(w,h),8,1) ); 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cornerPoints1 = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f)*numVisPoints) ); 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cornerPoints2 = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f)*numVisPoints) ); 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( status = (char*)cvAlloc( sizeof(char)*numVisPoints) ); 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( errors = (float*)cvAlloc( 2 * sizeof(float)*numVisPoints) ); 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numVisPoints; i++ ) 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status[i] = 1; 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* !!! Need test creation errors */ 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !grayImage1.Create(w,h,8)) EXIT; 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !grayImage2.Create(w,h,8)) EXIT; 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !pyrImage1. Create(w,h,8)) EXIT; 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !pyrImage2. Create(w,h,8)) EXIT; 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn */ 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCvtColor(image1,grayImage1,CV_BGR2GRAY); 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCvtColor(image2,grayImage2,CV_BGR2GRAY); 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn grayImage1.CopyOf(image1,0); 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn grayImage2.CopyOf(image2,0); 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn */ 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy points good points from input data */ 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar *stat1 = pntStatus1->data.ptr; 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar *stat2 = pntStatus2->data.ptr; 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int curr = 0; 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( stat1[i] ) 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cornerPoints1[curr].x = (float)cvmGet(points1,0,i); 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cornerPoints1[curr].y = (float)cvmGet(points1,1,i); 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn curr++; 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Define number of levels of pyramid */ 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCalcOpticalFlowPyrLK( grayImage1, grayImage2, 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pyrImage1, pyrImage2, 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cornerPoints1, cornerPoints2, 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numVisPoints, cvSize(10,10), 3, 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status, errors, 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0/*CV_LKFLOW_PYR_A_READY*/ ); 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset(stat2,0,sizeof(uchar)*numPoints); 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currVis = 0; 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int totalCorns = 0; 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy new points and set status */ 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* stat1 may not be the same as stat2 */ 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( stat1[i] ) 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( status[currVis] && errors[currVis] < 1000 ) 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stat2[i] = 1; 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points2,0,i,cornerPoints2[currVis].x); 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points2,1,i,cornerPoints2[currVis].y); 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn totalCorns++; 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currVis++; 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resNumCorrPoints = totalCorns; 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Filter points using RANSAC */ 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( useFilter ) 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resNumCorrPoints = 0; 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Use RANSAC filter for found points */ 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( totalCorns > 7 ) 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create array with good points only */ 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( tmpPoints1 = cvCreateMat(2,totalCorns,CV_64F) ); 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( tmpPoints2 = cvCreateMat(2,totalCorns,CV_64F) ); 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy just good points */ 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currPoint = 0; 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( stat2[i] ) 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(tmpPoints1,0,currPoint,cvmGet(points1,0,i)); 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(tmpPoints1,1,currPoint,cvmGet(points1,1,i)); 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(tmpPoints2,0,currPoint,cvmGet(points2,0,i)); 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(tmpPoints2,1,currPoint,cvmGet(points2,1,i)); 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currPoint++; 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute fundamental matrix */ 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat fundMatr; 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double fundMatr_dat[9]; 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fundMatr = cvMat(3,3,CV_64F,fundMatr_dat); 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( pStatus = cvCreateMat(1,totalCorns,CV_32F) ); 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int num = cvFindFundamentalMat(tmpPoints1,tmpPoints2,&fundMatr,CV_FM_RANSAC,threshold,0.99,pStatus); 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( num > 0 ) 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int curr = 0; 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Set final status for points2 */ 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( stat2[i] ) 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvmGet(pStatus,0,curr) == 0 ) 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stat2[i] = 0; 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn curr++; 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn resNumCorrPoints = curr; 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Free allocated memory */ 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&cornerPoints1); 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&cornerPoints2); 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&status); 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&errors); 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&tmpPoints1); 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree(&tmpPoints2); 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &pStatus ); 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseImage( &grayImage1 ); 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseImage( &grayImage2 ); 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseImage( &pyrImage1 ); 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseImage( &pyrImage2 ); 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return resNumCorrPoints; 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*-------------------------------------------------------------------------------------*/ 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvGrowPointsAndStatus(CvMat **oldPoints,CvMat **oldStatus,CvMat *addPoints,CvMat *addStatus,int addCreateNum) 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Add to existing points and status arrays new points or just grow */ 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newOldPoint = 0; 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newOldStatus = 0; 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int newTotalNumber = 0; 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvGrowPointsAndStatus" ); 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for errors */ 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( oldPoints == 0 || oldStatus == 0 ) 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( *oldPoints == 0 || *oldStatus == 0 ) 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(*oldPoints)) 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "oldPoints must be a pointer to a matrix" ); 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MASK_ARR(*oldStatus)) 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "oldStatus must be a pointer to a mask array" ); 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int oldNum; 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn oldNum = (*oldPoints)->cols; 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( oldNum < 1 ) 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of old points must be > 0" ); 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Define if need number of add points */ 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int addNum; 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn addNum = 0; 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( addPoints != 0 && addStatus != 0 ) 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* We have aditional points */ 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_IS_MAT(addPoints) && CV_IS_MASK_ARR(addStatus) ) 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn addNum = addPoints->cols; 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( addStatus->cols != addNum ) 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of add points and statuses must be the same" ); 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* */ 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numCoord; 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numCoord = (*oldPoints)->rows; 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newTotalNumber = oldNum + addNum + addCreateNum; 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( newTotalNumber ) 4996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Free allocated memory */ 5016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newOldPoint = cvCreateMat(numCoord,newTotalNumber,CV_64F); 5026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newOldStatus = cvCreateMat(1,newTotalNumber,CV_8S); 5036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy old values to */ 5056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 5066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Clear all values */ 5086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero(newOldPoint); 5096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero(newOldStatus); 5106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < oldNum; i++ ) 5126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currCoord; 5146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currCoord = 0; currCoord < numCoord; currCoord++ ) 5156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(newOldPoint,currCoord,i,cvmGet(*oldPoints,currCoord,i)); 5176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newOldStatus->data.ptr[i] = (*oldStatus)->data.ptr[i]; 5196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy additional points and statuses */ 5226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( addNum ) 5236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < addNum; i++ ) 5256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currCoord; 5276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currCoord = 0; currCoord < numCoord; currCoord++ ) 5286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(newOldPoint,currCoord,i+oldNum,cvmGet(addPoints,currCoord,i)); 5306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newOldStatus->data.ptr[i+oldNum] = addStatus->data.ptr[i]; 5326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //cvmSet(newOldStatus,0,i,cvmGet(addStatus,0,i)); 5336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Delete previous data */ 5376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(oldPoints); 5386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(oldStatus); 5396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* copy pointers */ 5416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *oldPoints = newOldPoint; 5426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *oldStatus = newOldStatus; 5436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 5466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return newTotalNumber; 5486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*-------------------------------------------------------------------------------------*/ 5506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvRemoveDoublePoins( CvMat *oldPoints,/* Points on prev image */ 5516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newPoints,/* New points */ 5526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *oldStatus,/* Status for old points */ 5536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newStatus, 5546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *origStatus, 5556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float threshold)/* Status for new points */ 5566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMemStorage* storage = 0; 5596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSubdiv2D* subdiv = 0; 5606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSeq* seq = 0; 5616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int originalPoints = 0; 5636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvRemoveDoublePoins" ); 5656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 5666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test input data */ 5686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( oldPoints == 0 || newPoints == 0 || 5696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn oldStatus == 0 || newStatus == 0 || origStatus == 0 ) 5706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 5726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(oldPoints) || !CV_IS_MAT(newPoints) ) 5756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Input parameters points must be a matrices" ); 5776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MASK_ARR(oldStatus) || !CV_IS_MASK_ARR(newStatus) || !CV_IS_MASK_ARR(origStatus) ) 5806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Input parameters statuses must be a mask array" ); 5826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int oldNumPoints; 5856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn oldNumPoints = oldPoints->cols; 5866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( oldNumPoints < 0 ) 5876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of oldPoints must be >= 0" ); 5896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( oldStatus->cols != oldNumPoints ) 5926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of old Points and old Statuses must be the same" ); 5946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int newNumPoints; 5976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newNumPoints = newPoints->cols; 5986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( newNumPoints < 0 ) 5996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of newPoints must be >= 0" ); 6016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( newStatus->cols != newNumPoints ) 6046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of new Points and new Statuses must be the same" ); 6066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( origStatus->cols != newNumPoints ) 6096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of new Points and new original Status must be the same" ); 6116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( oldPoints->rows != 2) 6146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "OldPoints must have 2 coordinates >= 0" ); 6166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( newPoints->rows != 2) 6196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "NewPoints must have 2 coordinates >= 0" ); 6216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( oldStatus->rows != 1 || newStatus->rows != 1 || origStatus->rows != 1 ) 6246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Statuses must have 1 row" ); 6266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* we have points on image and wants add new points */ 6296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* use subdivision for find nearest points */ 6306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Define maximum and minimum X and Y */ 6326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float minX,minY; 6336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float maxX,maxY; 6346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minX = minY = FLT_MAX; 6366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxX = maxY = FLT_MIN; 6376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 6396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < oldNumPoints; i++ ) 6416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( oldStatus->data.ptr[i] ) 6436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float x = (float)cvmGet(oldPoints,0,i); 6456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float y = (float)cvmGet(oldPoints,1,i); 6466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( x < minX ) 6486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minX = x; 6496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( x > maxX ) 6516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxX = x; 6526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( y < minY ) 6546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minY = y; 6556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( y > maxY ) 6576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxY = y; 6586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < newNumPoints; i++ ) 6626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( newStatus->data.ptr[i] ) 6646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float x = (float)cvmGet(newPoints,0,i); 6666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float y = (float)cvmGet(newPoints,1,i); 6676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( x < minX ) 6696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minX = x; 6706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( x > maxX ) 6726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxX = x; 6736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( y < minY ) 6756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn minY = y; 6766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( y > maxY ) 6786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxY = y; 6796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Creare subdivision for old image */ 6846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn storage = cvCreateMemStorage(0); 6856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// subdiv = cvCreateSubdivDelaunay2D( cvRect( 0, 0, size.width, size.height ), storage ); 6866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn subdiv = cvCreateSubdivDelaunay2D( cvRect( cvRound(minX)-5, cvRound(minY)-5, cvRound(maxX-minX)+10, cvRound(maxY-minY)+10 ), storage ); 6876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn seq = cvCreateSeq( 0, sizeof(*seq), sizeof(CvPoint2D32f), storage ); 6886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Insert each point from first image */ 6906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < oldNumPoints; i++ ) 6916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Add just exist points */ 6936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( oldStatus->data.ptr[i] ) 6946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f pt; 6966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt.x = (float)cvmGet(oldPoints,0,i); 6976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt.y = (float)cvmGet(oldPoints,1,i); 6986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSubdiv2DPoint* point; 7006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point = cvSubdivDelaunay2DInsert( subdiv, pt ); 7016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find nearest points */ 7066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* for each new point */ 7076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int flag; 7086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < newNumPoints; i++ ) 7096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn flag = 0; 7116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test just exist points */ 7126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( newStatus->data.ptr[i] ) 7136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn flag = 1; 7156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Let this is a good point */ 7166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //originalPoints++; 7176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f pt; 7196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt.x = (float)cvmGet(newPoints,0,i); 7216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt.y = (float)cvmGet(newPoints,1,i); 7226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSubdiv2DPoint* point = cvFindNearestPoint2D( subdiv, pt ); 7246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( point ) 7266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test distance of found nearest point */ 7286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double minDistance = icvSqDist2D32f( pt, point->pt ); 7296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( minDistance < threshold*threshold ) 7316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Point is double. Turn it off */ 7336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Set status */ 7346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //newStatus->data.ptr[i] = 0; 7356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* No this is a double point */ 7376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //originalPoints--; 7386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn flag = 0; 7396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn originalPoints += flag; 7436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn origStatus->data .ptr[i] = (uchar)flag; 7446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 7476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMemStorage( &storage ); 7496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return originalPoints; 7526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 7556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeProjectMatrix(CvMat* objPoints,CvMat* projPoints,CvMat* projMatr); 7576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*-------------------------------------------------------------------------------------*/ 7596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvComputeProjectMatrixStatus(CvMat *objPoints4D,CvMat *points2,CvMat *status, CvMat *projMatr) 7606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 7616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute number of good points */ 7626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int num = cvCountNonZero(status); 7636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create arrays */ 7656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *objPoints = 0; 7666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn objPoints = cvCreateMat(4,num,CV_64F); 7676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *points2D = 0; 7696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points2D = cvCreateMat(2,num,CV_64F); 7706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currVis = 0; 7726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 7736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 1 7746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn FILE *file; 7756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn file = fopen("d:\\test\\projStatus.txt","w"); 7766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 7776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int totalNum = objPoints4D->cols; 7786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < totalNum; i++ ) 7796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"%d (%d) ",i,status->data.ptr[i]); 7816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( status->data.ptr[i] ) 7826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 1 7856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double X,Y,Z,W; 7866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x,y; 7876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X = cvmGet(objPoints4D,0,i); 7886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Y = cvmGet(objPoints4D,1,i); 7896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Z = cvmGet(objPoints4D,2,i); 7906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn W = cvmGet(objPoints4D,3,i); 7916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = cvmGet(points2,0,i); 7936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = cvmGet(points2,1,i); 7946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"%d (%lf %lf %lf %lf) - (%lf %lf)",i,X,Y,Z,W,x,y ); 7956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 7966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(objPoints,0,currVis,cvmGet(objPoints4D,0,i)); 7976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(objPoints,1,currVis,cvmGet(objPoints4D,1,i)); 7986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(objPoints,2,currVis,cvmGet(objPoints4D,2,i)); 7996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(objPoints,3,currVis,cvmGet(objPoints4D,3,i)); 8006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points2D,0,currVis,cvmGet(points2,0,i)); 8026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points2D,1,currVis,cvmGet(points2,1,i)); 8036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currVis++; 8056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fprintf(file,"\n"); 8086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 1 8116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fclose(file); 8126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 8136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeProjectMatrix(objPoints,points2D,projMatr); 8156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Free allocated memory */ 8176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&objPoints); 8186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&points2D); 8196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 8206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*-------------------------------------------------------------------------------------*/ 8246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* For given N images 8256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn we have corresponding points on N images 8266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn computed projection matrices 8276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn reconstructed 4D points 8286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn we must to compute 8306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn*/ 8336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvAddNewImageToPrevious____( 8356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IplImage *newImage,//Image to add 8366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IplImage *oldImage,//Previous image 8376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *oldPoints,// previous 2D points on prev image (some points may be not visible) 8386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *oldPntStatus,//Status for each point on prev image 8396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *objPoints4D,//prev 4D points 8406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newPoints, //Points on new image corr for prev 8416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newPntStatus,// New point status for new image 8426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newFPoints2D1,//new feature points on prev image 8436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newFPoints2D2,//new feature points on new image 8446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newFPointsStatus, 8456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newProjMatr, 8466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int useFilter, 8476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double threshold)//New projection matrix 8486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 8496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *points2 = 0; 8506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *status = 0; 8516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newFPointsStatusTmp = 0; 8526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //CV_FUNCNAME( "icvAddNewImageToPrevious____" ); 8546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 8556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* First found correspondence points for images */ 8576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test input params */ 8596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 8616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = oldPoints->cols; 8626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Allocate memory */ 8646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points2 = cvCreateMat(2,numPoints,CV_64F); 8666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status = cvCreateMat(1,numPoints,CV_8S); 8676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newFPointsStatusTmp = cvCreateMat(1, newFPoints2D1->cols,CV_8S); 8686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int corrNum; 8706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn corrNum = icvFindCorrForGivenPoints( oldImage,/* Image 1 */ 8716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newImage,/* Image 2 */ 8726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn oldPoints, 8736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn oldPntStatus, 8746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn points2, 8756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status, 8766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn useFilter,/*Use fundamental matrix to filter points */ 8776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn threshold);/* Threshold for good points in filter */ 8786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(status,newPntStatus); 8806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(points2,newPoints); 8816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat projMatr; 8836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double projMatr_dat[12]; 8846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatr = cvMat(3,4,CV_64F,projMatr_dat); 8856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( corrNum >= 6 ) 8876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* We can compute projection matrix */ 8886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// icvComputeProjectMatrix(objPoints4D,points2,&projMatr); 8896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeProjectMatrixStatus(objPoints4D,points2,status,&projMatr); 8906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(&projMatr,newProjMatr); 8916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create new points and find correspondence */ 8936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCreateFeaturePoints(newImage, newFPoints2D2,newFPointsStatus); 8946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Good if we test new points before find corr points */ 8966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find correspondence for new found points */ 8986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvFindCorrForGivenPoints( newImage,/* Image 1 */ 8996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn oldImage,/* Image 2 */ 9006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newFPoints2D2, 9016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newFPointsStatus,//prev status 9026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newFPoints2D1, 9036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newFPointsStatusTmp,//new status 9046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn useFilter,/*Use fundamental matrix to filter points */ 9056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn threshold);/* Threshold for good points in filter */ 9066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We generated new points on image test for exist points */ 9086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Remove all new double points */ 9106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int origNum; 9126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find point of old image */ 9136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn origNum = icvRemoveDoublePoins( oldPoints,/* Points on prev image */ 9146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newFPoints2D1,/* New points */ 9156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn oldPntStatus,/* Status for old points */ 9166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newFPointsStatusTmp, 9176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newFPointsStatusTmp,//orig status 9186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20);/* Status for new points */ 9196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find double points on new image */ 9216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn origNum = icvRemoveDoublePoins( newPoints,/* Points on prev image */ 9226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newFPoints2D2,/* New points */ 9236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newPntStatus,/* Status for old points */ 9246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newFPointsStatusTmp, 9256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newFPointsStatusTmp,//orig status 9266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20);/* Status for new points */ 9276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Add all new good points to result */ 9316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy new status to old */ 9346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(newFPointsStatusTmp,newFPointsStatus); 9356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 9426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Free allocated memory */ 9446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 9466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 9476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*-------------------------------------------------------------------------------------*/ 9486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//int icvDelete// 9496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//CreateGood 9506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*-------------------------------------------------------------------------------------*/ 9526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint icvDeleteSparsInPoints( int numImages, 9536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat **points, 9546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat **status, 9556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *wasStatus)/* status of previous configuration */ 9566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 9576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Delete points which no exist on any of images */ 9586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* numImages - number of images */ 9596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* points - arrays of points for each image. Changing */ 9606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* status - arrays of status for each image. Changing */ 9616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Function returns number of common points */ 9626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int comNumber = 0; 9646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "icvDeleteSparsInPoints" ); 9656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 9666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Test for errors */ 9686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( numImages < 1 ) 9696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Number of images must be more than 0" ); 9716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( points == 0 || status == 0 ) 9746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 9766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints; 9786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numPoints = points[0]->cols; 9806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ////////// TESTS ////////// 9816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numCoord; 9836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numCoord = points[0]->rows;// !!! may be number of coordinates is not correct !!! 9846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 9866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currExistPoint; 9876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currExistPoint = 0; 9886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( wasStatus ) 9906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero(wasStatus); 9926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currImage; 9956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < numPoints; i++ ) 9966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int flag = 0; 9986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 9996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn flag |= status[currImage]->data.ptr[i]; 10016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flag ) 10046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Current point exists */ 10066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy points and status */ 10076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( currExistPoint != i )/* Copy just if different */ 10086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 10106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy points */ 10126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int currCoord = 0; currCoord < numCoord; currCoord++ ) 10136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(points[currImage],currCoord,currExistPoint, cvmGet(points[currImage],currCoord,i) ); 10156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy status */ 10186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status[currImage]->data.ptr[currExistPoint] = status[currImage]->data.ptr[i]; 10196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( wasStatus ) 10226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn wasStatus->data.ptr[i] = 1; 10246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currExistPoint++; 10276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Rest of final status of points must be set to 0 */ 10326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = currExistPoint; i < numPoints; i++ ) 10336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( currImage = 0; currImage < numImages; currImage++ ) 10356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status[currImage]->data.ptr[i] = 0; 10376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn comNumber = currExistPoint; 10416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 10436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return comNumber; 10446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 10456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 0 10476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*-------------------------------------------------------------------------------------*/ 10486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvGrowPointsArray(CvMat **points) 10496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 10506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 10536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*-------------------------------------------------------------------------------------*/ 10556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid icvAddNewArrayPoints() 10566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 10576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 10596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*-------------------------------------------------------------------------------------*/ 10616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 10626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn////////////////////////////////////////////////////////////////////////////////////////// 10646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn////////////////////////////////////////////////////////////////////////////////////////// 10656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn////////////////////////////////////////////////////////////////////////////////////////// 10666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn////////////////////////////////////////////////////////////////////////////////////////// 10676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Add image to existing images and corr points */ 10696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 0 10706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Returns: 1 if new image was added good */ 10716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* 0 image was not added. Not enought corr points */ 10726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint AddImageToStruct( IplImage *newImage,//Image to add 10736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IplImage *oldImage,//Previous image 10746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *oldPoints,// previous 2D points on prev image (some points may be not visible) 10756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *oldPntStatus,//Status for each point on prev image 10766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *objPoints4D,//prev 4D points 10776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newPntStatus,// New point status for new image 10786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newPoints,//New corresponding points on new image 10796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newPoints2D1,//new points on prev image 10806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newPoints2D2,//new points on new image 10816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newProjMatr);//New projection matrix 10826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 10836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Add new image. Create new corr points */ 10856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Track exist points from oldImage to newImage */ 10866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create new vector status */ 10876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *status; 10886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numPoints = oldPoints->cols; 10896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status = cvCreateMat(1,numPoints,CV_64F); 10906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Copy status */ 10916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert(pntStatus,status); 10926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int corrNum = FindCorrForGivenPoints(oldImage,newImage,oldPoints,newPoints,status); 10946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Status has new status of points */ 10966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat projMatr; 10986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double projMatr_dat[12]; 10996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn projMatr = cvMat(3,4,CV_64F,projMatr_dat); 11006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* If number of corr points is 6 or more can compute projection matrix */ 11026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( corrNum >= 6) 11036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute projection matrix for new image using corresponding points */ 11056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvComputeProjectMatrix(objPoints4D,newPoints,&projMatr); 11066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *tmpPoints; 11086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Create new points and find correspondence */ 11096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int num = FindFeaturePoints(newImage, &tmpPoints); 11106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( num > 0 ) 11116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newPoints; 11136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn newPoints = cvCreateMat(2,num,CV_64F); 11146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *status; 11156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn status = cvCreateMat(1,num,CV_64F); 11166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Set status for all points */ 11176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 11186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < num; i++ ) 11196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(status,0,i,1.0); 11216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int corrNum2 = FindCorrForGivenPoints(oldImage,newImage,tmpPoints,newPoints,status); 11246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* !!! Filter points using projection matrices or not ??? */ 11266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* !!! Need to filter nearest points */ 11286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Add new found points to exist points and optimize again */ 11306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *new2DPoints; 11316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *newStatus; 11326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* add new status to old status */ 11346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 11416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* No new points were found */ 11436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 11466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We can't compute projection matrix for new image */ 11486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return 0; 11496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 11506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 11526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 1153