1 /*M/////////////////////////////////////////////////////////////////////////////////////// 2 // 3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 // 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. 8 // 9 // 10 // Intel License Agreement 11 // For Open Source Computer Vision Library 12 // 13 // Copyright (C) 2000, Intel Corporation, all rights reserved. 14 // Third party copyrights are property of their respective owners. 15 // 16 // Redistribution and use in source and binary forms, with or without modification, 17 // are permitted provided that the following conditions are met: 18 // 19 // * Redistribution's of source code must retain the above copyright notice, 20 // this list of conditions and the following disclaimer. 21 // 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. 25 // 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. 28 // 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. 39 // 40 //M*/ 41 42 #include "_cvaux.h" 43 44 #include "cvtypes.h" 45 #include <float.h> 46 #include <limits.h> 47 #include "cv.h" 48 49 /* Valery Mosyagin */ 50 51 typedef void (*pointer_LMJac)( const CvMat* src, CvMat* dst ); 52 typedef void (*pointer_LMFunc)( const CvMat* src, CvMat* dst ); 53 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); 59 60 void icvReconstructPointsFor3View( CvMat* projMatr1,CvMat* projMatr2,CvMat* projMatr3, 61 CvMat* projPoints1,CvMat* projPoints2,CvMat* projPoints3, 62 CvMat* points4D); 63 64 65 /* Jacobian computation for trifocal case */ 66 void icvJacobianFunction_ProjTrifocal(const CvMat *vectX,CvMat *Jacobian) 67 { 68 CV_FUNCNAME( "icvJacobianFunction_ProjTrifocal" ); 69 __BEGIN__; 70 71 /* Test data for errors */ 72 if( vectX == 0 || Jacobian == 0 ) 73 { 74 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 75 } 76 77 if( !CV_IS_MAT(vectX) || !CV_IS_MAT(Jacobian) ) 78 { 79 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" ); 80 } 81 82 int numPoints; 83 numPoints = (vectX->rows - 36)/4; 84 85 if( numPoints < 1 )//!!! Need to correct this minimal number of points 86 { 87 CV_ERROR( CV_StsUnmatchedSizes, "number of points must be more than 0" ); 88 } 89 90 if( Jacobian->rows == numPoints*6 || Jacobian->cols != 36+numPoints*4 ) 91 { 92 CV_ERROR( CV_StsUnmatchedSizes, "Size of Jacobian is not correct it must be 6*numPoints x (36+numPoints*4)" ); 93 } 94 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) */ 101 102 /* Will store derivates as */ 103 /* Fill Jacobian matrix */ 104 int currProjPoint; 105 int currMatr; 106 107 cvZero(Jacobian); 108 for( currMatr = 0; currMatr < 3; currMatr++ ) 109 { 110 double p[12]; 111 for( int i=0;i<12;i++ ) 112 { 113 p[i] = cvmGet(vectX,currMatr*12+i,0); 114 } 115 116 int currVal = 36; 117 for( currProjPoint = 0; currProjPoint < numPoints; currProjPoint++ ) 118 { 119 /* Compute */ 120 double X[4]; 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); 125 126 double piX[3]; 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]; 130 131 int i,j; 132 /* fill derivate by point */ 133 134 double tmp3 = 1/(piX[2]*piX[2]); 135 136 double tmp1 = -piX[0]*tmp3; 137 double tmp2 = -piX[1]*tmp3; 138 for( j = 0; j < 2; j++ )//for x and y 139 { 140 for( i = 0; i < 4; i++ )// for X,Y,Z,W 141 { 142 cvmSet( Jacobian, 143 currMatr*numPoints*2+currProjPoint*2+j, 36+currProjPoint*4+i, 144 (p[j*4+i]*piX[2]-p[8+i]*piX[j]) * tmp3 ); 145 } 146 } 147 /* fill derivate by projection matrix */ 148 for( i = 0; i < 4; i++ ) 149 { 150 /* derivate for x */ 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 153 154 /* derivate for y */ 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 157 } 158 159 } 160 } 161 162 __END__; 163 return; 164 } 165 166 void icvFunc_ProjTrifocal(const CvMat *vectX, CvMat *resFunc) 167 { 168 /* Computes function in a given point */ 169 /* Computers project points using 3 projection matrices and points 3D */ 170 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(?) */ 174 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 */ 178 179 /* Compute projection of points */ 180 181 /* Fill projection matrices */ 182 183 CV_FUNCNAME( "icvFunc_ProjTrifocal" ); 184 __BEGIN__; 185 186 /* Test data for errors */ 187 if( vectX == 0 || resFunc == 0 ) 188 { 189 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 190 } 191 192 if( !CV_IS_MAT(vectX) || !CV_IS_MAT(resFunc) ) 193 { 194 CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" ); 195 } 196 197 int numPoints; 198 numPoints = (vectX->rows - 36)/4; 199 200 if( numPoints < 1 )//!!! Need to correct this minimal number of points 201 { 202 CV_ERROR( CV_StsUnmatchedSizes, "number of points must be more than 0" ); 203 } 204 205 if( resFunc->rows == 2*numPoints*3 || resFunc->cols != 1 ) 206 { 207 CV_ERROR( CV_StsUnmatchedSizes, "Size of resFunc is not correct it must be 2*numPoints*3 x 1"); 208 } 209 210 211 CvMat projMatrs[3]; 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); 216 217 CvMat point3D; 218 double point3D_dat[3]; 219 point3D = cvMat(3,1,CV_64F,point3D_dat); 220 221 int currMatr; 222 int currV; 223 int i,j; 224 225 currV=0; 226 for( currMatr = 0; currMatr < 3; currMatr++ ) 227 { 228 for( i = 0; i < 3; i++ ) 229 { 230 for( j = 0;j < 4; j++ ) 231 { 232 double val = cvmGet(vectX,currV,0); 233 cvmSet(&projMatrs[currMatr],i,j,val); 234 currV++; 235 } 236 } 237 } 238 239 /* Project points */ 240 int currPoint; 241 CvMat point4D; 242 double point4D_dat[4]; 243 point4D = cvMat(4,1,CV_64F,point4D_dat); 244 for( currPoint = 0; currPoint < numPoints; currPoint++ ) 245 { 246 /* get curr point */ 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); 251 252 for( currMatr = 0; currMatr < 3; currMatr++ ) 253 { 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); 259 } 260 } 261 262 __END__; 263 return; 264 } 265 266 267 /*----------------------------------------------------------------------------------------*/ 268 269 void icvOptimizeProjectionTrifocal(CvMat **projMatrs,CvMat **projPoints, 270 CvMat **resultProjMatrs, CvMat *resultPoints4D) 271 { 272 273 CvMat *optimX = 0; 274 CvMat *points4D = 0; 275 CvMat *vectorX0 = 0; 276 CvMat *observRes = 0; 277 //CvMat *error = 0; 278 279 CV_FUNCNAME( "icvOptimizeProjectionTrifocal" ); 280 __BEGIN__; 281 282 /* Test data for errors */ 283 if( projMatrs == 0 || projPoints == 0 || resultProjMatrs == 0 || resultPoints4D == 0) 284 { 285 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 286 } 287 288 if( !CV_IS_MAT(resultPoints4D) ) 289 { 290 CV_ERROR( CV_StsUnsupportedFormat, "resultPoints4D must be a matrix" ); 291 } 292 293 int numPoints; 294 numPoints = resultPoints4D->cols; 295 if( numPoints < 1 ) 296 { 297 CV_ERROR( CV_StsOutOfRange, "Number points of resultPoints4D must be more than 0" ); 298 } 299 300 if( resultPoints4D->rows != 4 ) 301 { 302 CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of points4D must be 4" ); 303 } 304 305 int i; 306 for( i = 0; i < 3; i++ ) 307 { 308 if( projMatrs[i] == 0 ) 309 { 310 CV_ERROR( CV_StsNullPtr, "Some of projMatrs is a NULL pointer" ); 311 } 312 313 if( projPoints[i] == 0 ) 314 { 315 CV_ERROR( CV_StsNullPtr, "Some of projPoints is a NULL pointer" ); 316 } 317 318 if( resultProjMatrs[i] == 0 ) 319 { 320 CV_ERROR( CV_StsNullPtr, "Some of resultProjMatrs is a NULL pointer" ); 321 } 322 323 /* ----------- test for matrix ------------- */ 324 if( !CV_IS_MAT(projMatrs[i]) ) 325 { 326 CV_ERROR( CV_StsUnsupportedFormat, "Each of projMatrs must be a matrix" ); 327 } 328 329 if( !CV_IS_MAT(projPoints[i]) ) 330 { 331 CV_ERROR( CV_StsUnsupportedFormat, "Each of projPoints must be a matrix" ); 332 } 333 334 if( !CV_IS_MAT(resultProjMatrs[i]) ) 335 { 336 CV_ERROR( CV_StsUnsupportedFormat, "Each of resultProjMatrs must be a matrix" ); 337 } 338 339 /* ------------- Test sizes --------------- */ 340 if( projMatrs[i]->rows != 3 || projMatrs[i]->cols != 4 ) 341 { 342 CV_ERROR( CV_StsUnmatchedSizes, "Size of projMatr must be 3x4" ); 343 } 344 345 if( projPoints[i]->rows != 2 || projPoints[i]->cols != numPoints ) 346 { 347 CV_ERROR( CV_StsUnmatchedSizes, "Size of resultProjMatrs must be 3x4" ); 348 } 349 350 if( resultProjMatrs[i]->rows != 3 || resultProjMatrs[i]->cols != 4 ) 351 { 352 CV_ERROR( CV_StsUnmatchedSizes, "Size of resultProjMatrs must be 3x4" ); 353 } 354 } 355 356 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) ); 363 364 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], 368 points4D); 369 370 371 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 */ 376 int currMatr; 377 for( currMatr = 0; currMatr < 3; currMatr++ ) 378 { 379 for( i = 0; i < numPoints; i++ ) 380 { 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 */ 383 } 384 } 385 386 /* Fill with projection matrices */ 387 for( currMatr = 0; currMatr < 3; currMatr++ ) 388 { 389 int i; 390 for( i = 0; i < 12; i++ ) 391 { 392 cvmSet(vectorX0,currMatr*12+i,0,cvmGet(projMatrs[currMatr],i/4,i%4)); 393 } 394 } 395 396 /* Fill with 4D points */ 397 398 int currPoint; 399 for( currPoint = 0; currPoint < numPoints; currPoint++ ) 400 { 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)); 405 } 406 407 408 /* Allocate memory for result */ 409 cvLevenbergMarquardtOptimization( icvJacobianFunction_ProjTrifocal, icvFunc_ProjTrifocal, 410 vectorX0,observRes,optimX,100,1e-6); 411 412 /* Copy results */ 413 for( currMatr = 0; currMatr < 3; currMatr++ ) 414 { 415 /* Copy projection matrices */ 416 for(int i=0;i<12;i++) 417 { 418 cvmSet(resultProjMatrs[currMatr],i/4,i%4,cvmGet(optimX,currMatr*12+i,0)); 419 } 420 } 421 422 /* Copy 4D points */ 423 for( currPoint = 0; currPoint < numPoints; currPoint++ ) 424 { 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)); 429 } 430 431 __END__; 432 433 /* Free allocated memory */ 434 cvReleaseMat(&optimX); 435 cvReleaseMat(&points4D); 436 cvReleaseMat(&vectorX0); 437 cvReleaseMat(&observRes); 438 439 return; 440 441 442 } 443 444 /*------------------------------------------------------------------------------*/ 445 /* Create good points using status information */ 446 void icvCreateGoodPoints(CvMat *points,CvMat **goodPoints, CvMat *status) 447 { 448 *goodPoints = 0; 449 450 CV_FUNCNAME( "icvCreateGoodPoints" ); 451 __BEGIN__; 452 453 int numPoints; 454 numPoints = points->cols; 455 456 if( numPoints < 1 ) 457 { 458 CV_ERROR( CV_StsOutOfRange, "Number of points must be more than 0" ); 459 } 460 461 int numCoord; 462 numCoord = points->rows; 463 if( numCoord < 1 ) 464 { 465 CV_ERROR( CV_StsOutOfRange, "Number of points coordinates must be more than 0" ); 466 } 467 468 /* Define number of good points */ 469 int goodNum; 470 int i,j; 471 472 goodNum = 0; 473 for( i = 0; i < numPoints; i++) 474 { 475 if( cvmGet(status,0,i) > 0 ) 476 goodNum++; 477 } 478 479 /* Allocate memory for good points */ 480 CV_CALL( *goodPoints = cvCreateMat(numCoord,goodNum,CV_64F) ); 481 482 for( i = 0; i < numCoord; i++ ) 483 { 484 int currPoint = 0; 485 for( j = 0; j < numPoints; j++) 486 { 487 if( cvmGet(status,0,j) > 0 ) 488 { 489 cvmSet(*goodPoints,i,currPoint,cvmGet(points,i,j)); 490 currPoint++; 491 } 492 } 493 } 494 __END__; 495 return; 496 } 497 498
