1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
51 typedef void (*pointer_LMJac)( const CvMat* src, CvMat* dst );
52 typedef void (*pointer_LMFunc)( const CvMat* src, CvMat* dst );
54 void cvLevenbergMarquardtOptimization(pointer_LMJac JacobianFunction,
55 pointer_LMFunc function,
56 /*pointer_Err error_function,*/
57 CvMat *X0,CvMat *observRes,CvMat *resultX,
58 int maxIter,double epsilon);
60 void icvReconstructPointsFor3View( CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3,
61 CvMat* projPoints1,CvMat* projPoints2,CvMat* projPoints3,
65 /* Jacobian computation for trifocal case */
66 void icvJacobianFunction_ProjTrifocal(const CvMat *vectX,CvMat *Jacobian)
68 CV_FUNCNAME( "icvJacobianFunction_ProjTrifocal" );
71 /* Test data for errors */
72 if( vectX == 0 || Jacobian == 0 )
74 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
77 if( !CV_IS_MAT(vectX) || !CV_IS_MAT(Jacobian) )
79 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
83 numPoints = (vectX->rows - 36)/4;
85 if( numPoints < 1 )//!!! Need to correct this minimal number of points
87 CV_ERROR( CV_StsUnmatchedSizes, "number of points must be more than 0" );
90 if( Jacobian->rows == numPoints*6 || Jacobian->cols != 36+numPoints*4 )
92 CV_ERROR( CV_StsUnmatchedSizes, "Size of Jacobian is not correct it must be 6*numPoints x (36+numPoints*4)" );
95 /* Computed Jacobian in a given point */
96 /* This is for function with 3 projection matrices */
97 /* vector X consists of projection matrices and points3D */
98 /* each 3D points has X,Y,Z,W */
99 /* each projection matrices has 3x4 coeffs */
100 /* For N points 4D we have Jacobian 2N x (12*3+4N) */
102 /* Will store derivates as */
103 /* Fill Jacobian matrix */
108 for( currMatr = 0; currMatr < 3; currMatr++ )
111 for( int i=0;i<12;i++ )
113 p[i] = cvmGet(vectX,currMatr*12+i,0);
117 for( currProjPoint = 0; currProjPoint < numPoints; currProjPoint++ )
121 X[0] = cvmGet(vectX,currVal++,0);
122 X[1] = cvmGet(vectX,currVal++,0);
123 X[2] = cvmGet(vectX,currVal++,0);
124 X[3] = cvmGet(vectX,currVal++,0);
127 piX[0] = X[0]*p[0] + X[1]*p[1] + X[2]*p[2] + X[3]*p[3];
128 piX[1] = X[0]*p[4] + X[1]*p[5] + X[2]*p[6] + X[3]*p[7];
129 piX[2] = X[0]*p[8] + X[1]*p[9] + X[2]*p[10] + X[3]*p[11];
132 /* fill derivate by point */
134 double tmp3 = 1/(piX[2]*piX[2]);
136 double tmp1 = -piX[0]*tmp3;
137 double tmp2 = -piX[1]*tmp3;
138 for( j = 0; j < 2; j++ )//for x and y
140 for( i = 0; i < 4; i++ )// for X,Y,Z,W
143 currMatr*numPoints*2+currProjPoint*2+j, 36+currProjPoint*4+i,
144 (p[j*4+i]*piX[2]-p[8+i]*piX[j]) * tmp3 );
147 /* fill derivate by projection matrix */
148 for( i = 0; i < 4; i++ )
151 cvmSet(Jacobian,currMatr*numPoints*2+currProjPoint*2,currMatr*12+i,X[i]/piX[2]);//x' p1i
152 cvmSet(Jacobian,currMatr*numPoints*2+currProjPoint*2,currMatr*12+8+i,X[i]*tmp1);//x' p3i
155 cvmSet(Jacobian,currMatr*numPoints*2+currProjPoint*2+1,currMatr*12+4+i,X[i]/piX[2]);//y' p2i
156 cvmSet(Jacobian,currMatr*numPoints*2+currProjPoint*2+1,currMatr*12+8+i,X[i]*tmp2);//y' p3i
166 void icvFunc_ProjTrifocal(const CvMat *vectX, CvMat *resFunc)
168 /* Computes function in a given point */
169 /* Computers project points using 3 projection matrices and points 3D */
171 /* vector X consists of projection matrices and points3D */
172 /* each projection matrices has 3x4 coeffs */
173 /* each 3D points has X,Y,Z,W(?) */
175 /* result of function is projection of N 3D points using 3 projection matrices */
176 /* projected points store as (projection by matrix P1),(projection by matrix P2),(projection by matrix P3) */
177 /* each projection is x1,y1,x2,y2,x3,y3,x4,y4 */
179 /* Compute projection of points */
181 /* Fill projection matrices */
183 CV_FUNCNAME( "icvFunc_ProjTrifocal" );
186 /* Test data for errors */
187 if( vectX == 0 || resFunc == 0 )
189 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
192 if( !CV_IS_MAT(vectX) || !CV_IS_MAT(resFunc) )
194 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
198 numPoints = (vectX->rows - 36)/4;
200 if( numPoints < 1 )//!!! Need to correct this minimal number of points
202 CV_ERROR( CV_StsUnmatchedSizes, "number of points must be more than 0" );
205 if( resFunc->rows == 2*numPoints*3 || resFunc->cols != 1 )
207 CV_ERROR( CV_StsUnmatchedSizes, "Size of resFunc is not correct it must be 2*numPoints*3 x 1");
212 double projMatrs_dat[36];
213 projMatrs[0] = cvMat(3,4,CV_64F,projMatrs_dat);
214 projMatrs[1] = cvMat(3,4,CV_64F,projMatrs_dat+12);
215 projMatrs[2] = cvMat(3,4,CV_64F,projMatrs_dat+24);
218 double point3D_dat[3];
219 point3D = cvMat(3,1,CV_64F,point3D_dat);
226 for( currMatr = 0; currMatr < 3; currMatr++ )
228 for( i = 0; i < 3; i++ )
230 for( j = 0;j < 4; j++ )
232 double val = cvmGet(vectX,currV,0);
233 cvmSet(&projMatrs[currMatr],i,j,val);
242 double point4D_dat[4];
243 point4D = cvMat(4,1,CV_64F,point4D_dat);
244 for( currPoint = 0; currPoint < numPoints; currPoint++ )
247 point4D_dat[0] = cvmGet(vectX,currV++,0);
248 point4D_dat[1] = cvmGet(vectX,currV++,0);
249 point4D_dat[2] = cvmGet(vectX,currV++,0);
250 point4D_dat[3] = cvmGet(vectX,currV++,0);
252 for( currMatr = 0; currMatr < 3; currMatr++ )
254 /* Compute projection for current point */
255 cvmMul(&projMatrs[currMatr],&point4D,&point3D);
256 double z = point3D_dat[2];
257 cvmSet(resFunc,currMatr*numPoints*2 + currPoint*2, 0,point3D_dat[0]/z);
258 cvmSet(resFunc,currMatr*numPoints*2 + currPoint*2+1,0,point3D_dat[1]/z);
267 /*----------------------------------------------------------------------------------------*/
269 void icvOptimizeProjectionTrifocal(CvMat **projMatrs,CvMat **projPoints,
270 CvMat **resultProjMatrs, CvMat *resultPoints4D)
276 CvMat *observRes = 0;
279 CV_FUNCNAME( "icvOptimizeProjectionTrifocal" );
282 /* Test data for errors */
283 if( projMatrs == 0 || projPoints == 0 || resultProjMatrs == 0 || resultPoints4D == 0)
285 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
288 if( !CV_IS_MAT(resultPoints4D) )
290 CV_ERROR( CV_StsUnsupportedFormat, "resultPoints4D must be a matrix" );
294 numPoints = resultPoints4D->cols;
297 CV_ERROR( CV_StsOutOfRange, "Number points of resultPoints4D must be more than 0" );
300 if( resultPoints4D->rows != 4 )
302 CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of points4D must be 4" );
306 for( i = 0; i < 3; i++ )
308 if( projMatrs[i] == 0 )
310 CV_ERROR( CV_StsNullPtr, "Some of projMatrs is a NULL pointer" );
313 if( projPoints[i] == 0 )
315 CV_ERROR( CV_StsNullPtr, "Some of projPoints is a NULL pointer" );
318 if( resultProjMatrs[i] == 0 )
320 CV_ERROR( CV_StsNullPtr, "Some of resultProjMatrs is a NULL pointer" );
323 /* ----------- test for matrix ------------- */
324 if( !CV_IS_MAT(projMatrs[i]) )
326 CV_ERROR( CV_StsUnsupportedFormat, "Each of projMatrs must be a matrix" );
329 if( !CV_IS_MAT(projPoints[i]) )
331 CV_ERROR( CV_StsUnsupportedFormat, "Each of projPoints must be a matrix" );
334 if( !CV_IS_MAT(resultProjMatrs[i]) )
336 CV_ERROR( CV_StsUnsupportedFormat, "Each of resultProjMatrs must be a matrix" );
339 /* ------------- Test sizes --------------- */
340 if( projMatrs[i]->rows != 3 || projMatrs[i]->cols != 4 )
342 CV_ERROR( CV_StsUnmatchedSizes, "Size of projMatr must be 3x4" );
345 if( projPoints[i]->rows != 2 || projPoints[i]->cols != numPoints )
347 CV_ERROR( CV_StsUnmatchedSizes, "Size of resultProjMatrs must be 3x4" );
350 if( resultProjMatrs[i]->rows != 3 || resultProjMatrs[i]->cols != 4 )
352 CV_ERROR( CV_StsUnmatchedSizes, "Size of resultProjMatrs must be 3x4" );
357 /* Allocate memory for points 4D */
358 CV_CALL( points4D = cvCreateMat(4,numPoints,CV_64F) );
359 CV_CALL( vectorX0 = cvCreateMat(36 + numPoints*4,1,CV_64F) );
360 CV_CALL( observRes = cvCreateMat(2*numPoints*3,1,CV_64F) );
361 CV_CALL( optimX = cvCreateMat(36+numPoints*4,1,CV_64F) );
362 //CV_CALL( error = cvCreateMat(numPoints*2*3,1,CV_64F) );
365 /* Reconstruct points 4D using projected points and projection matrices */
366 icvReconstructPointsFor3View( projMatrs[0],projMatrs[1],projMatrs[2],
367 projPoints[0],projPoints[1],projPoints[2],
372 /* Fill observed points on images */
373 /* result of function is projection of N 3D points using 3 projection matrices */
374 /* projected points store as (projection by matrix P1),(projection by matrix P2),(projection by matrix P3) */
375 /* each projection is x1,y1,x2,y2,x3,y3,x4,y4 */
377 for( currMatr = 0; currMatr < 3; currMatr++ )
379 for( i = 0; i < numPoints; i++ )
381 cvmSet(observRes,currMatr*numPoints*2+i*2 ,0,cvmGet(projPoints[currMatr],0,i) );/* x */
382 cvmSet(observRes,currMatr*numPoints*2+i*2+1,0,cvmGet(projPoints[currMatr],1,i) );/* y */
386 /* Fill with projection matrices */
387 for( currMatr = 0; currMatr < 3; currMatr++ )
390 for( i = 0; i < 12; i++ )
392 cvmSet(vectorX0,currMatr*12+i,0,cvmGet(projMatrs[currMatr],i/4,i%4));
396 /* Fill with 4D points */
399 for( currPoint = 0; currPoint < numPoints; currPoint++ )
401 cvmSet(vectorX0,36 + currPoint*4 + 0,0,cvmGet(points4D,0,currPoint));
402 cvmSet(vectorX0,36 + currPoint*4 + 1,0,cvmGet(points4D,1,currPoint));
403 cvmSet(vectorX0,36 + currPoint*4 + 2,0,cvmGet(points4D,2,currPoint));
404 cvmSet(vectorX0,36 + currPoint*4 + 3,0,cvmGet(points4D,3,currPoint));
408 /* Allocate memory for result */
409 cvLevenbergMarquardtOptimization( icvJacobianFunction_ProjTrifocal, icvFunc_ProjTrifocal,
410 vectorX0,observRes,optimX,100,1e-6);
413 for( currMatr = 0; currMatr < 3; currMatr++ )
415 /* Copy projection matrices */
416 for(int i=0;i<12;i++)
418 cvmSet(resultProjMatrs[currMatr],i/4,i%4,cvmGet(optimX,currMatr*12+i,0));
423 for( currPoint = 0; currPoint < numPoints; currPoint++ )
425 cvmSet(resultPoints4D,0,currPoint,cvmGet(optimX,36 + currPoint*4,0));
426 cvmSet(resultPoints4D,1,currPoint,cvmGet(optimX,36 + currPoint*4+1,0));
427 cvmSet(resultPoints4D,2,currPoint,cvmGet(optimX,36 + currPoint*4+2,0));
428 cvmSet(resultPoints4D,3,currPoint,cvmGet(optimX,36 + currPoint*4+3,0));
433 /* Free allocated memory */
434 cvReleaseMat(&optimX);
435 cvReleaseMat(&points4D);
436 cvReleaseMat(&vectorX0);
437 cvReleaseMat(&observRes);
444 /*------------------------------------------------------------------------------*/
445 /* Create good points using status information */
446 void icvCreateGoodPoints(CvMat *points,CvMat **goodPoints, CvMat *status)
450 CV_FUNCNAME( "icvCreateGoodPoints" );
454 numPoints = points->cols;
458 CV_ERROR( CV_StsOutOfRange, "Number of points must be more than 0" );
462 numCoord = points->rows;
465 CV_ERROR( CV_StsOutOfRange, "Number of points coordinates must be more than 0" );
468 /* Define number of good points */
473 for( i = 0; i < numPoints; i++)
475 if( cvmGet(status,0,i) > 0 )
479 /* Allocate memory for good points */
480 CV_CALL( *goodPoints = cvCreateMat(numCoord,goodNum,CV_64F) );
482 for( i = 0; i < numCoord; i++ )
485 for( j = 0; j < numPoints; j++)
487 if( cvmGet(status,0,j) > 0 )
489 cvmSet(*goodPoints,i,currPoint,cvmGet(points,i,j));