+++ /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 "windows.h"
-
-#include <stdio.h>
-
-/* Valery Mosyagin */
-
-/* Function defenitions */
-
-/* ----------------- */
-
-void cvOptimizeLevenbergMarquardtBundle( CvMat** projMatrs, CvMat** observProjPoints,
- CvMat** pointsPres, int numImages,
- CvMat** resultProjMatrs, CvMat* resultPoints4D,int maxIter,double epsilon );
-
-int icvComputeProjectMatrices6Points( CvMat* points1,CvMat* points2,CvMat* points3,
- CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3);
-
-void icvFindBaseTransform(CvMat* points,CvMat* resultT);
-
-void GetGeneratorReduceFundSolution(CvMat* points1,CvMat* points2,CvMat* fundReduceCoef1,CvMat* fundReduceCoef2);
-
-int GetGoodReduceFundamMatrFromTwo(CvMat* fundReduceCoef1,CvMat* fundReduceCoef2,CvMat* resFundReduceCoef);
-
-void GetProjMatrFromReducedFundamental(CvMat* fundReduceCoefs,CvMat* projMatrCoefs);
-
-void icvComputeProjectMatrix(CvMat* objPoints,CvMat* projPoints,CvMat* projMatr);
-
-void icvComputeTransform4D(CvMat* points1,CvMat* points2,CvMat* transMatr);
-
-int icvComputeProjectMatricesNPoints( CvMat* points1,CvMat* points2,CvMat* points3,
- CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3,
- double threshold,/* Threshold for good point */
- double p,/* Probability of good result. */
- CvMat* status,
- CvMat* points4D);
-
-int icvComputeProjectMatricesNPoints( CvMat* points1,CvMat* points2,CvMat* points3,
- CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3,
- double threshold,/* Threshold for good point */
- double p,/* Probability of good result. */
- CvMat* status,
- CvMat* points4D);
-
-void icvReconstructPointsFor3View( CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3,
- CvMat* projPoints1,CvMat* projPoints2,CvMat* projPoints3,
- CvMat* points4D);
-
-void icvReconstructPointsFor3View( CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3,
- CvMat* projPoints1,CvMat* projPoints2,CvMat* projPoints3,
- CvMat* points4D);
-
-/*==========================================================================================*/
-/* Functions for calculation the tensor */
-/*==========================================================================================*/
-#if 1
-void fprintMatrix(FILE* file,CvMat* matrix)
-{
- int i,j;
- fprintf(file,"\n");
- for( i=0;i<matrix->rows;i++ )
- {
- for(j=0;j<matrix->cols;j++)
- {
- fprintf(file,"%10.7lf ",cvmGet(matrix,i,j));
- }
- fprintf(file,"\n");
- }
-}
-#endif
-/*==========================================================================================*/
-
-void icvNormalizePoints( CvMat* points, CvMat* normPoints,CvMat* cameraMatr )
-{
- /* Normalize image points using camera matrix */
-
- CV_FUNCNAME( "icvNormalizePoints" );
- __BEGIN__;
-
- /* Test for null pointers */
- if( points == 0 || normPoints == 0 || cameraMatr == 0 )
- {
- CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
- }
-
- if( !CV_IS_MAT(points) || !CV_IS_MAT(normPoints) || !CV_IS_MAT(cameraMatr) )
- {
- CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
- }
-
- int numPoints;
- numPoints = points->cols;
- if( numPoints <= 0 || numPoints != normPoints->cols )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be the same and more than 0" );
- }
-
- if( normPoints->rows != 2 || normPoints->rows != points->rows )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Points must have 2 coordinates" );
- }
-
- if(cameraMatr->rows != 3 || cameraMatr->cols != 3)
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of camera matrix must be 3x3" );
- }
-
- double fx,fy,cx,cy;
-
- fx = cvmGet(cameraMatr,0,0);
- fy = cvmGet(cameraMatr,1,1);
- cx = cvmGet(cameraMatr,0,2);
- cy = cvmGet(cameraMatr,1,2);
-
- int i;
- for( i = 0; i < numPoints; i++ )
- {
- cvmSet(normPoints, 0, i, (cvmGet(points,0,i) - cx) / fx );
- cvmSet(normPoints, 1, i, (cvmGet(points,1,i) - cy) / fy );
- }
-
- __END__;
-
- return;
-}
-
-
-/*=====================================================================================*/
-/*
-Computes projection matrices for given 6 points on 3 images
-May returns 3 results. */
-int icvComputeProjectMatrices6Points( CvMat* points1,CvMat* points2,CvMat* points3,
- CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3/*,
- CvMat* points4D*/)
-{
- /* Test input data correctness */
-
- int numSol = 0;
-
- CV_FUNCNAME( "icvComputeProjectMatrices6Points" );
- __BEGIN__;
-
- /* Test for null pointers */
- if( points1 == 0 || points2 == 0 || points3 == 0 ||
- projMatr1 == 0 || projMatr2 == 0 || projMatr3 == 0 )
- {
- CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
- }
-
- if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2) || !CV_IS_MAT(points3) ||
- !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) || !CV_IS_MAT(projMatr3) )
- {
- CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
- }
-
- if( (points1->cols != points2->cols) || (points1->cols != points3->cols) || (points1->cols != 6) /* || (points4D->cols !=6) */)
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be same and == 6" );
- }
-
- if( points1->rows != 2 || points2->rows != 2 || points3->rows != 2 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of points coordinates must be 2" );
- }
-
- if( projMatr1->cols != 4 || projMatr2->cols != 4 || projMatr3->cols != 4 ||
- !(projMatr1->rows == 3 && projMatr2->rows == 3 && projMatr3->rows == 3) &&
- !(projMatr1->rows == 9 && projMatr2->rows == 9 && projMatr3->rows == 9) )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of project matrix must be 3x4 or 9x4 (for 3 matrices)" );
- }
-
-#if 0
- if( points4D->row != 4 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of points4D must be 4" );
- }
-#endif
-
- /* Find transform matrix for each camera */
- int i;
- CvMat* points[3];
- points[0] = points1;
- points[1] = points2;
- points[2] = points3;
-
- CvMat* projMatrs[3];
- projMatrs[0] = projMatr1;
- projMatrs[1] = projMatr2;
- projMatrs[2] = projMatr3;
-
- CvMat transMatr;
- double transMatr_dat[9];
- transMatr = cvMat(3,3,CV_64F,transMatr_dat);
-
- CvMat corrPoints1;
- CvMat corrPoints2;
-
- double corrPoints_dat[3*3*2];/* 3-point(images) by 3-coordinates by 2-correspondence*/
-
- corrPoints1 = cvMat(3,3,CV_64F,corrPoints_dat); /* 3-coordinates for each of 3-points(3-image) */
- corrPoints2 = cvMat(3,3,CV_64F,corrPoints_dat+9);/* 3-coordinates for each of 3-points(3-image) */
-
- for( i = 0; i < 3; i++ )/* for each image */
- {
- /* Get last 4 points for computing transformation */
- CvMat tmpPoints;
- /* find base points transform for last four points on i-th image */
- cvGetSubRect(points[i],&tmpPoints,cvRect(2,0,4,2));
- icvFindBaseTransform(&tmpPoints,&transMatr);
-
- {/* We have base transform. Compute error scales for three first points */
- CvMat trPoint;
- double trPoint_dat[3*3];
- trPoint = cvMat(3,3,CV_64F,trPoint_dat);
- /* fill points */
- for( int kk = 0; kk < 3; kk++ )
- {
- cvmSet(&trPoint,0,kk,cvmGet(points[i],0,kk+2));
- cvmSet(&trPoint,1,kk,cvmGet(points[i],1,kk+2));
- cvmSet(&trPoint,2,kk,1);
- }
-
- /* Transform points */
- CvMat resPnts;
- double resPnts_dat[9];
- resPnts = cvMat(3,3,CV_64F,resPnts_dat);
- cvmMul(&transMatr,&trPoint,&resPnts);
- }
-
- /* Transform two first points */
- for( int j = 0; j < 2; j++ )
- {
- CvMat pnt;
- double pnt_dat[3];
- pnt = cvMat(3,1,CV_64F,pnt_dat);
- pnt_dat[0] = cvmGet(points[i],0,j);
- pnt_dat[1] = cvmGet(points[i],1,j);
- pnt_dat[2] = 1.0;
-
- CvMat trPnt;
- double trPnt_dat[3];
- trPnt = cvMat(3,1,CV_64F,trPnt_dat);
-
- cvmMul(&transMatr,&pnt,&trPnt);
-
- /* Collect transformed points */
- corrPoints_dat[j * 9 + 0 * 3 + i] = trPnt_dat[0];/* x */
- corrPoints_dat[j * 9 + 1 * 3 + i] = trPnt_dat[1];/* y */
- corrPoints_dat[j * 9 + 2 * 3 + i] = trPnt_dat[2];/* w */
- }
- }
-
- /* We have computed corr points. Now we can compute generators for reduced fundamental matrix */
-
- /* Compute generators for reduced fundamental matrix from 3 pair of collect points */
- CvMat fundReduceCoef1;
- CvMat fundReduceCoef2;
- double fundReduceCoef1_dat[5];
- double fundReduceCoef2_dat[5];
-
- fundReduceCoef1 = cvMat(1,5,CV_64F,fundReduceCoef1_dat);
- fundReduceCoef2 = cvMat(1,5,CV_64F,fundReduceCoef2_dat);
-
- GetGeneratorReduceFundSolution(&corrPoints1, &corrPoints2, &fundReduceCoef1, &fundReduceCoef2);
-
- /* Choose best solutions for two generators. We can get 3 solutions */
- CvMat resFundReduceCoef;
- double resFundReduceCoef_dat[3*5];
-
- resFundReduceCoef = cvMat(3,5,CV_64F,resFundReduceCoef_dat);
-
- numSol = GetGoodReduceFundamMatrFromTwo(&fundReduceCoef1, &fundReduceCoef2,&resFundReduceCoef);
-
- int maxSol;
- maxSol = projMatrs[0]->rows / 3;
-
- int currSol;
- for( currSol = 0; (currSol < numSol && currSol < maxSol); currSol++ )
- {
- /* For current solution compute projection matrix */
- CvMat fundCoefs;
- cvGetSubRect(&resFundReduceCoef, &fundCoefs, cvRect(0,currSol,5,1));
-
- CvMat projMatrCoefs;
- double projMatrCoefs_dat[4];
- projMatrCoefs = cvMat(1,4,CV_64F,projMatrCoefs_dat);
-
- GetProjMatrFromReducedFundamental(&fundCoefs,&projMatrCoefs);
- /* we have computed coeffs for reduced project matrix */
-
- CvMat objPoints;
- double objPoints_dat[4*6];
- objPoints = cvMat(4,6,CV_64F,objPoints_dat);
- cvZero(&objPoints);
-
- /* fill object points */
- for( i =0; i < 4; i++ )
- {
- objPoints_dat[i*6] = 1;
- objPoints_dat[i*6+1] = projMatrCoefs_dat[i];
- objPoints_dat[i*7+2] = 1;
- }
-
- int currCamera;
- for( currCamera = 0; currCamera < 3; currCamera++ )
- {
-
- CvMat projPoints;
- double projPoints_dat[3*6];
- projPoints = cvMat(3,6,CV_64F,projPoints_dat);
-
- /* fill projected points for current camera */
- for( i = 0; i < 6; i++ )/* for each points for current camera */
- {
- projPoints_dat[6*0+i] = cvmGet(points[currCamera],0,i);/* x */
- projPoints_dat[6*1+i] = cvmGet(points[currCamera],1,i);/* y */
- projPoints_dat[6*2+i] = 1;/* w */
- }
-
- /* compute project matrix for current camera */
- CvMat projMatrix;
- double projMatrix_dat[3*4];
- projMatrix = cvMat(3,4,CV_64F,projMatrix_dat);
-
- icvComputeProjectMatrix(&objPoints,&projPoints,&projMatrix);
-
- /* Add this matrix to result */
- CvMat tmpSubRes;
- cvGetSubRect(projMatrs[currCamera],&tmpSubRes,cvRect(0,currSol*3,4,3));
- cvConvert(&projMatrix,&tmpSubRes);
- }
-
- /* We know project matrices. And we can reconstruct 6 3D-points if need */
-#if 0
- if( points4D )
- {
- if( currSol < points4D->rows / 4 )
- {
- CvMat tmpPoints4D;
- double tmpPoints4D_dat[4*6];
- tmpPoints4D = cvMat(4,6,CV_64F,tmpPoints4D_dat);
-
- icvReconstructPointsFor3View( &wProjMatr[0], &wProjMatr[1], &wProjMatr[2],
- points1, points2, points3,
- &tmpPoints4D);
-
- CvMat tmpSubRes;
- cvGetSubRect(points4D,tmpSubRes,cvRect(0,currSol*4,6,4));
- cvConvert(tmpPoints4D,points4D);
- }
- }
-#endif
-
- }/* for all sollutions */
-
- __END__;
- return numSol;
-}
-
-/*==========================================================================================*/
-int icvGetRandNumbers(int range,int count,int* arr)
-{
- /* Generate random numbers [0,range-1] */
-
- CV_FUNCNAME( "icvGetRandNumbers" );
- __BEGIN__;
-
- /* Test input data */
- if( arr == 0 )
- {
- CV_ERROR( CV_StsNullPtr, "Parameter 'arr' is a NULL pointer" );
- }
-
-
- /* Test for errors input data */
- if( range < count || range <= 0 )
- {
- CV_ERROR( CV_StsOutOfRange, "Can't generate such numbers. Count must be <= range and range must be > 0" );
- }
-
- int i,j;
- int newRand;
- for( i = 0; i < count; i++ )
- {
-
- int haveRep = 0;/* firstly we have not repeats */
- do
- {
- /* generate new number */
- newRand = rand()%range;
- haveRep = 0;
- /* Test for repeats in previous numbers */
- for( j = 0; j < i; j++ )
- {
- if( arr[j] == newRand )
- {
- haveRep = 1;
- break;
- }
- }
- } while(haveRep);
-
- /* We have good random number */
- arr[i] = newRand;
- }
- __END__;
- return 1;
-}
-/*==========================================================================================*/
-void icvSelectColsByNumbers(CvMat* srcMatr, CvMat* dstMatr, int* indexes,int number)
-{
-
- CV_FUNCNAME( "icvSelectColsByNumbers" );
- __BEGIN__;
-
- /* Test input data */
- if( srcMatr == 0 || dstMatr == 0 || indexes == 0)
- {
- CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
- }
-
- if( !CV_IS_MAT(srcMatr) || !CV_IS_MAT(dstMatr) )
- {
- CV_ERROR( CV_StsUnsupportedFormat, "srcMatr and dstMatr must be a matrices" );
- }
-
- int srcSize;
- int numRows;
- numRows = srcMatr->rows;
- srcSize = srcMatr->cols;
-
- if( numRows != dstMatr->rows )
- {
- CV_ERROR( CV_StsOutOfRange, "Number of rows of matrices must be the same" );
- }
-
- int dst;
- for( dst = 0; dst < number; dst++ )
- {
- int src = indexes[dst];
- if( src >=0 && src < srcSize )
- {
- /* Copy each elements in column */
- int i;
- for( i = 0; i < numRows; i++ )
- {
- cvmSet(dstMatr,i,dst,cvmGet(srcMatr,i,src));
- }
- }
- }
-
- __END__;
- return;
-}
-
-/*==========================================================================================*/
-void icvProject4DPoints(CvMat* points4D,CvMat* projMatr, CvMat* projPoints)
-{
-
- CvMat* tmpProjPoints = 0;
-
- CV_FUNCNAME( "icvProject4DPoints" );
-
- __BEGIN__;
-
- if( points4D == 0 || projMatr == 0 || projPoints == 0)
- {
- CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
- }
-
- if( !CV_IS_MAT(points4D) || !CV_IS_MAT(projMatr) || !CV_IS_MAT(projPoints) )
- {
- CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
- }
-
- int numPoints;
- numPoints = points4D->cols;
- if( numPoints < 1 )
- {
- CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" );
- }
-
- if( numPoints != projPoints->cols )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be the same");
- }
-
- if( projPoints->rows != 2 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of projected points must be 2");
- }
-
- if( points4D->rows != 4 )
- {
- CV_ERROR(CV_StsUnmatchedSizes, "Number of coordinates of 4D points must be 4");
- }
-
- if( projMatr->cols != 4 || projMatr->rows != 3 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of projection matrix must be 3x4");
- }
-
-
- CV_CALL( tmpProjPoints = cvCreateMat(3,numPoints,CV_64F) );
-
- cvmMul(projMatr,points4D,tmpProjPoints);
-
- /* Scale points */
- int i;
- for( i = 0; i < numPoints; i++ )
- {
- double scale,x,y;
-
- scale = cvmGet(tmpProjPoints,2,i);
- x = cvmGet(tmpProjPoints,0,i);
- y = cvmGet(tmpProjPoints,1,i);
-
- if( fabs(scale) > 1e-7 )
- {
- x /= scale;
- y /= scale;
- }
- else
- {
- x = 1e8;
- y = 1e8;
- }
-
- cvmSet(projPoints,0,i,x);
- cvmSet(projPoints,1,i,y);
- }
-
- __END__;
-
- cvReleaseMat(&tmpProjPoints);
-
- return;
-}
-/*==========================================================================================*/
-int icvCompute3ProjectMatricesNPointsStatus( CvMat** points,/* 3 arrays of points on image */
- CvMat** projMatrs,/* array of 3 prejection matrices */
- CvMat** statuses,/* 3 arrays of status of points */
- double threshold,/* Threshold for good point */
- double p,/* Probability of good result. */
- CvMat* resStatus,
- CvMat* points4D)
-{
- int numProjMatrs = 0;
- unsigned char *comStat = 0;
- CvMat *triPoints[3] = {0,0,0};
- CvMat *status = 0;
- CvMat *triPoints4D = 0;
-
- CV_FUNCNAME( "icvCompute3ProjectMatricesNPointsStatus" );
- __BEGIN__;
-
- /* Test for errors */
- if( points == 0 || projMatrs == 0 || statuses == 0 || resStatus == 0 )
- {
- CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
- }
-
- int currImage;
- for( currImage = 0; currImage < 3; currImage++ )
- {
- /* Test for null pointers */
- if( points[currImage] == 0 )
- {
- CV_ERROR( CV_StsNullPtr, "Some of points arrays is a NULL pointer" );
- }
-
- if( projMatrs[currImage] == 0 )
- {
- CV_ERROR( CV_StsNullPtr, "Some of projMatr is a NULL pointer" );
- }
-
- if( statuses[currImage] == 0 )
- {
- CV_ERROR( CV_StsNullPtr, "Some of status arrays is a NULL pointer" );
- }
-
- /* Test for matrices */
- if( !CV_IS_MAT(points[currImage]) )
- {
- CV_ERROR( CV_StsNullPtr, "Some of points arrays is not a matrix" );
- }
-
- if( !CV_IS_MAT(projMatrs[currImage]) )
- {
- CV_ERROR( CV_StsNullPtr, "Some of projMatr is not a matrix" );
- }
-
- if( !CV_IS_MASK_ARR(statuses[currImage]) )
- {
- CV_ERROR( CV_StsNullPtr, "Some of status arrays is not a mask array" );
- }
- }
-
- int numPoints;
- numPoints = points[0]->cols;
- if( numPoints < 6 )
- {
- CV_ERROR( CV_StsOutOfRange, "Number points must be more than 6" );
- }
-
- for( currImage = 0; currImage < 3; currImage++ )
- {
- if( points[currImage]->cols != numPoints || statuses[currImage]->cols != numPoints )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of points and statuses must be the same" );
- }
-
- if( points[currImage]->rows != 2 )
- {
- CV_ERROR( CV_StsOutOfRange, "Number of points coordinates must be == 2" );
- }
-
- if( statuses[currImage]->rows != 1 )
- {
- CV_ERROR( CV_StsOutOfRange, "Each of status must be matrix 1xN" );
- }
-
- if( projMatrs[currImage]->rows != 3 || projMatrs[currImage]->cols != 4 )
- {
- CV_ERROR( CV_StsOutOfRange, "Each of projection matrix must be 3x4" );
- }
- }
-
-
- /* Create common status for all points */
-
- int i;
-
- CV_CALL( comStat = (unsigned char*)cvAlloc(sizeof(unsigned char)*numPoints) );
-
- unsigned char *stats[3];
-
- stats[0] = statuses[0]->data.ptr;
- stats[1] = statuses[1]->data.ptr;
- stats[2] = statuses[2]->data.ptr;
-
- int numTripl;
- numTripl = 0;
- for( i = 0; i < numPoints; i++ )
- {
- comStat[i] = (unsigned char)(stats[0][i] * stats[1][i] * stats[2][i]);
- numTripl += comStat[i];
- }
-
- if( numTripl > 0 )
- {
- /* Create new arrays with points */
- CV_CALL( triPoints[0] = cvCreateMat(2,numTripl,CV_64F) );
- CV_CALL( triPoints[1] = cvCreateMat(2,numTripl,CV_64F) );
- CV_CALL( triPoints[2] = cvCreateMat(2,numTripl,CV_64F) );
- if( points4D )
- {
- CV_CALL( triPoints4D = cvCreateMat(4,numTripl,CV_64F) );
- }
-
- /* Create status array */
- CV_CALL( status = cvCreateMat(1,numTripl,CV_64F) );
-
- /* Copy points to new arrays */
- int currPnt = 0;
- for( i = 0; i < numPoints; i++ )
- {
- if( comStat[i] )
- {
- for( currImage = 0; currImage < 3; currImage++ )
- {
- cvmSet(triPoints[currImage],0,currPnt,cvmGet(points[currImage],0,i));
- cvmSet(triPoints[currImage],1,currPnt,cvmGet(points[currImage],1,i));
- }
- currPnt++;
- }
- }
-
- /* Call function */
- numProjMatrs = icvComputeProjectMatricesNPoints( triPoints[0],triPoints[1],triPoints[2],
- projMatrs[0],projMatrs[1],projMatrs[2],
- threshold,/* Threshold for good point */
- p,/* Probability of good result. */
- status,
- triPoints4D);
-
- /* Get computed status and set to result */
- cvZero(resStatus);
- currPnt = 0;
- for( i = 0; i < numPoints; i++ )
- {
- if( comStat[i] )
- {
- if( cvmGet(status,0,currPnt) > 0 )
- {
- resStatus->data.ptr[i] = 1;
- }
- currPnt++;
- }
- }
-
- if( triPoints4D )
- {
- /* Copy copmuted 4D points */
- cvZero(points4D);
- currPnt = 0;
- for( i = 0; i < numPoints; i++ )
- {
- if( comStat[i] )
- {
- if( cvmGet(status,0,currPnt) > 0 )
- {
- cvmSet( points4D, 0, i, cvmGet( triPoints4D , 0, currPnt) );
- cvmSet( points4D, 1, i, cvmGet( triPoints4D , 1, currPnt) );
- cvmSet( points4D, 2, i, cvmGet( triPoints4D , 2, currPnt) );
- cvmSet( points4D, 3, i, cvmGet( triPoints4D , 3, currPnt) );
- }
- currPnt++;
- }
- }
- }
- }
-
- __END__;
-
- /* Free allocated memory */
- cvReleaseMat(&status);
- cvFree( &comStat);
- cvReleaseMat(&status);
-
- cvReleaseMat(&triPoints[0]);
- cvReleaseMat(&triPoints[1]);
- cvReleaseMat(&triPoints[2]);
- cvReleaseMat(&triPoints4D);
-
- return numProjMatrs;
-
-}
-
-/*==========================================================================================*/
-int icvComputeProjectMatricesNPoints( CvMat* points1,CvMat* points2,CvMat* points3,
- CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3,
- double threshold,/* Threshold for good point */
- double p,/* Probability of good result. */
- CvMat* status,
- CvMat* points4D)
-{
- /* Returns status for each point, Good or bad */
-
- /* Compute projection matrices using N points */
-
- char* flags = 0;
- char* bestFlags = 0;
-
- int numProjMatrs = 0;
-
- CvMat* tmpProjPoints[3]={0,0,0};
- CvMat* recPoints4D = 0;
- CvMat *reconPoints4D = 0;
-
-
- CV_FUNCNAME( "icvComputeProjectMatricesNPoints" );
- __BEGIN__;
-
- CvMat* points[3];
- points[0] = points1;
- points[1] = points2;
- points[2] = points3;
-
- /* Test for errors */
- if( points1 == 0 || points2 == 0 || points3 == 0 ||
- projMatr1 == 0 || projMatr2 == 0 || projMatr3 == 0 ||
- status == 0)
- {
- CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
- }
-
- if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2) || !CV_IS_MAT(points3) ||
- !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) || !CV_IS_MAT(projMatr3) ||
- !CV_IS_MAT(status) )
- {
- CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
- }
-
- int numPoints;
- numPoints = points1->cols;
-
- if( numPoints < 6 )
- {
- CV_ERROR( CV_StsOutOfRange, "Number points must be more than 6" );
- }
-
- if( numPoints != points2->cols || numPoints != points3->cols )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "number of points must be the same" );
- }
-
- if( p < 0 || p > 1.0 )
- {
- CV_ERROR( CV_StsOutOfRange, "Probability must be >=0 and <=1" );
- }
-
- if( threshold < 0 )
- {
- CV_ERROR( CV_StsOutOfRange, "Threshold for good points must be at least >= 0" );
- }
-
- CvMat* projMatrs[3];
-
- projMatrs[0] = projMatr1;
- projMatrs[1] = projMatr2;
- projMatrs[2] = projMatr3;
-
- int i;
- for( i = 0; i < 3; i++ )
- {
- if( projMatrs[i]->cols != 4 || projMatrs[i]->rows != 3 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of projection matrices must be 3x4" );
- }
- }
-
- for( i = 0; i < 3; i++ )
- {
- if( points[i]->rows != 2)
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of points must be 2" );
- }
- }
-
- /* use RANSAC algorithm to compute projection matrices */
-
- CV_CALL( recPoints4D = cvCreateMat(4,numPoints,CV_64F) );
- CV_CALL( tmpProjPoints[0] = cvCreateMat(2,numPoints,CV_64F) );
- CV_CALL( tmpProjPoints[1] = cvCreateMat(2,numPoints,CV_64F) );
- CV_CALL( tmpProjPoints[2] = cvCreateMat(2,numPoints,CV_64F) );
-
- CV_CALL( flags = (char*)cvAlloc(sizeof(char)*numPoints) );
- CV_CALL( bestFlags = (char*)cvAlloc(sizeof(char)*numPoints) );
-
- {
- int NumSamples = 500;/* just init number of samples */
- int wasCount = 0; /* count of choosing samples */
- int maxGoodPoints = 0;
- int numGoodPoints = 0;
-
- double bestProjMatrs_dat[36];
- CvMat bestProjMatrs[3];
- bestProjMatrs[0] = cvMat(3,4,CV_64F,bestProjMatrs_dat);
- bestProjMatrs[1] = cvMat(3,4,CV_64F,bestProjMatrs_dat+12);
- bestProjMatrs[2] = cvMat(3,4,CV_64F,bestProjMatrs_dat+24);
-
- double tmpProjMatr_dat[36*3];
- CvMat tmpProjMatr[3];
- tmpProjMatr[0] = cvMat(9,4,CV_64F,tmpProjMatr_dat);
- tmpProjMatr[1] = cvMat(9,4,CV_64F,tmpProjMatr_dat+36);
- tmpProjMatr[2] = cvMat(9,4,CV_64F,tmpProjMatr_dat+72);
-
- /* choosen points */
-
- while( wasCount < NumSamples )
- {
- /* select samples */
- int randNumbs[6];
- icvGetRandNumbers(numPoints,6,randNumbs);
-
- /* random numbers of points was generated */
- /* select points */
-
- double selPoints_dat[2*6*3];
- CvMat selPoints[3];
- selPoints[0] = cvMat(2,6,CV_64F,selPoints_dat);
- selPoints[1] = cvMat(2,6,CV_64F,selPoints_dat+12);
- selPoints[2] = cvMat(2,6,CV_64F,selPoints_dat+24);
-
- /* Copy 6 point for random indexes */
- icvSelectColsByNumbers( points[0], &selPoints[0], randNumbs,6);
- icvSelectColsByNumbers( points[1], &selPoints[1], randNumbs,6);
- icvSelectColsByNumbers( points[2], &selPoints[2], randNumbs,6);
-
- /* Compute projection matrices for this points */
- int numProj = icvComputeProjectMatrices6Points( &selPoints[0],&selPoints[1],&selPoints[2],
- &tmpProjMatr[0],&tmpProjMatr[1],&tmpProjMatr[2]);
-
- /* Compute number of good points for each matrix */
- CvMat proj6[3];
- for( int currProj = 0; currProj < numProj; currProj++ )
- {
- cvGetSubArr(&tmpProjMatr[0],&proj6[0],cvRect(0,currProj*3,4,3));
- cvGetSubArr(&tmpProjMatr[1],&proj6[1],cvRect(0,currProj*3,4,3));
- cvGetSubArr(&tmpProjMatr[2],&proj6[2],cvRect(0,currProj*3,4,3));
-
- /* Reconstruct points for projection matrices */
- icvReconstructPointsFor3View( &proj6[0],&proj6[1],&proj6[2],
- points[0], points[1], points[2],
- recPoints4D);
-
- /* Project points to images using projection matrices */
- icvProject4DPoints(recPoints4D,&proj6[0],tmpProjPoints[0]);
- icvProject4DPoints(recPoints4D,&proj6[1],tmpProjPoints[1]);
- icvProject4DPoints(recPoints4D,&proj6[2],tmpProjPoints[2]);
-
- /* Compute distances and number of good points (inliers) */
- int i;
- int currImage;
- numGoodPoints = 0;
- for( i = 0; i < numPoints; i++ )
- {
- double dist=-1;
- dist = 0;
- /* Choose max distance for each of three points */
- for( currImage = 0; currImage < 3; currImage++ )
- {
- double x1,y1,x2,y2;
- x1 = cvmGet(tmpProjPoints[currImage],0,i);
- y1 = cvmGet(tmpProjPoints[currImage],1,i);
- x2 = cvmGet(points[currImage],0,i);
- y2 = cvmGet(points[currImage],1,i);
-
- double dx,dy;
- dx = x1-x2;
- dy = y1-y2;
-#if 1
- double newDist = dx*dx+dy*dy;
- if( newDist > dist )
- {
- dist = newDist;
- }
-#else
- dist += sqrt(dx*dx+dy*dy)/3.0;
-#endif
- }
- dist = sqrt(dist);
- flags[i] = (char)(dist > threshold ? 0 : 1);
- numGoodPoints += flags[i];
-
- }
-
-
- if( numGoodPoints > maxGoodPoints )
- {/* Copy current projection matrices as best */
-
- cvCopy(&proj6[0],&bestProjMatrs[0]);
- cvCopy(&proj6[1],&bestProjMatrs[1]);
- cvCopy(&proj6[2],&bestProjMatrs[2]);
-
- maxGoodPoints = numGoodPoints;
- /* copy best flags */
- memcpy(bestFlags,flags,sizeof(flags[0])*numPoints);
-
- /* Adaptive number of samples to count*/
- double ep = 1 - (double)numGoodPoints / (double)numPoints;
- if( ep == 1 )
- {
- ep = 0.5;/* if there is not good points set ration of outliers to 50% */
- }
-
- double newNumSamples = (log(1-p) / log(1-pow(1-ep,6)));
- if( newNumSamples < double(NumSamples) )
- {
- NumSamples = cvRound(newNumSamples);
- }
- }
- }
-
- wasCount++;
- }
-#if 0
- char str[300];
- sprintf(str,"Initial numPoints = %d\nmaxGoodPoints=%d\nRANSAC made %d steps",
- numPoints,
- maxGoodPoints,
- cvRound(wasCount));
- MessageBox(0,str,"Info",MB_OK|MB_TASKMODAL);
-#endif
-
- /* we may have best 6-point projection matrices. */
- /* and best points */
- /* use these points to improve matrices */
-
- if( maxGoodPoints < 6 )
- {
- /* matrix not found */
- numProjMatrs = 0;
- }
- else
- {
- /* We may Improove matrices using ---- method */
- /* We may try to use Levenberg-Marquardt optimization */
- //int currIter = 0;
- int finalGoodPoints = 0;
- char *goodFlags = 0;
- goodFlags = (char*)cvAlloc(numPoints*sizeof(char));
-
- int needRepeat;
- do
- {
-#if 0
-/* Version without using status for Levenberg-Marquardt minimization */
-
- CvMat *optStatus;
- optStatus = cvCreateMat(1,numPoints,CV_64F);
- int testNumber = 0;
- for( i=0;i<numPoints;i++ )
- {
- cvmSet(optStatus,0,i,(double)bestFlags[i]);
- testNumber += bestFlags[i];
- }
-
- char str2[200];
- sprintf(str2,"test good num=%d\nmaxGoodPoints=%d",testNumber,maxGoodPoints);
- MessageBox(0,str2,"Info",MB_OK|MB_TASKMODAL);
-
- CvMat *gPresPoints;
- gPresPoints = cvCreateMat(1,maxGoodPoints,CV_64F);
- for( i = 0; i < maxGoodPoints; i++)
- {
- cvmSet(gPresPoints,0,i,1.0);
- }
-
- /* Create array of points pres */
- CvMat *pointsPres[3];
- pointsPres[0] = gPresPoints;
- pointsPres[1] = gPresPoints;
- pointsPres[2] = gPresPoints;
-
- /* Create just good points 2D */
- CvMat *gPoints[3];
- icvCreateGoodPoints(points[0],&gPoints[0],optStatus);
- icvCreateGoodPoints(points[1],&gPoints[1],optStatus);
- icvCreateGoodPoints(points[2],&gPoints[2],optStatus);
-
- /* Create 4D points array for good points */
- CvMat *resPoints4D;
- resPoints4D = cvCreateMat(4,maxGoodPoints,CV_64F);
-
- CvMat* projMs[3];
-
- projMs[0] = &bestProjMatrs[0];
- projMs[1] = &bestProjMatrs[1];
- projMs[2] = &bestProjMatrs[2];
-
-
- CvMat resProjMatrs[3];
- double resProjMatrs_dat[36];
- resProjMatrs[0] = cvMat(3,4,CV_64F,resProjMatrs_dat);
- resProjMatrs[1] = cvMat(3,4,CV_64F,resProjMatrs_dat+12);
- resProjMatrs[2] = cvMat(3,4,CV_64F,resProjMatrs_dat+24);
-
- CvMat* resMatrs[3];
- resMatrs[0] = &resProjMatrs[0];
- resMatrs[1] = &resProjMatrs[1];
- resMatrs[2] = &resProjMatrs[2];
-
- cvOptimizeLevenbergMarquardtBundle( projMs,//projMs,
- gPoints,//points,//points2D,
- pointsPres,//pointsPres,
- 3,
- resMatrs,//resProjMatrs,
- resPoints4D,//resPoints4D,
- 100, 1e-9 );
-
- /* We found optimized projection matrices */
-
- CvMat *reconPoints4D;
- reconPoints4D = cvCreateMat(4,numPoints,CV_64F);
-
- /* Reconstruct all points using found projection matrices */
- icvReconstructPointsFor3View( &resProjMatrs[0],&resProjMatrs[1],&resProjMatrs[2],
- points[0], points[1], points[2],
- reconPoints4D);
-
- /* Project points to images using projection matrices */
- icvProject4DPoints(reconPoints4D,&resProjMatrs[0],tmpProjPoints[0]);
- icvProject4DPoints(reconPoints4D,&resProjMatrs[1],tmpProjPoints[1]);
- icvProject4DPoints(reconPoints4D,&resProjMatrs[2],tmpProjPoints[2]);
-
-
- /* Compute error for each point and select good */
-
- int currImage;
- finalGoodPoints = 0;
- for( i = 0; i < numPoints; i++ )
- {
- double dist=-1;
- /* Choose max distance for each of three points */
- for( currImage = 0; currImage < 3; currImage++ )
- {
- double x1,y1,x2,y2;
- x1 = cvmGet(tmpProjPoints[currImage],0,i);
- y1 = cvmGet(tmpProjPoints[currImage],1,i);
- x2 = cvmGet(points[currImage],0,i);
- y2 = cvmGet(points[currImage],1,i);
-
- double dx,dy;
- dx = x1-x2;
- dy = y1-y2;
-
- double newDist = dx*dx+dy*dy;
- if( newDist > dist )
- {
- dist = newDist;
- }
- }
- dist = sqrt(dist);
- goodFlags[i] = (char)(dist > threshold ? 0 : 1);
- finalGoodPoints += goodFlags[i];
- }
-
- char str[200];
- sprintf(str,"Was num = %d\nNew num=%d",maxGoodPoints,finalGoodPoints);
- MessageBox(0,str,"Info",MB_OK|MB_TASKMODAL);
- if( finalGoodPoints > maxGoodPoints )
- {
- /* Copy new version of projection matrices */
- cvCopy(&resProjMatrs[0],&bestProjMatrs[0]);
- cvCopy(&resProjMatrs[1],&bestProjMatrs[1]);
- cvCopy(&resProjMatrs[2],&bestProjMatrs[2]);
- memcpy(bestFlags,goodFlags,numPoints*sizeof(char));
- maxGoodPoints = finalGoodPoints;
- }
-
- cvReleaseMat(&optStatus);
- cvReleaseMat(&resPoints4D);
-#else
-/* Version with using status for Levenberd-Marquardt minimization */
-
- /* Create status */
- CvMat *optStatus;
- optStatus = cvCreateMat(1,numPoints,CV_64F);
- for( i=0;i<numPoints;i++ )
- {
- cvmSet(optStatus,0,i,(double)bestFlags[i]);
- }
-
- CvMat *pointsPres[3];
- pointsPres[0] = optStatus;
- pointsPres[1] = optStatus;
- pointsPres[2] = optStatus;
-
- /* Create 4D points array for good points */
- CvMat *resPoints4D;
- resPoints4D = cvCreateMat(4,numPoints,CV_64F);
-
- CvMat* projMs[3];
-
- projMs[0] = &bestProjMatrs[0];
- projMs[1] = &bestProjMatrs[1];
- projMs[2] = &bestProjMatrs[2];
-
- CvMat resProjMatrs[3];
- double resProjMatrs_dat[36];
- resProjMatrs[0] = cvMat(3,4,CV_64F,resProjMatrs_dat);
- resProjMatrs[1] = cvMat(3,4,CV_64F,resProjMatrs_dat+12);
- resProjMatrs[2] = cvMat(3,4,CV_64F,resProjMatrs_dat+24);
-
- CvMat* resMatrs[3];
- resMatrs[0] = &resProjMatrs[0];
- resMatrs[1] = &resProjMatrs[1];
- resMatrs[2] = &resProjMatrs[2];
-
- cvOptimizeLevenbergMarquardtBundle( projMs,//projMs,
- points,//points2D,
- pointsPres,//pointsPres,
- 3,
- resMatrs,//resProjMatrs,
- resPoints4D,//resPoints4D,
- 100, 1e-9 );
-
- /* We found optimized projection matrices */
-
- reconPoints4D = cvCreateMat(4,numPoints,CV_64F);
-
- /* Reconstruct all points using found projection matrices */
- icvReconstructPointsFor3View( &resProjMatrs[0],&resProjMatrs[1],&resProjMatrs[2],
- points[0], points[1], points[2],
- reconPoints4D);
-
- /* Project points to images using projection matrices */
- icvProject4DPoints(reconPoints4D,&resProjMatrs[0],tmpProjPoints[0]);
- icvProject4DPoints(reconPoints4D,&resProjMatrs[1],tmpProjPoints[1]);
- icvProject4DPoints(reconPoints4D,&resProjMatrs[2],tmpProjPoints[2]);
-
-
- /* Compute error for each point and select good */
-
- int currImage;
- finalGoodPoints = 0;
- for( i = 0; i < numPoints; i++ )
- {
- double dist=-1;
- /* Choose max distance for each of three points */
- for( currImage = 0; currImage < 3; currImage++ )
- {
- double x1,y1,x2,y2;
- x1 = cvmGet(tmpProjPoints[currImage],0,i);
- y1 = cvmGet(tmpProjPoints[currImage],1,i);
- x2 = cvmGet(points[currImage],0,i);
- y2 = cvmGet(points[currImage],1,i);
-
- double dx,dy;
- dx = x1-x2;
- dy = y1-y2;
-
- double newDist = dx*dx+dy*dy;
- if( newDist > dist )
- {
- dist = newDist;
- }
- }
- dist = sqrt(dist);
- goodFlags[i] = (char)(dist > threshold ? 0 : 1);
- finalGoodPoints += goodFlags[i];
- }
-
- /*char str[200];
- sprintf(str,"Was num = %d\nNew num=%d",maxGoodPoints,finalGoodPoints);
- MessageBox(0,str,"Info",MB_OK|MB_TASKMODAL);*/
-
- needRepeat = 0;
- if( finalGoodPoints > maxGoodPoints )
- {
- /* Copy new version of projection matrices */
- cvCopy(&resProjMatrs[0],&bestProjMatrs[0]);
- cvCopy(&resProjMatrs[1],&bestProjMatrs[1]);
- cvCopy(&resProjMatrs[2],&bestProjMatrs[2]);
- memcpy(bestFlags,goodFlags,numPoints*sizeof(char));
- maxGoodPoints = finalGoodPoints;
- needRepeat = 1;
- }
-
- cvReleaseMat(&optStatus);
- cvReleaseMat(&resPoints4D);
-
-
-#endif
- } while ( needRepeat );
-
- cvFree( &goodFlags);
-
-
-
-
- numProjMatrs = 1;
-
- /* Copy projection matrices */
- cvConvert(&bestProjMatrs[0],projMatr1);
- cvConvert(&bestProjMatrs[1],projMatr2);
- cvConvert(&bestProjMatrs[2],projMatr3);
-
- if( status )
- {
- /* copy status for each points if need */
- for( int i = 0; i < numPoints; i++)
- {
- cvmSet(status,0,i,(double)bestFlags[i]);
- }
- }
- }
- }
-
- if( points4D )
- {/* Fill reconstructed points */
-
- cvZero(points4D);
- icvReconstructPointsFor3View( projMatr1,projMatr2,projMatr3,
- points[0], points[1], points[2],
- points4D);
- }
-
-
-
- __END__;
-
- cvFree( &flags);
- cvFree( &bestFlags);
-
- cvReleaseMat(&recPoints4D);
- cvReleaseMat(&tmpProjPoints[0]);
- cvReleaseMat(&tmpProjPoints[1]);
- cvReleaseMat(&tmpProjPoints[2]);
-
- return numProjMatrs;
-}
-
-/*==========================================================================================*/
-
-void icvFindBaseTransform(CvMat* points,CvMat* resultT)
-{
-
- CV_FUNCNAME( "icvFindBaseTransform" );
- __BEGIN__;
-
- if( points == 0 || resultT == 0 )
- {
- CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
- }
-
- if( !CV_IS_MAT(points) || !CV_IS_MAT(resultT) )
- {
- CV_ERROR( CV_StsUnsupportedFormat, "points and resultT must be a matrices" );
- }
-
- if( points->rows != 2 || points->cols != 4 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be 4. And they must have 2 coordinates" );
- }
-
- if( resultT->rows != 3 || resultT->cols != 3 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "size of matrix resultT must be 3x3" );
- }
-
- /* Function gets four points and compute transformation to e1=(100) e2=(010) e3=(001) e4=(111) */
-
- /* !!! test each three points not collinear. Need to test */
-
- /* Create matrices */
- CvMat matrA;
- CvMat vectB;
- double matrA_dat[3*3];
- double vectB_dat[3];
- matrA = cvMat(3,3,CV_64F,matrA_dat);
- vectB = cvMat(3,1,CV_64F,vectB_dat);
-
- /* fill matrices */
- int i;
- for( i = 0; i < 3; i++ )
- {
- cvmSet(&matrA,0,i,cvmGet(points,0,i));
- cvmSet(&matrA,1,i,cvmGet(points,1,i));
- cvmSet(&matrA,2,i,1);
- }
-
- /* Fill vector B */
- cvmSet(&vectB,0,0,cvmGet(points,0,3));
- cvmSet(&vectB,1,0,cvmGet(points,1,3));
- cvmSet(&vectB,2,0,1);
-
- /* result scale */
- CvMat scale;
- double scale_dat[3];
- scale = cvMat(3,1,CV_64F,scale_dat);
-
- cvSolve(&matrA,&vectB,&scale,CV_SVD);
-
- /* multiply by scale */
- int j;
- for( j = 0; j < 3; j++ )
- {
- double sc = scale_dat[j];
- for( i = 0; i < 3; i++ )
- {
- matrA_dat[i*3+j] *= sc;
- }
- }
-
- /* Convert inverse matrix */
- CvMat tmpRes;
- double tmpRes_dat[9];
- tmpRes = cvMat(3,3,CV_64F,tmpRes_dat);
- cvInvert(&matrA,&tmpRes);
-
- cvConvert(&tmpRes,resultT);
-
- __END__;
-
- return;
-}
-
-
-/*==========================================================================================*/
-void GetGeneratorReduceFundSolution(CvMat* points1,CvMat* points2,CvMat* fundReduceCoef1,CvMat* fundReduceCoef2)
-{
-
- CV_FUNCNAME( "GetGeneratorReduceFundSolution" );
- __BEGIN__;
-
- /* Test input data for errors */
-
- if( points1 == 0 || points2 == 0 || fundReduceCoef1 == 0 || fundReduceCoef2 == 0)
- {
- CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
- }
-
- if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2) || !CV_IS_MAT(fundReduceCoef1) || !CV_IS_MAT(fundReduceCoef2) )
- {
- CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
- }
-
-
-
- if( points1->rows != 3 || points1->cols != 3 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of points1 must be 3 and and have 3 coordinates" );
- }
-
- if( points2->rows != 3 || points2->cols != 3 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of points2 must be 3 and and have 3 coordinates" );
- }
-
- if( fundReduceCoef1->rows != 1 || fundReduceCoef1->cols != 5 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of fundReduceCoef1 must be 1x5" );
- }
-
- if( fundReduceCoef2->rows != 1 || fundReduceCoef2->cols != 5 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of fundReduceCoef2 must be 1x5" );
- }
-
- /* Using 3 corr. points compute reduce */
-
- /* Create matrix */
- CvMat matrA;
- double matrA_dat[3*5];
- matrA = cvMat(3,5,CV_64F,matrA_dat);
- int i;
- for( i = 0; i < 3; i++ )
- {
- double x1,y1,w1,x2,y2,w2;
- x1 = cvmGet(points1,0,i);
- y1 = cvmGet(points1,1,i);
- w1 = cvmGet(points1,2,i);
-
- x2 = cvmGet(points2,0,i);
- y2 = cvmGet(points2,1,i);
- w2 = cvmGet(points2,2,i);
-
- cvmSet(&matrA,i,0,y1*x2-y1*w2);
- cvmSet(&matrA,i,1,w1*x2-y1*w2);
- cvmSet(&matrA,i,2,x1*y2-y1*w2);
- cvmSet(&matrA,i,3,w1*y2-y1*w2);
- cvmSet(&matrA,i,4,x1*w2-y1*w2);
- }
-
- /* solve system using svd */
- CvMat matrU;
- CvMat matrW;
- CvMat matrV;
-
- double matrU_dat[3*3];
- double matrW_dat[3*5];
- double matrV_dat[5*5];
-
- matrU = cvMat(3,3,CV_64F,matrU_dat);
- matrW = cvMat(3,5,CV_64F,matrW_dat);
- matrV = cvMat(5,5,CV_64F,matrV_dat);
-
- /* From svd we need just two last vectors of V or two last row V' */
- /* We get transposed matrixes U and V */
-
- cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T);
-
- /* copy results to fundamental matrices */
- for(i=0;i<5;i++)
- {
- cvmSet(fundReduceCoef1,0,i,cvmGet(&matrV,3,i));
- cvmSet(fundReduceCoef2,0,i,cvmGet(&matrV,4,i));
- }
-
- __END__;
- return;
-
-}
-
-/*==========================================================================================*/
-
-int GetGoodReduceFundamMatrFromTwo(CvMat* fundReduceCoef1,CvMat* fundReduceCoef2,CvMat* resFundReduceCoef)
-{
- int numRoots = 0;
-
- CV_FUNCNAME( "GetGoodReduceFundamMatrFromTwo" );
- __BEGIN__;
-
- if( fundReduceCoef1 == 0 || fundReduceCoef2 == 0 || resFundReduceCoef == 0 )
- {
- CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
- }
-
- if( !CV_IS_MAT(fundReduceCoef1) || !CV_IS_MAT(fundReduceCoef2) || !CV_IS_MAT(resFundReduceCoef) )
- {
- CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
- }
-
- /* using two fundamental matrix comute matrixes for det(F)=0 */
- /* May compute 1 or 3 matrices. Returns number of solutions */
- /* Here we will use case F=a*F1+(1-a)*F2 instead of F=m*F1+l*F2 */
-
- /* Test for errors */
- if( fundReduceCoef1->rows != 1 || fundReduceCoef1->cols != 5 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of fundReduceCoef1 must be 1x5" );
- }
-
- if( fundReduceCoef2->rows != 1 || fundReduceCoef2->cols != 5 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of fundReduceCoef2 must be 1x5" );
- }
-
- if( (resFundReduceCoef->rows != 1 && resFundReduceCoef->rows != 3) || resFundReduceCoef->cols != 5 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of resFundReduceCoef must be 1x5" );
- }
-
- double p1,q1,r1,s1,t1;
- double p2,q2,r2,s2,t2;
- p1 = cvmGet(fundReduceCoef1,0,0);
- q1 = cvmGet(fundReduceCoef1,0,1);
- r1 = cvmGet(fundReduceCoef1,0,2);
- s1 = cvmGet(fundReduceCoef1,0,3);
- t1 = cvmGet(fundReduceCoef1,0,4);
-
- p2 = cvmGet(fundReduceCoef2,0,0);
- q2 = cvmGet(fundReduceCoef2,0,1);
- r2 = cvmGet(fundReduceCoef2,0,2);
- s2 = cvmGet(fundReduceCoef2,0,3);
- t2 = cvmGet(fundReduceCoef2,0,4);
-
- /* solve equation */
- CvMat result;
- CvMat coeffs;
- double result_dat[2*3];
- double coeffs_dat[4];
- result = cvMat(2,3,CV_64F,result_dat);
- coeffs = cvMat(1,4,CV_64F,coeffs_dat);
-
- coeffs_dat[0] = ((r1-r2)*(-p1-q1-r1-s1-t1+p2+q2+r2+s2+t2)*(q1-q2)+(p1-p2)*(s1-s2)*(t1-t2));/* *a^3 */
- coeffs_dat[1] = ((r2*(-p1-q1-r1-s1-t1+p2+q2+r2+s2+t2)+(r1-r2)*(-p2-q2-r2-s2-t2))*(q1-q2)+(r1-r2)*(-p1-q1-r1-s1-t1+p2+q2+r2+s2+t2)*q2+(p2*(s1-s2)+(p1-p2)*s2)*(t1-t2)+(p1-p2)*(s1-s2)*t2);/* *a^2 */
- coeffs_dat[2] = (r2*(-p2-q2-r2-s2-t2)*(q1-q2)+(r2*(-p1-q1-r1-s1-t1+p2+q2+r2+s2+t2)+(r1-r2)*(-p2-q2-r2-s2-t2))*q2+p2*s2*(t1-t2)+(p2*(s1-s2)+(p1-p2)*s2)*t2);/* *a */
- coeffs_dat[3] = r2*(-p2-q2-r2-s2-t2)*q2+p2*s2*t2;/* 1 */
-
- int num;
- num = cvSolveCubic(&coeffs,&result);
-
-
- /* test number of solutions and test for real solutions */
- int i;
- for( i = 0; i < num; i++ )
- {
- if( fabs(cvmGet(&result,1,i)) < 1e-8 )
- {
- double alpha = cvmGet(&result,0,i);
- int j;
- for( j = 0; j < 5; j++ )
- {
- cvmSet(resFundReduceCoef,numRoots,j,
- alpha * cvmGet(fundReduceCoef1,0,j) + (1-alpha) * cvmGet(fundReduceCoef2,0,j) );
- }
- numRoots++;
- }
- }
-
- __END__;
- return numRoots;
-}
-
-/*==========================================================================================*/
-
-void GetProjMatrFromReducedFundamental(CvMat* fundReduceCoefs,CvMat* projMatrCoefs)
-{
- CV_FUNCNAME( "GetProjMatrFromReducedFundamental" );
- __BEGIN__;
-
- /* Test for errors */
- if( fundReduceCoefs == 0 || projMatrCoefs == 0 )
- {
- CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
- }
-
- if( !CV_IS_MAT(fundReduceCoefs) || !CV_IS_MAT(projMatrCoefs) )
- {
- CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
- }
-
-
- if( fundReduceCoefs->rows != 1 || fundReduceCoefs->cols != 5 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of fundReduceCoefs must be 1x5" );
- }
-
- if( projMatrCoefs->rows != 1 || projMatrCoefs->cols != 4 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of projMatrCoefs must be 1x4" );
- }
-
- /* Computes project matrix from given reduced matrix */
- /* we have p,q,r,s,t and need get a,b,c,d */
- /* Fill matrix to compute ratio a:b:c as A:B:C */
-
- CvMat matrA;
- double matrA_dat[3*3];
- matrA = cvMat(3,3,CV_64F,matrA_dat);
-
- double p,q,r,s,t;
- p = cvmGet(fundReduceCoefs,0,0);
- q = cvmGet(fundReduceCoefs,0,1);
- r = cvmGet(fundReduceCoefs,0,2);
- s = cvmGet(fundReduceCoefs,0,3);
- t = cvmGet(fundReduceCoefs,0,4);
-
- matrA_dat[0] = p;
- matrA_dat[1] = r;
- matrA_dat[2] = 0;
-
- matrA_dat[3] = q;
- matrA_dat[4] = 0;
- matrA_dat[5] = t;
-
- matrA_dat[6] = 0;
- matrA_dat[7] = s;
- matrA_dat[8] = -(p+q+r+s+t);
-
- CvMat matrU;
- CvMat matrW;
- CvMat matrV;
-
- double matrU_dat[3*3];
- double matrW_dat[3*3];
- double matrV_dat[3*3];
-
- matrU = cvMat(3,3,CV_64F,matrU_dat);
- matrW = cvMat(3,3,CV_64F,matrW_dat);
- matrV = cvMat(3,3,CV_64F,matrV_dat);
-
- /* From svd we need just last vector of V or last row V' */
- /* We get transposed matrixes U and V */
-
- cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T);
-
- double A1,B1,C1;
- A1 = matrV_dat[6];
- B1 = matrV_dat[7];
- C1 = matrV_dat[8];
-
- /* Get second coeffs */
- matrA_dat[0] = 0;
- matrA_dat[1] = r;
- matrA_dat[2] = t;
-
- matrA_dat[3] = p;
- matrA_dat[4] = 0;
- matrA_dat[5] = -(p+q+r+s+t);
-
- matrA_dat[6] = q;
- matrA_dat[7] = s;
- matrA_dat[8] = 0;
-
- cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T);
-
- double A2,B2,C2;
- A2 = matrV_dat[6];
- B2 = matrV_dat[7];
- C2 = matrV_dat[8];
-
- double a,b,c,d;
- {
- CvMat matrK;
- double matrK_dat[36];
- matrK = cvMat(6,6,CV_64F,matrK_dat);
- cvZero(&matrK);
-
- matrK_dat[0] = 1;
- matrK_dat[7] = 1;
- matrK_dat[14] = 1;
-
- matrK_dat[18] = -1;
- matrK_dat[25] = -1;
- matrK_dat[32] = -1;
-
- matrK_dat[21] = 1;
- matrK_dat[27] = 1;
- matrK_dat[33] = 1;
-
- matrK_dat[0*6+4] = -A1;
- matrK_dat[1*6+4] = -B1;
- matrK_dat[2*6+4] = -C1;
-
- matrK_dat[3*6+5] = -A2;
- matrK_dat[4*6+5] = -B2;
- matrK_dat[5*6+5] = -C2;
-
- CvMat matrU;
- CvMat matrW;
- CvMat matrV;
-
- double matrU_dat[36];
- double matrW_dat[36];
- double matrV_dat[36];
-
- matrU = cvMat(6,6,CV_64F,matrU_dat);
- matrW = cvMat(6,6,CV_64F,matrW_dat);
- matrV = cvMat(6,6,CV_64F,matrV_dat);
-
- /* From svd we need just last vector of V or last row V' */
- /* We get transposed matrixes U and V */
-
- cvSVD(&matrK,&matrW,0,&matrV,CV_SVD_V_T);
-
- a = matrV_dat[6*5+0];
- b = matrV_dat[6*5+1];
- c = matrV_dat[6*5+2];
- d = matrV_dat[6*5+3];
- /* we don't need last two coefficients. Because it just a k1,k2 */
-
- cvmSet(projMatrCoefs,0,0,a);
- cvmSet(projMatrCoefs,0,1,b);
- cvmSet(projMatrCoefs,0,2,c);
- cvmSet(projMatrCoefs,0,3,d);
-
- }
-
- __END__;
- return;
-}
-
-/*==========================================================================================*/
-
-void icvComputeProjectMatrix(CvMat* objPoints,CvMat* projPoints,CvMat* projMatr)
-{/* Using SVD method */
-
- /* Reconstruct points using object points and projected points */
- /* Number of points must be >=6 */
-
- CvMat matrV;
- CvMat* matrA = 0;
- CvMat* matrW = 0;
- CvMat* workProjPoints = 0;
- CvMat* tmpProjPoints = 0;
-
- CV_FUNCNAME( "icvComputeProjectMatrix" );
- __BEGIN__;
-
- /* Test for errors */
- if( objPoints == 0 || projPoints == 0 || projMatr == 0)
- {
- CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
- }
-
- if( !CV_IS_MAT(objPoints) || !CV_IS_MAT(projPoints) || !CV_IS_MAT(projMatr) )
- {
- CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
- }
-
- if( projMatr->rows != 3 || projMatr->cols != 4 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of projMatr must be 3x4" );
- }
-
- int numPoints;
- numPoints = projPoints->cols;
- if( numPoints < 6 )
- {
- CV_ERROR( CV_StsOutOfRange, "Number of points must be at least 6" );
- }
-
- if( numPoints != objPoints->cols )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be same" );
- }
-
- if( objPoints->rows != 4 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Object points must have 4 coordinates" );
- }
-
- if( projPoints->rows != 3 && projPoints->rows != 2 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Projected points must have 2 or 3 coordinates" );
- }
-
- /* Create and fill matrix A */
- CV_CALL( matrA = cvCreateMat(numPoints*3, 12, CV_64F) );
- CV_CALL( matrW = cvCreateMat(numPoints*3, 12, CV_64F) );
-
- if( projPoints->rows == 2 )
- {
- CV_CALL( tmpProjPoints = cvCreateMat(3,numPoints,CV_64F) );
- cvMake3DPoints(projPoints,tmpProjPoints);
- workProjPoints = tmpProjPoints;
- }
- else
- {
- workProjPoints = projPoints;
- }
-
- double matrV_dat[144];
- matrV = cvMat(12,12,CV_64F,matrV_dat);
- int i;
-
- char* dat;
- dat = (char*)(matrA->data.db);
-
-#if 1
- FILE *file;
- file = fopen("d:\\test\\recProjMatr.txt","w");
-
-#endif
- for( i = 0;i < numPoints; i++ )
- {
- double x,y,w;
- double X,Y,Z,W;
- double* matrDat = (double*)dat;
-
- x = cvmGet(workProjPoints,0,i);
- y = cvmGet(workProjPoints,1,i);
- w = cvmGet(workProjPoints,2,i);
-
-
- X = cvmGet(objPoints,0,i);
- Y = cvmGet(objPoints,1,i);
- Z = cvmGet(objPoints,2,i);
- W = cvmGet(objPoints,3,i);
-
-#if 1
- fprintf(file,"%d (%lf %lf %lf %lf) - (%lf %lf %lf)\n",i,X,Y,Z,W,x,y,w );
-#endif
-
-/*---*/
- matrDat[ 0] = 0;
- matrDat[ 1] = 0;
- matrDat[ 2] = 0;
- matrDat[ 3] = 0;
-
- matrDat[ 4] = -w*X;
- matrDat[ 5] = -w*Y;
- matrDat[ 6] = -w*Z;
- matrDat[ 7] = -w*W;
-
- matrDat[ 8] = y*X;
- matrDat[ 9] = y*Y;
- matrDat[10] = y*Z;
- matrDat[11] = y*W;
-/*---*/
- matrDat[12] = w*X;
- matrDat[13] = w*Y;
- matrDat[14] = w*Z;
- matrDat[15] = w*W;
-
- matrDat[16] = 0;
- matrDat[17] = 0;
- matrDat[18] = 0;
- matrDat[19] = 0;
-
- matrDat[20] = -x*X;
- matrDat[21] = -x*Y;
- matrDat[22] = -x*Z;
- matrDat[23] = -x*W;
-/*---*/
- matrDat[24] = -y*X;
- matrDat[25] = -y*Y;
- matrDat[26] = -y*Z;
- matrDat[27] = -y*W;
-
- matrDat[28] = x*X;
- matrDat[29] = x*Y;
- matrDat[30] = x*Z;
- matrDat[31] = x*W;
-
- matrDat[32] = 0;
- matrDat[33] = 0;
- matrDat[34] = 0;
- matrDat[35] = 0;
-/*---*/
- dat += (matrA->step)*3;
- }
-#if 1
- fclose(file);
-
-#endif
-
- /* Solve this system */
-
- /* From svd we need just last vector of V or last row V' */
- /* We get transposed matrix V */
-
- cvSVD(matrA,matrW,0,&matrV,CV_SVD_V_T);
-
- /* projected matrix was computed */
- for( i = 0; i < 12; i++ )
- {
- cvmSet(projMatr,i/4,i%4,cvmGet(&matrV,11,i));
- }
-
- cvReleaseMat(&matrA);
- cvReleaseMat(&matrW);
- cvReleaseMat(&tmpProjPoints);
- __END__;
-}
-
-
-/*==========================================================================================*/
-/* May be useless function */
-void icvComputeTransform4D(CvMat* points1,CvMat* points2,CvMat* transMatr)
-{
- CvMat* matrA = 0;
- CvMat* matrW = 0;
-
- double matrV_dat[256];
- CvMat matrV = cvMat(16,16,CV_64F,matrV_dat);
-
- CV_FUNCNAME( "icvComputeTransform4D" );
- __BEGIN__;
-
- if( points1 == 0 || points2 == 0 || transMatr == 0)
- {
- CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
- }
-
- if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2) || !CV_IS_MAT(transMatr) )
- {
- CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
- }
-
- /* Computes transformation matrix (4x4) for points1 -> points2 */
- /* p2=H*p1 */
-
- /* Test for errors */
- int numPoints;
- numPoints = points1->cols;
-
- /* we must have at least 5 points */
- if( numPoints < 5 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be at least 5" );
- }
-
- if( numPoints != points2->cols )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be the same" );
- }
-
- if( transMatr->rows != 4 || transMatr->cols != 4 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of transMatr must be 4x4" );
- }
-
- if( points1->rows != 4 || points2->rows != 4 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of points must be 4" );
- }
-
- /* Create matrix */
- CV_CALL( matrA = cvCreateMat(6*numPoints,16,CV_64F) );
- CV_CALL( matrW = cvCreateMat(6*numPoints,16,CV_64F) );
-
- cvZero(matrA);
-
- /* Fill matrices */
- int i;
- for( i = 0; i < numPoints; i++ )/* For each point */
- {
- double X1,Y1,Z1,W1;
- double P[4];
-
- P[0] = cvmGet(points1,0,i);
- P[1] = cvmGet(points1,1,i);
- P[2] = cvmGet(points1,2,i);
- P[3] = cvmGet(points1,3,i);
-
- X1 = cvmGet(points2,0,i);
- Y1 = cvmGet(points2,1,i);
- Z1 = cvmGet(points2,2,i);
- W1 = cvmGet(points2,3,i);
-
- /* Fill matrA */
- for( int j = 0; j < 4; j++ )/* For each coordinate */
- {
- double x,y,z,w;
-
- x = X1*P[j];
- y = Y1*P[j];
- z = Z1*P[j];
- w = W1*P[j];
-
- cvmSet(matrA,6*i+0,4*0+j,y);
- cvmSet(matrA,6*i+0,4*1+j,-x);
-
- cvmSet(matrA,6*i+1,4*0+j,z);
- cvmSet(matrA,6*i+1,4*2+j,-x);
-
- cvmSet(matrA,6*i+2,4*0+j,w);
- cvmSet(matrA,6*i+2,4*3+j,-x);
-
- cvmSet(matrA,6*i+3,4*1+j,-z);
- cvmSet(matrA,6*i+3,4*2+j,y);
-
- cvmSet(matrA,6*i+4,4*1+j,-w);
- cvmSet(matrA,6*i+4,4*3+j,y);
-
- cvmSet(matrA,6*i+5,4*2+j,-w);
- cvmSet(matrA,6*i+5,4*3+j,z);
- }
- }
-
- /* From svd we need just two last vectors of V or two last row V' */
- /* We get transposed matrixes U and V */
-
- cvSVD(matrA,matrW,0,&matrV,CV_SVD_V_T);
-
- /* Copy result to result matrix */
- for( i = 0; i < 16; i++ )
- {
- cvmSet(transMatr,i/4,i%4,cvmGet(&matrV,15,i));
- }
-
- cvReleaseMat(&matrA);
- cvReleaseMat(&matrW);
-
- __END__;
- return;
-}
-
-/*==========================================================================================*/
-
-void icvReconstructPointsFor3View( CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3,
- CvMat* projPoints1,CvMat* projPoints2,CvMat* projPoints3,
- CvMat* points4D)
-{
- CV_FUNCNAME( "icvReconstructPointsFor3View" );
- __BEGIN__;
-
- if( projMatr1 == 0 || projMatr2 == 0 || projMatr3 == 0 ||
- projPoints1 == 0 || projPoints2 == 0 || projPoints3 == 0 ||
- points4D == 0)
- {
- CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
- }
-
- if( !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) || !CV_IS_MAT(projMatr3) ||
- !CV_IS_MAT(projPoints1) || !CV_IS_MAT(projPoints2) || !CV_IS_MAT(projPoints3) ||
- !CV_IS_MAT(points4D) )
- {
- CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
- }
-
- int numPoints;
- numPoints = projPoints1->cols;
-
- if( numPoints < 1 )
- {
- CV_ERROR( CV_StsOutOfRange, "Number of points must be more than zero" );
- }
-
- if( projPoints2->cols != numPoints || projPoints3->cols != numPoints || points4D->cols != numPoints )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be the same" );
- }
-
- if( projPoints1->rows != 2 || projPoints2->rows != 2 || projPoints3->rows != 2)
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of proj points coordinates must be == 2" );
- }
-
- if( points4D->rows != 4 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of world points coordinates must be == 4" );
- }
-
- if( projMatr1->cols != 4 || projMatr1->rows != 3 ||
- projMatr2->cols != 4 || projMatr2->rows != 3 ||
- projMatr3->cols != 4 || projMatr3->rows != 3)
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of projection matrices must be 3x4" );
- }
-
- CvMat matrA;
- double matrA_dat[36];
- matrA = cvMat(9,4,CV_64F,matrA_dat);
-
- //CvMat matrU;
- CvMat matrW;
- CvMat matrV;
- //double matrU_dat[9*9];
- double matrW_dat[9*4];
- double matrV_dat[4*4];
-
- //matrU = cvMat(9,9,CV_64F,matrU_dat);
- matrW = cvMat(9,4,CV_64F,matrW_dat);
- matrV = cvMat(4,4,CV_64F,matrV_dat);
-
- CvMat* projPoints[3];
- CvMat* projMatrs[3];
-
- projPoints[0] = projPoints1;
- projPoints[1] = projPoints2;
- projPoints[2] = projPoints3;
-
- projMatrs[0] = projMatr1;
- projMatrs[1] = projMatr2;
- projMatrs[2] = projMatr3;
-
- /* Solve system for each point */
- int i,j;
- for( i = 0; i < numPoints; i++ )/* For each point */
- {
- /* Fill matrix for current point */
- for( j = 0; j < 3; j++ )/* For each view */
- {
- double x,y;
- x = cvmGet(projPoints[j],0,i);
- y = cvmGet(projPoints[j],1,i);
- for( int k = 0; k < 4; k++ )
- {
- cvmSet(&matrA, j*3+0, k, x * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],0,k) );
- cvmSet(&matrA, j*3+1, k, y * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],1,k) );
- cvmSet(&matrA, j*3+2, k, x * cvmGet(projMatrs[j],1,k) - y * cvmGet(projMatrs[j],0,k) );
- }
- }
- /* Solve system for current point */
- {
- cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T);
-
- /* Copy computed point */
- cvmSet(points4D,0,i,cvmGet(&matrV,3,0));/* X */
- cvmSet(points4D,1,i,cvmGet(&matrV,3,1));/* Y */
- cvmSet(points4D,2,i,cvmGet(&matrV,3,2));/* Z */
- cvmSet(points4D,3,i,cvmGet(&matrV,3,3));/* W */
- }
- }
-
- /* Points was reconstructed. Try to reproject points */
- /* We can compute reprojection error if need */
- {
- int i;
- CvMat point3D;
- double point3D_dat[4];
- point3D = cvMat(4,1,CV_64F,point3D_dat);
-
- CvMat point2D;
- double point2D_dat[3];
- point2D = cvMat(3,1,CV_64F,point2D_dat);
-
- for( i = 0; i < numPoints; i++ )
- {
- double W = cvmGet(points4D,3,i);
-
- point3D_dat[0] = cvmGet(points4D,0,i)/W;
- point3D_dat[1] = cvmGet(points4D,1,i)/W;
- point3D_dat[2] = cvmGet(points4D,2,i)/W;
- point3D_dat[3] = 1;
-
- /* !!! Project this point for each camera */
- for( int currCamera = 0; currCamera < 3; currCamera++ )
- {
- cvmMul(projMatrs[currCamera], &point3D, &point2D);
-
- float x,y;
- float xr,yr,wr;
- x = (float)cvmGet(projPoints[currCamera],0,i);
- y = (float)cvmGet(projPoints[currCamera],1,i);
-
- wr = (float)point2D_dat[2];
- xr = (float)(point2D_dat[0]/wr);
- yr = (float)(point2D_dat[1]/wr);
-
- float deltaX,deltaY;
- deltaX = (float)fabs(x-xr);
- deltaY = (float)fabs(y-yr);
- }
- }
- }
-
- __END__;
- return;
-}
-
-
-
-
-#if 0
-void ReconstructPointsFor3View_bySolve( CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3,
- CvMat* projPoints1,CvMat* projPoints2,CvMat* projPoints3,
- CvMat* points3D)
-{
- CV_FUNCNAME( "ReconstructPointsFor3View" );
- __BEGIN__;
-
-
- int numPoints;
- numPoints = projPoints1->cols;
- if( projPoints2->cols != numPoints || projPoints3->cols != numPoints || points3D->cols != numPoints )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of points must be the same" );
- }
-
- if( projPoints1->rows != 2 || projPoints2->rows != 2 || projPoints3->rows != 2)
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of proj points coordinates must be == 2" );
- }
-
- if( points3D->rows != 4 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of world points coordinates must be == 4" );
- }
-
- if( projMatr1->cols != 4 || projMatr1->rows != 3 ||
- projMatr2->cols != 4 || projMatr2->rows != 3 ||
- projMatr3->cols != 4 || projMatr3->rows != 3)
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of proj matrix must be 3x4" );
- }
-
- CvMat matrA;
- double matrA_dat[3*3*3];
- matrA = cvMat(3*3,3,CV_64F,matrA_dat);
-
- CvMat vectB;
- double vectB_dat[9];
- vectB = cvMat(9,1,CV_64F,vectB_dat);
-
- CvMat result;
- double result_dat[3];
- result = cvMat(3,1,CV_64F,result_dat);
-
- CvMat* projPoints[3];
- CvMat* projMatrs[3];
-
- projPoints[0] = projPoints1;
- projPoints[1] = projPoints2;
- projPoints[2] = projPoints3;
-
- projMatrs[0] = projMatr1;
- projMatrs[1] = projMatr2;
- projMatrs[2] = projMatr3;
-
- /* Solve system for each point */
- int i,j;
- for( i = 0; i < numPoints; i++ )/* For each point */
- {
- /* Fill matrix for current point */
- for( j = 0; j < 3; j++ )/* For each view */
- {
- double x,y;
- x = cvmGet(projPoints[j],0,i);
- y = cvmGet(projPoints[j],1,i);
-
- cvmSet(&vectB,j*3+0,0,x-cvmGet(projMatrs[j],0,3));
- cvmSet(&vectB,j*3+1,0,y-cvmGet(projMatrs[j],1,3));
- cvmSet(&vectB,j*3+2,0,1-cvmGet(projMatrs[j],2,3));
-
- for( int t = 0; t < 3; t++ )
- {
- for( int k = 0; k < 3; k++ )
- {
- cvmSet(&matrA, j*3+t, k, cvmGet(projMatrs[j],t,k) );
- }
- }
- }
-
-
- /* Solve system for current point */
- cvSolve(&matrA,&vectB,&result,CV_SVD);
-
- cvmSet(points3D,0,i,result_dat[0]);/* X */
- cvmSet(points3D,1,i,result_dat[1]);/* Y */
- cvmSet(points3D,2,i,result_dat[2]);/* Z */
- cvmSet(points3D,3,i,1);/* W */
-
- }
-
- /* Points was reconstructed. Try to reproject points */
- {
- int i;
- CvMat point3D;
- double point3D_dat[4];
- point3D = cvMat(4,1,CV_64F,point3D_dat);
-
- CvMat point2D;
- double point2D_dat[3];
- point2D = cvMat(3,1,CV_64F,point2D_dat);
-
- for( i = 0; i < numPoints; i++ )
- {
- double W = cvmGet(points3D,3,i);
-
- point3D_dat[0] = cvmGet(points3D,0,i)/W;
- point3D_dat[1] = cvmGet(points3D,1,i)/W;
- point3D_dat[2] = cvmGet(points3D,2,i)/W;
- point3D_dat[3] = 1;
-
- /* Project this point for each camera */
- for( int currCamera = 0; currCamera < 3; currCamera++ )
- {
- cvmMul(projMatrs[currCamera], &point3D, &point2D);
- float x,y;
- float xr,yr,wr;
- x = (float)cvmGet(projPoints[currCamera],0,i);
- y = (float)cvmGet(projPoints[currCamera],1,i);
-
- wr = (float)point2D_dat[2];
- xr = (float)(point2D_dat[0]/wr);
- yr = (float)(point2D_dat[1]/wr);
-
- }
- }
- }
-
- __END__;
- return;
-}
-#endif
-
-/*==========================================================================================*/
-
-void icvComputeCameraExrinnsicByPosition(CvMat* camPos, CvMat* rotMatr, CvMat* transVect)
-{
- /* We know position of camera. we must to compute rotate matrix and translate vector */
-
- CV_FUNCNAME( "icvComputeCameraExrinnsicByPosition" );
- __BEGIN__;
-
- /* Test input paramaters */
- if( camPos == 0 || rotMatr == 0 || transVect == 0 )
- {
- CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
- }
-
- if( !CV_IS_MAT(camPos) || !CV_IS_MAT(rotMatr) || !CV_IS_MAT(transVect) )
- {
- CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
- }
-
- if( camPos->cols != 1 || camPos->rows != 3 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of camera position must be 3x1 vector" );
- }
-
- if( rotMatr->cols != 3 || rotMatr->rows != 3 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Rotate matrix must be 3x3" );
- }
-
- if( transVect->cols != 1 || transVect->rows != 3 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Translate vector must be 3x1" );
- }
-
- double x,y,z;
- x = cvmGet(camPos,0,0);
- y = cvmGet(camPos,1,0);
- z = cvmGet(camPos,2,0);
-
- /* Set translate vector. It same as camea position */
- cvmSet(transVect,0,0,x);
- cvmSet(transVect,1,0,y);
- cvmSet(transVect,2,0,z);
-
- /* Compute rotate matrix. Compute each unit transformed vector */
-
- /* normalize flat direction x,y */
- double vectorX[3];
- double vectorY[3];
- double vectorZ[3];
-
- vectorX[0] = -z;
- vectorX[1] = 0;
- vectorX[2] = x;
-
- vectorY[0] = x*y;
- vectorY[1] = x*x+z*z;
- vectorY[2] = z*y;
-
- vectorZ[0] = -x;
- vectorZ[1] = -y;
- vectorZ[2] = -z;
-
- /* normaize vectors */
- double norm;
- int i;
-
- /* Norm X */
- norm = 0;
- for( i = 0; i < 3; i++ )
- norm += vectorX[i]*vectorX[i];
- norm = sqrt(norm);
- for( i = 0; i < 3; i++ )
- vectorX[i] /= norm;
-
- /* Norm Y */
- norm = 0;
- for( i = 0; i < 3; i++ )
- norm += vectorY[i]*vectorY[i];
- norm = sqrt(norm);
- for( i = 0; i < 3; i++ )
- vectorY[i] /= norm;
-
- /* Norm Z */
- norm = 0;
- for( i = 0; i < 3; i++ )
- norm += vectorZ[i]*vectorZ[i];
- norm = sqrt(norm);
- for( i = 0; i < 3; i++ )
- vectorZ[i] /= norm;
-
- /* Set output results */
-
- for( i = 0; i < 3; i++ )
- {
- cvmSet(rotMatr,i,0,vectorX[i]);
- cvmSet(rotMatr,i,1,vectorY[i]);
- cvmSet(rotMatr,i,2,vectorZ[i]);
- }
-
- {/* Try to inverse rotate matrix */
- CvMat tmpInvRot;
- double tmpInvRot_dat[9];
- tmpInvRot = cvMat(3,3,CV_64F,tmpInvRot_dat);
- cvInvert(rotMatr,&tmpInvRot,CV_SVD);
- cvConvert(&tmpInvRot,rotMatr);
-
-
-
- }
-
- __END__;
-
- return;
-}
-
-/*==========================================================================================*/
-
-void FindTransformForProjectMatrices(CvMat* projMatr1,CvMat* projMatr2,CvMat* rotMatr,CvMat* transVect)
-{
- /* Computes homography for project matrix be "canonical" form */
- CV_FUNCNAME( "computeProjMatrHomography" );
- __BEGIN__;
-
- /* Test input paramaters */
- if( projMatr1 == 0 || projMatr2 == 0 || rotMatr == 0 || transVect == 0 )
- {
- CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
- }
-
- if( !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) || !CV_IS_MAT(rotMatr) || !CV_IS_MAT(transVect) )
- {
- CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
- }
-
- if( projMatr1->cols != 4 || projMatr1->rows != 3 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of project matrix 1 must be 3x4" );
- }
-
- if( projMatr2->cols != 4 || projMatr2->rows != 3 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of project matrix 2 must be 3x4" );
- }
-
- if( rotMatr->cols != 3 || rotMatr->rows != 3 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of rotation matrix must be 3x3" );
- }
-
- if( transVect->cols != 1 || transVect->rows != 3 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of translation vector must be 3x1" );
- }
-
- CvMat matrA;
- double matrA_dat[12*12];
- matrA = cvMat(12,12,CV_64F,matrA_dat);
- CvMat vectB;
- double vectB_dat[12];
- vectB = cvMat(12,1,CV_64F,vectB_dat);
-
- cvZero(&matrA);
- cvZero(&vectB);
- int i,j;
- for( i = 0; i < 12; i++ )
- {
- for( j = 0; j < 12; j++ )
- {
- cvmSet(&matrA,i,j,cvmGet(projMatr1,i/4,j%4));
- }
- /* Fill vector B */
-
- double val = cvmGet(projMatr2,i/4,i%4);
- if( (i+1)%4 == 0 )
- {
- val -= cvmGet(projMatr1,i/4,3);
-
- }
- cvmSet(&vectB,i,0,val);
- }
-
- /* Solve system */
- CvMat resVect;
- double resVect_dat[12];
- resVect = cvMat(12,1,CV_64F,resVect_dat);
-
- int sing;
- sing = cvSolve(&matrA,&vectB,&resVect);
-
- /* Fill rotation matrix */
- for( i = 0; i < 12; i++ )
- {
- double val = cvmGet(&resVect,i,0);
- if( i < 9 )
- cvmSet(rotMatr,i%3,i/3,val);
- else
- cvmSet(transVect,i-9,0,val);
- }
-
- __END__;
-
- return;
-}
-
-/*==========================================================================================*/
-#if 0
-void icvComputeQknowPrincipalPoint(int numImages, CvMat **projMatrs,CvMat *matrQ, double cx,double cy)
-{
- /* Computes matrix Q */
- /* focal x and y eqauls () */
- /* we know principal point for camera */
- /* focal may differ from image to image */
- /* image skew is 0 */
-
- if( numImages < 10 )
- {
- return;
- //Error. Number of images too few
- }
-
- /* Create */
-
-
- return;
-}
-#endif
-
-/*==========================================================================================*/
-
-/*==========================================================================================*/
-/*==========================================================================================*/
-/*==========================================================================================*/
-/*==========================================================================================*/
-/* Part with metric reconstruction */
-
-#if 1
-void icvComputeQ(int numMatr, CvMat** projMatr, CvMat** cameraMatr, CvMat* matrQ)
-{
- /* K*K' = P*Q*P' */
- /* try to solve Q by linear method */
-
- CvMat* matrA = 0;
- CvMat* vectB = 0;
-
- CV_FUNCNAME( "ComputeQ" );
- __BEGIN__;
-
- /* Define number of projection matrices */
- if( numMatr < 2 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Number of projection matrices must be at least 2" );
- }
-
-
- /* test matrices sizes */
- if( matrQ->cols != 4 || matrQ->rows != 4 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of matrix Q must be 3x3" );
- }
-
- int currMatr;
- for( currMatr = 0; currMatr < numMatr; currMatr++ )
- {
-
- if( cameraMatr[currMatr]->cols != 3 || cameraMatr[currMatr]->rows != 3 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of each camera matrix must be 3x3" );
- }
-
- if( projMatr[currMatr]->cols != 4 || projMatr[currMatr]->rows != 3 )
- {
- CV_ERROR( CV_StsUnmatchedSizes, "Size of each camera matrix must be 3x3" );
- }
- }
-
- CvMat matrw;
- double matrw_dat[9];
- matrw = cvMat(3,3,CV_64F,matrw_dat);
-
- CvMat matrKt;
- double matrKt_dat[9];
- matrKt = cvMat(3,3,CV_64F,matrKt_dat);
-
-
- /* Create matrix A and vector B */
- CV_CALL( matrA = cvCreateMat(9*numMatr,10,CV_64F) );
- CV_CALL( vectB = cvCreateMat(9*numMatr,1,CV_64F) );
-
- double dataQ[16];
-
- for( currMatr = 0; currMatr < numMatr; currMatr++ )
- {
- int ord10[10] = {0,1,2,3,5,6,7,10,11,15};
- /* Fill atrix A by data from matrices */
-
- /* Compute matrix w for current camera matrix */
- cvTranspose(cameraMatr[currMatr],&matrKt);
- cvmMul(cameraMatr[currMatr],&matrKt,&matrw);
-
- /* Fill matrix A and vector B */
-
- int currWi,currWj;
- int currMatr;
- for( currMatr = 0; currMatr < numMatr; currMatr++ )
- {
- for( currWi = 0; currWi < 3; currWi++ )
- {
- for( currWj = 0; currWj < 3; currWj++ )
- {
- int i,j;
- for( i = 0; i < 4; i++ )
- {
- for( j = 0; j < 4; j++ )
- {
- /* get elements from current projection matrix */
- dataQ[i*4+j] = cvmGet(projMatr[currMatr],currWi,j) *
- cvmGet(projMatr[currMatr],currWj,i);
- }
- }
-
- /* we know 16 elements in dataQ move them to matrQ 10 */
- dataQ[1] += dataQ[4];
- dataQ[2] += dataQ[8];
- dataQ[3] += dataQ[12];
- dataQ[6] += dataQ[9];
- dataQ[7] += dataQ[13];
- dataQ[11] += dataQ[14];
- /* Now first 10 elements has coeffs */
-
- /* copy to matrix A */
- for( i = 0; i < 10; i++ )
- {
- cvmSet(matrA,currMatr*9 + currWi*3+currWj,i,dataQ[ord10[i]]);
- }
- }
- }
-
- /* Fill vector B */
- for( int i = 0; i < 9; i++ )
- {
- cvmSet(vectB,currMatr*9+i,0,matrw_dat[i]);
- }
- }
- }
-
- /* Matrix A and vector B filled and we can solve system */
-
- /* Solve system */
- CvMat resQ;
- double resQ_dat[10];
- resQ = cvMat(10,1,CV_64F,resQ_dat);
-
- cvSolve(matrA,vectB,&resQ,CV_SVD);
-
- /* System was solved. We know matrix Q. But we must have condition det Q=0 */
- /* Just copy result matrix Q */
- {
- int curr = 0;
- int ord16[16] = {0,1,2,3,1,4,5,6,2,5,7,8,3,6,8,9};
-
- for( int i = 0; i < 4; i++ )
- {
- for( int j = 0; j < 4; j++ )
- {
- cvmSet(matrQ,i,j,resQ_dat[ord16[curr++]]);
- }
- }
- }
-
-
- __END__;
-
- /* Free allocated memory */
- cvReleaseMat(&matrA);
- cvReleaseMat(&vectB);
-
- return;
-}
-#endif
-/*-----------------------------------------------------------------------------------------------------*/
-
-void icvDecomposeQ(CvMat* /*matrQ*/,CvMat* /*matrH*/)
-{
-#if 0
- /* Use SVD to decompose matrix Q=H*I*H' */
- /* test input data */
-
- CvMat matrW;
- CvMat matrU;
-// CvMat matrV;
- double matrW_dat[16];
- double matrU_dat[16];
-// double matrV_dat[16];
-
- matrW = cvMat(4,4,CV_64F,matrW_dat);
- matrU = cvMat(4,4,CV_64F,matrU_dat);
-// matrV = cvMat(4,4,CV_64F,matrV_dat);
-
- cvSVD(matrQ,&matrW,&matrU,0);
-
- double eig[3];
- eig[0] = fsqrt(cvmGet(&matrW,0,0));
- eig[1] = fsqrt(cvmGet(&matrW,1,1));
- eig[2] = fsqrt(cvmGet(&matrW,2,2));
-
- CvMat matrIS;
- double matrIS_dat[16];
- matrIS =
-
-
-
-
-/* det for matrix Q with q1-q10 */
-/*
-+ q1*q5*q8*q10
-- q1*q5*q9*q9
-- q1*q6*q6*q10
-+ 2*q1*q6*q7*q9
-- q1*q7*q7*q8
-- q2*q2*q8*q10
-+ q2*q2*q9*q9
-+ 2*q2*q6*q3*q10
-- 2*q2*q6*q4*q9
-- 2*q2*q7*q3*q9
-+ 2*q2*q7*q4*q8
-- q5*q3*q3*q10
-+ 2*q3*q5*q4*q9
-+ q3*q3*q7*q7
-- 2*q3*q7*q4*q6
-- q5*q4*q4*q8
-+ q4*q4*q6*q6
-*/
-
-// (1-a)^4 = 1 - 4 * a + 6 * a * a - 4 * a * a * a + a * a * a * a;
-
-
-#endif
-}
-
projects / opencv / blobdiff