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 43#include "_cvaux.h" 44//#include "cvtypes.h" 45#include <float.h> 46#include <limits.h> 47//#include "cv.h" 48 49#include <stdio.h> 50 51void icvReconstructPoints4DStatus(CvMat** projPoints, CvMat **projMatrs, CvMat** presPoints, CvMat *points4D,int numImages,CvMat **projError=0); 52 53/* Valery Mosyagin */ 54 55/* If you want to save internal debug info to files uncomment next lines and set paths to files if need */ 56/* Note these file may be very large */ 57/* 58#define TRACK_BUNDLE 59#define TRACK_BUNDLE_FILE "d:\\test\\bundle.txt" 60#define TRACK_BUNDLE_FILE_JAC "d:\\test\\bundle.txt" 61#define TRACK_BUNDLE_FILE_JACERRPROJ "d:\\test\\JacErrProj.txt" 62#define TRACK_BUNDLE_FILE_JACERRPNT "d:\\test\\JacErrPoint.txt" 63#define TRACK_BUNDLE_FILE_MATRW "d:\\test\\matrWt.txt" 64#define TRACK_BUNDLE_FILE_DELTAP "d:\\test\\deltaP.txt" 65*/ 66#define TRACK_BUNDLE_FILE "d:\\test\\bundle.txt" 67 68 69/* ============== Bundle adjustment optimization ================= */ 70void icvComputeDerivateProj(CvMat *points4D,CvMat *projMatr, CvMat *status, CvMat *derivProj) 71{ 72 /* Compute derivate for given projection matrix points and status of points */ 73 74 CV_FUNCNAME( "icvComputeDerivateProj" ); 75 __BEGIN__; 76 77 78 /* ----- Test input params for errors ----- */ 79 if( points4D == 0 || projMatr == 0 || status == 0 || derivProj == 0) 80 { 81 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 82 } 83 84 if( !CV_IS_MAT(points4D) ) 85 { 86 CV_ERROR( CV_StsUnsupportedFormat, "points4D must be a matrix 4xN" ); 87 } 88 89 /* Compute number of points */ 90 int numPoints; 91 numPoints = points4D->cols; 92 93 if( numPoints < 1 ) 94 { 95 CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" ); 96 } 97 98 if( points4D->rows != 4 ) 99 { 100 CV_ERROR( CV_StsOutOfRange, "Number of coordinates of points4D must be 4" ); 101 } 102 103 if( !CV_IS_MAT(projMatr) ) 104 { 105 CV_ERROR( CV_StsUnsupportedFormat, "projMatr must be a matrix 3x4" ); 106 } 107 108 if( projMatr->rows != 3 || projMatr->cols != 4 ) 109 { 110 CV_ERROR( CV_StsOutOfRange, "Size of projection matrix (projMatr) must be 3x4" ); 111 } 112 113 if( !CV_IS_MAT(status) ) 114 { 115 CV_ERROR( CV_StsUnsupportedFormat, "Status must be a matrix 1xN" ); 116 } 117 118 if( status->rows != 1 || status->cols != numPoints ) 119 { 120 CV_ERROR( CV_StsOutOfRange, "Size of status of points must be 1xN" ); 121 } 122 123 if( !CV_IS_MAT(derivProj) ) 124 { 125 CV_ERROR( CV_StsUnsupportedFormat, "derivProj must be a matrix VisN x 12" ); 126 } 127 128 if( derivProj->cols != 12 ) 129 { 130 CV_ERROR( CV_StsOutOfRange, "derivProj must be a matrix VisN x 12" ); 131 } 132 /* ----- End test ----- */ 133 134 int i; 135 136 /* Allocate memory for derivates */ 137 138 double p[12]; 139 /* Copy projection matrix */ 140 for( i = 0; i < 12; i++ ) 141 { 142 p[i] = cvmGet(projMatr,i/4,i%4); 143 } 144 145 /* Fill deriv matrix */ 146 int currVisPoint; 147 int currPoint; 148 149 currVisPoint = 0; 150 for( currPoint = 0; currPoint < numPoints; currPoint++ ) 151 { 152 if( cvmGet(status,0,currPoint) > 0 ) 153 { 154 double X[4]; 155 X[0] = cvmGet(points4D,0,currVisPoint); 156 X[1] = cvmGet(points4D,1,currVisPoint); 157 X[2] = cvmGet(points4D,2,currVisPoint); 158 X[3] = cvmGet(points4D,3,currVisPoint); 159 160 /* Compute derivate for this point */ 161 162 double piX[3]; 163 piX[0] = X[0]*p[0] + X[1]*p[1] + X[2]*p[2] + X[3]*p[3]; 164 piX[1] = X[0]*p[4] + X[1]*p[5] + X[2]*p[6] + X[3]*p[7]; 165 piX[2] = X[0]*p[8] + X[1]*p[9] + X[2]*p[10] + X[3]*p[11]; 166 167 int i; 168 /* fill derivate by point */ 169 170 double tmp3 = 1/(piX[2]*piX[2]); 171 172 double tmp1 = -piX[0]*tmp3; 173 double tmp2 = -piX[1]*tmp3; 174 175 /* fill derivate by projection matrix */ 176 for( i = 0; i < 4; i++ ) 177 { 178 /* derivate for x */ 179 cvmSet(derivProj,currVisPoint*2,i,X[i]/piX[2]);//x' p1i 180 cvmSet(derivProj,currVisPoint*2,4+i,0);//x' p1i 181 cvmSet(derivProj,currVisPoint*2,8+i,X[i]*tmp1);//x' p3i 182 183 /* derivate for y */ 184 cvmSet(derivProj,currVisPoint*2+1,i,0);//y' p2i 185 cvmSet(derivProj,currVisPoint*2+1,4+i,X[i]/piX[2]);//y' p2i 186 cvmSet(derivProj,currVisPoint*2+1,8+i,X[i]*tmp2);//y' p3i 187 } 188 189 currVisPoint++; 190 } 191 } 192 193 if( derivProj->rows != currVisPoint * 2 ) 194 { 195 CV_ERROR( CV_StsOutOfRange, "derivProj must be a matrix 2VisN x 12" ); 196 } 197 198 199 __END__; 200 return; 201} 202/*======================================================================================*/ 203 204void icvComputeDerivateProjAll(CvMat *points4D, CvMat **projMatrs, CvMat **pointPres, int numImages,CvMat **projDerives) 205{ 206 CV_FUNCNAME( "icvComputeDerivateProjAll" ); 207 __BEGIN__; 208 209 /* ----- Test input params for errors ----- */ 210 if( numImages < 1 ) 211 { 212 CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); 213 } 214 if( projMatrs == 0 || pointPres == 0 || projDerives == 0 ) 215 { 216 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 217 } 218 /* ----- End test ----- */ 219 220 int currImage; 221 for( currImage = 0; currImage < numImages; currImage++ ) 222 { 223 icvComputeDerivateProj(points4D,projMatrs[currImage], pointPres[currImage], projDerives[currImage]); 224 } 225 226 __END__; 227 return; 228} 229/*======================================================================================*/ 230 231void icvComputeDerivatePoints(CvMat *points4D,CvMat *projMatr, CvMat *presPoints, CvMat *derivPoint) 232{ 233 234 CV_FUNCNAME( "icvComputeDerivatePoints" ); 235 __BEGIN__; 236 237 /* ----- Test input params for errors ----- */ 238 if( points4D == 0 || projMatr == 0 || presPoints == 0 || derivPoint == 0) 239 { 240 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 241 } 242 243 if( !CV_IS_MAT(points4D) ) 244 { 245 CV_ERROR( CV_StsUnsupportedFormat, "points4D must be a matrix N x 4" ); 246 } 247 248 int numPoints; 249 numPoints = presPoints->cols; 250 251 if( numPoints < 1 ) 252 { 253 CV_ERROR( CV_StsOutOfRange, "Number of points must be more than zero" ); 254 } 255 256 if( points4D->rows != 4 ) 257 { 258 CV_ERROR( CV_StsOutOfRange, "points4D must be a matrix N x 4" ); 259 } 260 261 if( !CV_IS_MAT(projMatr) ) 262 { 263 CV_ERROR( CV_StsUnsupportedFormat, "projMatr must be a matrix 3x4" ); 264 } 265 266 if( projMatr->rows != 3 || projMatr->cols != 4 ) 267 { 268 CV_ERROR( CV_StsOutOfRange, "Size of projection matrix (projMatr) must be 3x4" ); 269 } 270 271 if( !CV_IS_MAT(presPoints) ) 272 { 273 CV_ERROR( CV_StsUnsupportedFormat, "Status must be a matrix 1xN" ); 274 } 275 276 if( presPoints->rows != 1 || presPoints->cols != numPoints ) 277 { 278 CV_ERROR( CV_StsOutOfRange, "Size of presPoints status must be 1xN" ); 279 } 280 281 if( !CV_IS_MAT(derivPoint) ) 282 { 283 CV_ERROR( CV_StsUnsupportedFormat, "derivPoint must be a matrix 2 x 4VisNum" ); 284 } 285 /* ----- End test ----- */ 286 287 /* Compute derivates by points */ 288 289 double p[12]; 290 int i; 291 for( i = 0; i < 12; i++ ) 292 { 293 p[i] = cvmGet(projMatr,i/4,i%4); 294 } 295 296 int currVisPoint; 297 int currProjPoint; 298 299 currVisPoint = 0; 300 for( currProjPoint = 0; currProjPoint < numPoints; currProjPoint++ ) 301 { 302 if( cvmGet(presPoints,0,currProjPoint) > 0 ) 303 { 304 double X[4]; 305 X[0] = cvmGet(points4D,0,currProjPoint); 306 X[1] = cvmGet(points4D,1,currProjPoint); 307 X[2] = cvmGet(points4D,2,currProjPoint); 308 X[3] = cvmGet(points4D,3,currProjPoint); 309 310 double piX[3]; 311 piX[0] = X[0]*p[0] + X[1]*p[1] + X[2]*p[2] + X[3]*p[3]; 312 piX[1] = X[0]*p[4] + X[1]*p[5] + X[2]*p[6] + X[3]*p[7]; 313 piX[2] = X[0]*p[8] + X[1]*p[9] + X[2]*p[10] + X[3]*p[11]; 314 315 int i,j; 316 317 double tmp3 = 1/(piX[2]*piX[2]); 318 319 for( j = 0; j < 2; j++ )//for x and y 320 { 321 for( i = 0; i < 4; i++ )// for X,Y,Z,W 322 { 323 cvmSet( derivPoint, 324 j, currVisPoint*4+i, 325 (p[j*4+i]*piX[2]-p[8+i]*piX[j]) * tmp3 ); 326 } 327 } 328 currVisPoint++; 329 } 330 } 331 332 if( derivPoint->rows != 2 || derivPoint->cols != currVisPoint*4 ) 333 { 334 CV_ERROR( CV_StsUnsupportedFormat, "derivPoint must be a matrix 2 x 4VisNum" ); 335 } 336 337 __END__; 338 return; 339} 340/*======================================================================================*/ 341void icvComputeDerivatePointsAll(CvMat *points4D, CvMat **projMatrs, CvMat **pointPres, int numImages,CvMat **pointDerives) 342{ 343 CV_FUNCNAME( "icvComputeDerivatePointsAll" ); 344 __BEGIN__; 345 346 /* ----- Test input params for errors ----- */ 347 if( numImages < 1 ) 348 { 349 CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); 350 } 351 if( projMatrs == 0 || pointPres == 0 || pointDerives == 0 ) 352 { 353 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 354 } 355 /* ----- End test ----- */ 356 357 int currImage; 358 for( currImage = 0; currImage < numImages; currImage++ ) 359 { 360 icvComputeDerivatePoints(points4D, projMatrs[currImage], pointPres[currImage], pointDerives[currImage]); 361 } 362 363 __END__; 364 return; 365} 366/*======================================================================================*/ 367void icvComputeMatrixVAll(int numImages,CvMat **pointDeriv,CvMat **presPoints, CvMat **matrV) 368{ 369 int *shifts = 0; 370 371 CV_FUNCNAME( "icvComputeMatrixVAll" ); 372 __BEGIN__; 373 374 /* ----- Test input params for errors ----- */ 375 if( numImages < 1 ) 376 { 377 CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); 378 } 379 if( pointDeriv == 0 || presPoints == 0 || matrV == 0 ) 380 { 381 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 382 } 383 /* !!! not tested all parameters */ 384 /* ----- End test ----- */ 385 386 /* Compute all matrices U */ 387 int currImage; 388 int currPoint; 389 int numPoints; 390 numPoints = presPoints[0]->cols; 391 CV_CALL(shifts = (int*)cvAlloc(sizeof(int)*numImages)); 392 memset(shifts,0,sizeof(int)*numImages); 393 394 for( currPoint = 0; currPoint < numPoints; currPoint++ )//For each point (matrix V) 395 { 396 int i,j; 397 398 for( i = 0; i < 4; i++ ) 399 { 400 for( j = 0; j < 4; j++ ) 401 { 402 double sum = 0; 403 for( currImage = 0; currImage < numImages; currImage++ ) 404 { 405 if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) 406 { 407 sum += cvmGet(pointDeriv[currImage],0,shifts[currImage]*4+i) * 408 cvmGet(pointDeriv[currImage],0,shifts[currImage]*4+j); 409 410 sum += cvmGet(pointDeriv[currImage],1,shifts[currImage]*4+i) * 411 cvmGet(pointDeriv[currImage],1,shifts[currImage]*4+j); 412 } 413 } 414 415 cvmSet(matrV[currPoint],i,j,sum); 416 } 417 } 418 419 420 /* shift position of visible points */ 421 for( currImage = 0; currImage < numImages; currImage++ ) 422 { 423 if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) 424 { 425 shifts[currImage]++; 426 } 427 } 428 } 429 430 __END__; 431 cvFree( &shifts); 432 433 return; 434} 435/*======================================================================================*/ 436void icvComputeMatrixUAll(int numImages,CvMat **projDeriv,CvMat** matrU) 437{ 438 CV_FUNCNAME( "icvComputeMatrixVAll" ); 439 __BEGIN__; 440 /* ----- Test input params for errors ----- */ 441 if( numImages < 1 ) 442 { 443 CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); 444 } 445 if( projDeriv == 0 || matrU == 0 ) 446 { 447 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 448 } 449 /* !!! Not tested all input parameters */ 450 /* ----- End test ----- */ 451 452 /* Compute matrices V */ 453 int currImage; 454 for( currImage = 0; currImage < numImages; currImage++ ) 455 { 456 cvMulTransposed(projDeriv[currImage],matrU[currImage],1); 457 } 458 459 __END__; 460 return; 461} 462/*======================================================================================*/ 463void icvComputeMatrixW(int numImages, CvMat **projDeriv, CvMat **pointDeriv, CvMat **presPoints, CvMat *matrW) 464{ 465 CV_FUNCNAME( "icvComputeMatrixW" ); 466 __BEGIN__; 467 468 /* ----- Test input params for errors ----- */ 469 if( numImages < 1 ) 470 { 471 CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); 472 } 473 if( projDeriv == 0 || pointDeriv == 0 || presPoints == 0 || matrW == 0 ) 474 { 475 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 476 } 477 int numPoints; 478 numPoints = presPoints[0]->cols; 479 if( numPoints < 1 ) 480 { 481 CV_ERROR( CV_StsOutOfRange, "Number of points must more than zero" ); 482 } 483 if( !CV_IS_MAT(matrW) ) 484 { 485 CV_ERROR( CV_StsUnsupportedFormat, "matrW must be a matrix 12NumIm x 4NumPnt" ); 486 } 487 if( matrW->rows != numImages*12 || matrW->cols != numPoints*4 ) 488 { 489 CV_ERROR( CV_StsOutOfRange, "matrW must be a matrix 12NumIm x 4NumPnt" ); 490 } 491 /* !!! Not tested all input parameters */ 492 /* ----- End test ----- */ 493 494 /* Compute number of points */ 495 /* Compute matrix W using derivate proj and points */ 496 497 int currImage; 498 499 for( currImage = 0; currImage < numImages; currImage++ ) 500 { 501 for( int currLine = 0; currLine < 12; currLine++ ) 502 { 503 int currVis = 0; 504 for( int currPoint = 0; currPoint < numPoints; currPoint++ ) 505 { 506 if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) 507 { 508 509 for( int currCol = 0; currCol < 4; currCol++ ) 510 { 511 double sum; 512 sum = cvmGet(projDeriv[currImage],currVis*2+0,currLine) * 513 cvmGet(pointDeriv[currImage],0,currVis*4+currCol); 514 515 sum += cvmGet(projDeriv[currImage],currVis*2+1,currLine) * 516 cvmGet(pointDeriv[currImage],1,currVis*4+currCol); 517 518 cvmSet(matrW,currImage*12+currLine,currPoint*4+currCol,sum); 519 } 520 currVis++; 521 } 522 else 523 {/* set all sub elements to zero */ 524 for( int currCol = 0; currCol < 4; currCol++ ) 525 { 526 cvmSet(matrW,currImage*12+currLine,currPoint*4+currCol,0); 527 } 528 } 529 } 530 } 531 } 532 533#ifdef TRACK_BUNDLE 534 { 535 FILE *file; 536 file = fopen( TRACK_BUNDLE_FILE_MATRW ,"w"); 537 int currPoint,currImage; 538 for( currPoint = 0; currPoint < numPoints; currPoint++ ) 539 { 540 fprintf(file,"\nPoint=%d\n",currPoint); 541 int currRow; 542 for( currRow = 0; currRow < 4; currRow++ ) 543 { 544 for( currImage = 0; currImage< numImages; currImage++ ) 545 { 546 int i; 547 for( i = 0; i < 12; i++ ) 548 { 549 double val = cvmGet(matrW, currImage * 12 + i, currPoint * 4 + currRow); 550 fprintf(file,"%lf ",val); 551 } 552 } 553 fprintf(file,"\n"); 554 } 555 } 556 fclose(file); 557 } 558#endif 559 560 __END__; 561 return; 562} 563/*======================================================================================*/ 564/* Compute jacobian mult projection matrices error */ 565void icvComputeJacErrorProj(int numImages,CvMat **projDeriv,CvMat **projErrors,CvMat *jacProjErr ) 566{ 567 CV_FUNCNAME( "icvComputeJacErrorProj" ); 568 __BEGIN__; 569 570 /* ----- Test input params for errors ----- */ 571 if( numImages < 1 ) 572 { 573 CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); 574 } 575 if( projDeriv == 0 || projErrors == 0 || jacProjErr == 0 ) 576 { 577 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 578 } 579 if( !CV_IS_MAT(jacProjErr) ) 580 { 581 CV_ERROR( CV_StsUnsupportedFormat, "jacProjErr must be a matrix 12NumIm x 1" ); 582 } 583 if( jacProjErr->rows != numImages*12 || jacProjErr->cols != 1 ) 584 { 585 CV_ERROR( CV_StsOutOfRange, "jacProjErr must be a matrix 12NumIm x 1" ); 586 } 587 /* !!! Not tested all input parameters */ 588 /* ----- End test ----- */ 589 590 int currImage; 591 for( currImage = 0; currImage < numImages; currImage++ ) 592 { 593 for( int currCol = 0; currCol < 12; currCol++ ) 594 { 595 int num = projDeriv[currImage]->rows; 596 double sum = 0; 597 for( int i = 0; i < num; i++ ) 598 { 599 sum += cvmGet(projDeriv[currImage],i,currCol) * 600 cvmGet(projErrors[currImage],i%2,i/2); 601 } 602 cvmSet(jacProjErr,currImage*12+currCol,0,sum); 603 } 604 } 605 606#ifdef TRACK_BUNDLE 607 { 608 FILE *file; 609 file = fopen( TRACK_BUNDLE_FILE_JACERRPROJ ,"w"); 610 int currImage; 611 for( currImage = 0; currImage < numImages; currImage++ ) 612 { 613 fprintf(file,"\nImage=%d\n",currImage); 614 int currRow; 615 for( currRow = 0; currRow < 12; currRow++ ) 616 { 617 double val = cvmGet(jacProjErr, currImage * 12 + currRow, 0); 618 fprintf(file,"%lf\n",val); 619 } 620 fprintf(file,"\n"); 621 } 622 fclose(file); 623 } 624#endif 625 626 627 __END__; 628 return; 629} 630/*======================================================================================*/ 631/* Compute jacobian mult points error */ 632void icvComputeJacErrorPoint(int numImages,CvMat **pointDeriv,CvMat **projErrors, CvMat **presPoints,CvMat *jacPointErr ) 633{ 634 int *shifts = 0; 635 636 CV_FUNCNAME( "icvComputeJacErrorPoint" ); 637 __BEGIN__; 638 639 /* ----- Test input params for errors ----- */ 640 if( numImages < 1 ) 641 { 642 CV_ERROR( CV_StsOutOfRange, "Number of images must more than zero" ); 643 } 644 645 if( pointDeriv == 0 || projErrors == 0 || presPoints == 0 || jacPointErr == 0 ) 646 { 647 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 648 } 649 650 int numPoints; 651 numPoints = presPoints[0]->cols; 652 if( numPoints < 1 ) 653 { 654 CV_ERROR( CV_StsOutOfRange, "Number of points must more than zero" ); 655 } 656 657 if( !CV_IS_MAT(jacPointErr) ) 658 { 659 CV_ERROR( CV_StsUnsupportedFormat, "jacPointErr must be a matrix 4NumPnt x 1" ); 660 } 661 662 if( jacPointErr->rows != numPoints*4 || jacPointErr->cols != 1 ) 663 { 664 CV_ERROR( CV_StsOutOfRange, "jacPointErr must be a matrix 4NumPnt x 1" ); 665 } 666 /* !!! Not tested all input parameters */ 667 /* ----- End test ----- */ 668 669 int currImage; 670 int currPoint; 671 CV_CALL(shifts = (int*)cvAlloc(sizeof(int)*numImages)); 672 memset(shifts,0,sizeof(int)*numImages); 673 for( currPoint = 0; currPoint < numPoints; currPoint++ ) 674 { 675 for( int currCoord = 0; currCoord < 4; currCoord++ ) 676 { 677 double sum = 0; 678 { 679 int currVis = 0; 680 for( currImage = 0; currImage < numImages; currImage++ ) 681 { 682 if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) 683 { 684 sum += cvmGet(pointDeriv[currImage],0,shifts[currImage]*4+currCoord) * 685 cvmGet(projErrors[currImage],0,shifts[currImage]);//currVis); 686 687 sum += cvmGet(pointDeriv[currImage],1,shifts[currImage]*4+currCoord) * 688 cvmGet(projErrors[currImage],1,shifts[currImage]);//currVis); 689 690 currVis++; 691 } 692 } 693 } 694 695 cvmSet(jacPointErr,currPoint*4+currCoord,0,sum); 696 } 697 698 /* Increase shifts */ 699 for( currImage = 0; currImage < numImages; currImage++ ) 700 { 701 if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) 702 { 703 shifts[currImage]++; 704 } 705 } 706 } 707 708 709#ifdef TRACK_BUNDLE 710 { 711 FILE *file; 712 file = fopen(TRACK_BUNDLE_FILE_JACERRPNT,"w"); 713 int currPoint; 714 for( currPoint = 0; currPoint < numPoints; currPoint++ ) 715 { 716 fprintf(file,"\nPoint=%d\n",currPoint); 717 int currRow; 718 for( currRow = 0; currRow < 4; currRow++ ) 719 { 720 double val = cvmGet(jacPointErr, currPoint * 4 + currRow, 0); 721 fprintf(file,"%lf\n",val); 722 } 723 fprintf(file,"\n"); 724 } 725 fclose(file); 726 } 727#endif 728 729 730 731 __END__; 732 cvFree( &shifts); 733 734} 735/*======================================================================================*/ 736 737/* Reconstruct 4D points using status */ 738void icvReconstructPoints4DStatus(CvMat** projPoints, CvMat **projMatrs, CvMat** presPoints, 739 CvMat *points4D,int numImages,CvMat **projError) 740{ 741 742 double* matrA_dat = 0; 743 double* matrW_dat = 0; 744 745 CV_FUNCNAME( "icvReconstructPoints4DStatus" ); 746 __BEGIN__; 747 748 /* ----- Test input params for errors ----- */ 749 if( numImages < 2 ) 750 { 751 CV_ERROR( CV_StsOutOfRange, "Number of images must be more than one" ); 752 } 753 754 if( projPoints == 0 || projMatrs == 0 || presPoints == 0 || points4D == 0 ) 755 { 756 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 757 } 758 759 int numPoints; 760 numPoints = points4D->cols; 761 if( numPoints < 1 ) 762 { 763 CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" ); 764 } 765 766 if( points4D->rows != 4 ) 767 { 768 CV_ERROR( CV_StsOutOfRange, "Points must have 4 cordinates" ); 769 } 770 771 /* !!! Not tested all input parameters */ 772 /* ----- End test ----- */ 773 774 int currImage; 775 int currPoint; 776 777 /* Allocate maximum data */ 778 779 780 CvMat matrV; 781 double matrV_dat[4*4]; 782 matrV = cvMat(4,4,CV_64F,matrV_dat); 783 784 CV_CALL(matrA_dat = (double*)cvAlloc(3*numImages * 4 * sizeof(double))); 785 CV_CALL(matrW_dat = (double*)cvAlloc(3*numImages * 4 * sizeof(double))); 786 787 /* reconstruct each point */ 788 for( currPoint = 0; currPoint < numPoints; currPoint++ ) 789 { 790 /* Reconstruct current point */ 791 /* Define number of visible projections */ 792 int numVisProj = 0; 793 for( currImage = 0; currImage < numImages; currImage++ ) 794 { 795 if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) 796 { 797 numVisProj++; 798 } 799 } 800 801 if( numVisProj < 2 ) 802 { 803 /* This point can't be reconstructed */ 804 continue; 805 } 806 807 /* Allocate memory and create matrices */ 808 CvMat matrA; 809 matrA = cvMat(3*numVisProj,4,CV_64F,matrA_dat); 810 811 CvMat matrW; 812 matrW = cvMat(3*numVisProj,4,CV_64F,matrW_dat); 813 814 int currVisProj = 0; 815 for( currImage = 0; currImage < numImages; currImage++ )/* For each view */ 816 { 817 if( cvmGet(presPoints[currImage],0,currPoint) > 0 ) 818 { 819 double x,y; 820 x = cvmGet(projPoints[currImage],0,currPoint); 821 y = cvmGet(projPoints[currImage],1,currPoint); 822 for( int k = 0; k < 4; k++ ) 823 { 824 matrA_dat[currVisProj*12 + k] = 825 x * cvmGet(projMatrs[currImage],2,k) - cvmGet(projMatrs[currImage],0,k); 826 827 matrA_dat[currVisProj*12+4 + k] = 828 y * cvmGet(projMatrs[currImage],2,k) - cvmGet(projMatrs[currImage],1,k); 829 830 matrA_dat[currVisProj*12+8 + k] = 831 x * cvmGet(projMatrs[currImage],1,k) - y * cvmGet(projMatrs[currImage],0,k); 832 } 833 currVisProj++; 834 } 835 } 836 837 /* Solve system for current point */ 838 { 839 cvSVD(&matrA,&matrW,0,&matrV,CV_SVD_V_T); 840 841 /* Copy computed point */ 842 cvmSet(points4D,0,currPoint,cvmGet(&matrV,3,0));//X 843 cvmSet(points4D,1,currPoint,cvmGet(&matrV,3,1));//Y 844 cvmSet(points4D,2,currPoint,cvmGet(&matrV,3,2));//Z 845 cvmSet(points4D,3,currPoint,cvmGet(&matrV,3,3));//W 846 } 847 848 } 849 850 {/* Compute projection error */ 851 for( currImage = 0; currImage < numImages; currImage++ ) 852 { 853 CvMat point4D; 854 CvMat point3D; 855 double point3D_dat[3]; 856 point3D = cvMat(3,1,CV_64F,point3D_dat); 857 858 int currPoint; 859 int numVis = 0; 860 double totalError = 0; 861 for( currPoint = 0; currPoint < numPoints; currPoint++ ) 862 { 863 if( cvmGet(presPoints[currImage],0,currPoint) > 0) 864 { 865 double dx,dy; 866 cvGetCol(points4D,&point4D,currPoint); 867 cvmMul(projMatrs[currImage],&point4D,&point3D); 868 double w = point3D_dat[2]; 869 double x = point3D_dat[0] / w; 870 double y = point3D_dat[1] / w; 871 872 dx = cvmGet(projPoints[currImage],0,currPoint) - x; 873 dy = cvmGet(projPoints[currImage],1,currPoint) - y; 874 if( projError ) 875 { 876 cvmSet(projError[currImage],0,currPoint,dx); 877 cvmSet(projError[currImage],1,currPoint,dy); 878 } 879 totalError += sqrt(dx*dx+dy*dy); 880 numVis++; 881 } 882 } 883 884 //double meanError = totalError / (double)numVis; 885 886 } 887 888 } 889 890 __END__; 891 892 cvFree( &matrA_dat); 893 cvFree( &matrW_dat); 894 895 return; 896} 897 898/*======================================================================================*/ 899 900void icvProjPointsStatusFunc( int numImages, CvMat *points4D, CvMat **projMatrs, CvMat **pointsPres, CvMat **projPoints) 901{ 902 CV_FUNCNAME( "icvProjPointsStatusFunc" ); 903 __BEGIN__; 904 905 /* ----- Test input params for errors ----- */ 906 if( numImages < 1 ) 907 { 908 CV_ERROR( CV_StsOutOfRange, "Number of images must be more than zero" ); 909 } 910 911 if( points4D == 0 || projMatrs == 0 || pointsPres == 0 || projPoints == 0 ) 912 { 913 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 914 } 915 916 int numPoints; 917 numPoints = points4D->cols; 918 if( numPoints < 1 ) 919 { 920 CV_ERROR( CV_StsOutOfRange, "Number of points4D must be more than zero" ); 921 } 922 923 if( points4D->rows != 4 ) 924 { 925 CV_ERROR( CV_StsOutOfRange, "Points must have 4 cordinates" ); 926 } 927 928 /* !!! Not tested all input parameters */ 929 /* ----- End test ----- */ 930 931 CvMat point4D; 932 CvMat point3D; 933 double point4D_dat[4]; 934 double point3D_dat[3]; 935 point4D = cvMat(4,1,CV_64F,point4D_dat); 936 point3D = cvMat(3,1,CV_64F,point3D_dat); 937 938#ifdef TRACK_BUNDLE 939 { 940 FILE *file; 941 file = fopen( TRACK_BUNDLE_FILE ,"a"); 942 fprintf(file,"\n----- test 14.01 icvProjPointsStatusFunc -----\n"); 943 fclose(file); 944 } 945#endif 946 947 int currImage; 948 for( currImage = 0; currImage < numImages; currImage++ ) 949 { 950 /* Get project matrix */ 951 /* project visible points using current projection matrix */ 952 int currVisPoint = 0; 953 for( int currPoint = 0; currPoint < numPoints; currPoint++ ) 954 { 955 if( cvmGet(pointsPres[currImage],0,currPoint) > 0 ) 956 { 957 /* project current point */ 958 cvGetSubRect(points4D,&point4D,cvRect(currPoint,0,1,4)); 959 960#ifdef TRACK_BUNDLE 961 { 962 FILE *file; 963 file = fopen( TRACK_BUNDLE_FILE ,"a"); 964 fprintf(file,"\n----- test 14.02 point4D (%lf, %lf, %lf, %lf) -----\n", 965 cvmGet(&point4D,0,0), 966 cvmGet(&point4D,1,0), 967 cvmGet(&point4D,2,0), 968 cvmGet(&point4D,3,0)); 969 fclose(file); 970 } 971#endif 972 973 cvmMul(projMatrs[currImage],&point4D,&point3D); 974 double w = point3D_dat[2]; 975 cvmSet(projPoints[currImage],0,currVisPoint,point3D_dat[0]/w); 976 cvmSet(projPoints[currImage],1,currVisPoint,point3D_dat[1]/w); 977 978#ifdef TRACK_BUNDLE 979 { 980 FILE *file; 981 file = fopen( TRACK_BUNDLE_FILE ,"a"); 982 fprintf(file,"\n----- test 14.03 (%lf, %lf, %lf) -> (%lf, %lf)-----\n", 983 point3D_dat[0], 984 point3D_dat[1], 985 point3D_dat[2], 986 point3D_dat[0]/w, 987 point3D_dat[1]/w 988 ); 989 fclose(file); 990 } 991#endif 992 currVisPoint++; 993 } 994 } 995 } 996 997 __END__; 998} 999 1000/*======================================================================================*/ 1001void icvFreeMatrixArray(CvMat ***matrArray,int numMatr) 1002{ 1003 /* Free each matrix */ 1004 int currMatr; 1005 1006 if( *matrArray != 0 ) 1007 {/* Need delete */ 1008 for( currMatr = 0; currMatr < numMatr; currMatr++ ) 1009 { 1010 cvReleaseMat((*matrArray)+currMatr); 1011 } 1012 cvFree( matrArray); 1013 } 1014 return; 1015} 1016 1017/*======================================================================================*/ 1018void *icvClearAlloc(int size) 1019{ 1020 void *ptr = 0; 1021 1022 CV_FUNCNAME( "icvClearAlloc" ); 1023 __BEGIN__; 1024 1025 if( size > 0 ) 1026 { 1027 CV_CALL(ptr = cvAlloc(size)); 1028 memset(ptr,0,size); 1029 } 1030 1031 __END__; 1032 return ptr; 1033} 1034 1035/*======================================================================================*/ 1036#if 0 1037void cvOptimizeLevenbergMarquardtBundleWraper( CvMat** projMatrs, CvMat** observProjPoints, 1038 CvMat** pointsPres, int numImages, 1039 CvMat** resultProjMatrs, CvMat* resultPoints4D,int maxIter,double epsilon ) 1040{ 1041 /* Delete al sparse points */ 1042 1043int icvDeleteSparsInPoints( int numImages, 1044 CvMat **points, 1045 CvMat **status, 1046 CvMat *wasStatus)/* status of previous configuration */ 1047 1048} 1049#endif 1050/*======================================================================================*/ 1051/* !!! may be useful to return norm of error */ 1052/* !!! may be does not work correct with not all visible 4D points */ 1053void cvOptimizeLevenbergMarquardtBundle( CvMat** projMatrs, CvMat** observProjPoints, 1054 CvMat** pointsPres, int numImages, 1055 CvMat** resultProjMatrs, CvMat* resultPoints4D,int maxIter,double epsilon ) 1056{ 1057 1058 CvMat *vectorX_points4D = 0; 1059 CvMat **vectorX_projMatrs = 0; 1060 1061 CvMat *newVectorX_points4D = 0; 1062 CvMat **newVectorX_projMatrs = 0; 1063 1064 CvMat *changeVectorX_points4D = 0; 1065 CvMat *changeVectorX_projMatrs = 0; 1066 1067 CvMat **observVisPoints = 0; 1068 CvMat **projVisPoints = 0; 1069 CvMat **errorProjPoints = 0; 1070 CvMat **DerivProj = 0; 1071 CvMat **DerivPoint = 0; 1072 CvMat *matrW = 0; 1073 CvMat **matrsUk = 0; 1074 CvMat **workMatrsUk = 0; 1075 CvMat **matrsVi = 0; 1076 CvMat *workMatrVi = 0; 1077 CvMat **workMatrsInvVi = 0; 1078 CvMat *jacProjErr = 0; 1079 CvMat *jacPointErr = 0; 1080 1081 CvMat *matrTmpSys1 = 0; 1082 CvMat *matrSysDeltaP = 0; 1083 CvMat *vectTmpSys3 = 0; 1084 CvMat *vectSysDeltaP = 0; 1085 CvMat *deltaP = 0; 1086 CvMat *deltaM = 0; 1087 CvMat *vectTmpSysM = 0; 1088 1089 int numPoints = 0; 1090 1091 1092 CV_FUNCNAME( "cvOptimizeLevenbergMarquardtBundle" ); 1093 __BEGIN__; 1094 1095 /* ----- Test input params for errors ----- */ 1096 if( numImages < 1 ) 1097 { 1098 CV_ERROR( CV_StsOutOfRange, "Number of images must be more than zero" ); 1099 } 1100 1101 if( maxIter < 1 || maxIter > 2000 ) 1102 { 1103 CV_ERROR( CV_StsOutOfRange, "Maximum number of iteration must be in [1..1000]" ); 1104 } 1105 1106 if( epsilon < 0 ) 1107 { 1108 CV_ERROR( CV_StsOutOfRange, "Epsilon parameter must be >= 0" ); 1109 } 1110 1111 if( !CV_IS_MAT(resultPoints4D) ) 1112 { 1113 CV_ERROR( CV_StsUnsupportedFormat, "resultPoints4D must be a matrix 4 x NumPnt" ); 1114 } 1115 1116 numPoints = resultPoints4D->cols; 1117 if( numPoints < 1 ) 1118 { 1119 CV_ERROR( CV_StsOutOfRange, "Number of points must be more than zero" );//!!! 1120 } 1121 1122 if( resultPoints4D->rows != 4 ) 1123 { 1124 CV_ERROR( CV_StsOutOfRange, "resultPoints4D must have 4 cordinates" ); 1125 } 1126 1127 /* ----- End test ----- */ 1128 1129 /* Optimization using bundle adjustment */ 1130 /* work with non visible points */ 1131 1132 CV_CALL( vectorX_points4D = cvCreateMat(4,numPoints,CV_64F)); 1133 CV_CALL( vectorX_projMatrs = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages)); 1134 1135 CV_CALL( newVectorX_points4D = cvCreateMat(4,numPoints,CV_64F)); 1136 CV_CALL( newVectorX_projMatrs = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages)); 1137 1138 CV_CALL( changeVectorX_points4D = cvCreateMat(4,numPoints,CV_64F)); 1139 CV_CALL( changeVectorX_projMatrs = cvCreateMat(3,4,CV_64F)); 1140 1141 int currImage; 1142 1143 /* ----- Test input params ----- */ 1144 for( currImage = 0; currImage < numImages; currImage++ ) 1145 { 1146 /* Test size of input initial and result projection matrices */ 1147 if( !CV_IS_MAT(projMatrs[currImage]) ) 1148 { 1149 CV_ERROR( CV_StsUnsupportedFormat, "each of initial projMatrs must be a matrix 3 x 4" ); 1150 } 1151 if( projMatrs[currImage]->rows != 3 || projMatrs[currImage]->cols != 4 ) 1152 { 1153 CV_ERROR( CV_StsOutOfRange, "each of initial projMatrs must be a matrix 3 x 4" ); 1154 } 1155 1156 1157 if( !CV_IS_MAT(observProjPoints[currImage]) ) 1158 { 1159 CV_ERROR( CV_StsUnsupportedFormat, "each of observProjPoints must be a matrix 2 x NumPnts" ); 1160 } 1161 if( observProjPoints[currImage]->rows != 2 || observProjPoints[currImage]->cols != numPoints ) 1162 { 1163 CV_ERROR( CV_StsOutOfRange, "each of observProjPoints must be a matrix 2 x NumPnts" ); 1164 } 1165 1166 if( !CV_IS_MAT(pointsPres[currImage]) ) 1167 { 1168 CV_ERROR( CV_StsUnsupportedFormat, "each of pointsPres must be a matrix 1 x NumPnt" ); 1169 } 1170 if( pointsPres[currImage]->rows != 1 || pointsPres[currImage]->cols != numPoints ) 1171 { 1172 CV_ERROR( CV_StsOutOfRange, "each of pointsPres must be a matrix 1 x NumPnt" ); 1173 } 1174 1175 if( !CV_IS_MAT(resultProjMatrs[currImage]) ) 1176 { 1177 CV_ERROR( CV_StsUnsupportedFormat, "each of resultProjMatrs must be a matrix 3 x 4" ); 1178 } 1179 if( resultProjMatrs[currImage]->rows != 3 || resultProjMatrs[currImage]->cols != 4 ) 1180 { 1181 CV_ERROR( CV_StsOutOfRange, "each of resultProjMatrs must be a matrix 3 x 4" ); 1182 } 1183 1184 } 1185 /* ----- End test ----- */ 1186 1187 /* Copy projection matrices to vectorX0 */ 1188 for( currImage = 0; currImage < numImages; currImage++ ) 1189 { 1190 CV_CALL( vectorX_projMatrs[currImage] = cvCreateMat(3,4,CV_64F)); 1191 CV_CALL( newVectorX_projMatrs[currImage] = cvCreateMat(3,4,CV_64F)); 1192 cvCopy(projMatrs[currImage],vectorX_projMatrs[currImage]); 1193 } 1194 1195 /* Reconstruct points4D using projection matrices and status information */ 1196 icvReconstructPoints4DStatus(observProjPoints, projMatrs, pointsPres, vectorX_points4D, numImages); 1197 1198 /* ----- Allocate memory for work matrices ----- */ 1199 /* Compute number of good points on each images */ 1200 1201 CV_CALL( observVisPoints = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); 1202 CV_CALL( projVisPoints = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); 1203 CV_CALL( errorProjPoints = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); 1204 CV_CALL( DerivProj = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); 1205 CV_CALL( DerivPoint = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); 1206 CV_CALL( matrW = cvCreateMat(12*numImages,4*numPoints,CV_64F) ); 1207 CV_CALL( matrsUk = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); 1208 CV_CALL( workMatrsUk = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numImages) ); 1209 CV_CALL( matrsVi = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numPoints) ); 1210 CV_CALL( workMatrVi = cvCreateMat(4,4,CV_64F) ); 1211 CV_CALL( workMatrsInvVi = (CvMat**)icvClearAlloc(sizeof(CvMat*)*numPoints) ); 1212 1213 CV_CALL( jacProjErr = cvCreateMat(12*numImages,1,CV_64F) ); 1214 CV_CALL( jacPointErr = cvCreateMat(4*numPoints,1,CV_64F) ); 1215 1216 1217 int i; 1218 for( i = 0; i < numPoints; i++ ) 1219 { 1220 CV_CALL( matrsVi[i] = cvCreateMat(4,4,CV_64F) ); 1221 CV_CALL( workMatrsInvVi[i] = cvCreateMat(4,4,CV_64F) ); 1222 } 1223 1224 for( currImage = 0; currImage < numImages; currImage++ ) 1225 { 1226 CV_CALL( matrsUk[currImage] = cvCreateMat(12,12,CV_64F) ); 1227 CV_CALL( workMatrsUk[currImage] = cvCreateMat(12,12,CV_64F) ); 1228 1229 int currVisPoint = 0, currPoint, numVisPoint; 1230 for( currPoint = 0; currPoint < numPoints; currPoint++ ) 1231 { 1232 if( cvmGet(pointsPres[currImage],0,currPoint) > 0 ) 1233 { 1234 currVisPoint++; 1235 } 1236 } 1237 1238 numVisPoint = currVisPoint; 1239 1240 /* Allocate memory for current image data */ 1241 CV_CALL( observVisPoints[currImage] = cvCreateMat(2,numVisPoint,CV_64F) ); 1242 CV_CALL( projVisPoints[currImage] = cvCreateMat(2,numVisPoint,CV_64F) ); 1243 1244 /* create error matrix */ 1245 CV_CALL( errorProjPoints[currImage] = cvCreateMat(2,numVisPoint,CV_64F) ); 1246 1247 /* Create derivate matrices */ 1248 CV_CALL( DerivProj[currImage] = cvCreateMat(2*numVisPoint,12,CV_64F) ); 1249 CV_CALL( DerivPoint[currImage] = cvCreateMat(2,numVisPoint*4,CV_64F) ); 1250 1251 /* Copy observed projected visible points */ 1252 currVisPoint = 0; 1253 for( currPoint = 0; currPoint < numPoints; currPoint++ ) 1254 { 1255 if( cvmGet(pointsPres[currImage],0,currPoint) > 0 ) 1256 { 1257 cvmSet(observVisPoints[currImage],0,currVisPoint,cvmGet(observProjPoints[currImage],0,currPoint)); 1258 cvmSet(observVisPoints[currImage],1,currVisPoint,cvmGet(observProjPoints[currImage],1,currPoint)); 1259 currVisPoint++; 1260 } 1261 } 1262 } 1263 /*---------------------------------------------------*/ 1264 1265 CV_CALL( matrTmpSys1 = cvCreateMat(numPoints*4, numImages*12, CV_64F) ); 1266 CV_CALL( matrSysDeltaP = cvCreateMat(numImages*12, numImages*12, CV_64F) ); 1267 CV_CALL( vectTmpSys3 = cvCreateMat(numPoints*4,1,CV_64F) ); 1268 CV_CALL( vectSysDeltaP = cvCreateMat(numImages*12,1,CV_64F) ); 1269 CV_CALL( deltaP = cvCreateMat(numImages*12,1,CV_64F) ); 1270 CV_CALL( deltaM = cvCreateMat(numPoints*4,1,CV_64F) ); 1271 CV_CALL( vectTmpSysM = cvCreateMat(numPoints*4,1,CV_64F) ); 1272 1273 1274//#ifdef TRACK_BUNDLE 1275#if 1 1276 { 1277 /* Create file to track */ 1278 FILE* file; 1279 file = fopen( TRACK_BUNDLE_FILE ,"w"); 1280 fprintf(file,"begin\n"); 1281 fclose(file); 1282 } 1283#endif 1284 1285 /* ============= main optimization loop ============== */ 1286 1287 /* project all points using current vector X */ 1288 icvProjPointsStatusFunc(numImages, vectorX_points4D, vectorX_projMatrs, pointsPres, projVisPoints); 1289 1290 /* Compute error with observed value and computed projection */ 1291 double prevError; 1292 prevError = 0; 1293 for( currImage = 0; currImage < numImages; currImage++ ) 1294 { 1295 cvSub(observVisPoints[currImage],projVisPoints[currImage],errorProjPoints[currImage]); 1296 double currNorm = cvNorm(errorProjPoints[currImage]); 1297 prevError += currNorm * currNorm; 1298 } 1299 prevError = sqrt(prevError); 1300 1301 int currIter; 1302 double change; 1303 double alpha; 1304 1305//#ifdef TRACK_BUNDLE 1306#if 1 1307 { 1308 /* Create file to track */ 1309 FILE* file; 1310 file = fopen( TRACK_BUNDLE_FILE ,"a"); 1311 fprintf(file,"\n========================================\n");; 1312 fprintf(file,"Iter=0\n"); 1313 fprintf(file,"Error = %20.15lf\n",prevError); 1314 fprintf(file,"Change = %20.15lf\n",1.0); 1315 1316 fprintf(file,"projection errors\n"); 1317 1318 /* Print all proejction errors */ 1319 int currImage; 1320 for( currImage = 0; currImage < numImages; currImage++) 1321 { 1322 fprintf(file,"\nImage=%d\n",currImage); 1323 int numPn = errorProjPoints[currImage]->cols; 1324 for( int currPoint = 0; currPoint < numPn; currPoint++ ) 1325 { 1326 double ex,ey; 1327 ex = cvmGet(errorProjPoints[currImage],0,currPoint); 1328 ey = cvmGet(errorProjPoints[currImage],1,currPoint); 1329 fprintf(file,"%40.35lf, %40.35lf\n",ex,ey); 1330 } 1331 } 1332 fclose(file); 1333 } 1334#endif 1335 1336 currIter = 0; 1337 change = 1; 1338 alpha = 0.001; 1339 1340 1341 do 1342 { 1343 1344#ifdef TRACK_BUNDLE 1345 { 1346 FILE *file; 1347 file = fopen( TRACK_BUNDLE_FILE ,"a"); 1348 fprintf(file,"\n----- test 6 do main -----\n"); 1349 1350 double norm = cvNorm(vectorX_points4D); 1351 fprintf(file," test 6.01 prev normPnts=%lf\n",norm); 1352 1353 for( int i=0;i<numImages;i++ ) 1354 { 1355 double norm = cvNorm(vectorX_projMatrs[i]); 1356 fprintf(file," test 6.01 prev normProj=%lf\n",norm); 1357 } 1358 1359 fclose(file); 1360 } 1361#endif 1362 1363 /* Compute derivates by projectinon matrices */ 1364 icvComputeDerivateProjAll(vectorX_points4D,vectorX_projMatrs,pointsPres,numImages,DerivProj); 1365 1366 /* Compute derivates by 4D points */ 1367 icvComputeDerivatePointsAll(vectorX_points4D,vectorX_projMatrs,pointsPres,numImages,DerivPoint); 1368 1369 /* Compute matrces Uk */ 1370 icvComputeMatrixUAll(numImages,DerivProj,matrsUk); 1371 icvComputeMatrixVAll(numImages,DerivPoint,pointsPres,matrsVi); 1372 icvComputeMatrixW(numImages,DerivProj,DerivPoint,pointsPres,matrW); 1373 1374 1375#ifdef TRACK_BUNDLE 1376 { 1377 FILE *file; 1378 file = fopen( TRACK_BUNDLE_FILE ,"a"); 1379 fprintf(file,"\n----- test 6.03 do matrs U V -----\n"); 1380 1381 int i; 1382 for( i = 0; i < numImages; i++ ) 1383 { 1384 double norm = cvNorm(matrsUk[i]); 1385 fprintf(file," test 6.01 prev matrsUk=%lf\n",norm); 1386 } 1387 1388 for( i = 0; i < numPoints; i++ ) 1389 { 1390 double norm = cvNorm(matrsVi[i]); 1391 fprintf(file," test 6.01 prev matrsVi=%lf\n",norm); 1392 } 1393 1394 fclose(file); 1395 } 1396#endif 1397 1398 /* Compute jac errors */ 1399 icvComputeJacErrorProj(numImages, DerivProj, errorProjPoints, jacProjErr); 1400 icvComputeJacErrorPoint(numImages, DerivPoint, errorProjPoints, pointsPres, jacPointErr); 1401 1402#ifdef TRACK_BUNDLE 1403 { 1404 FILE *file; 1405 file = fopen( TRACK_BUNDLE_FILE ,"a"); 1406 fprintf(file,"\n----- test 6 do main -----\n"); 1407 double norm1 = cvNorm(vectorX_points4D); 1408 fprintf(file," test 6.02 post normPnts=%lf\n",norm1); 1409 fclose(file); 1410 } 1411#endif 1412 /* Copy matrices Uk to work matrices Uk */ 1413 for( currImage = 0; currImage < numImages; currImage++ ) 1414 { 1415 cvCopy(matrsUk[currImage],workMatrsUk[currImage]); 1416 } 1417 1418#ifdef TRACK_BUNDLE 1419 { 1420 FILE *file; 1421 file = fopen( TRACK_BUNDLE_FILE ,"a"); 1422 fprintf(file,"\n----- test 60.3 do matrs U V -----\n"); 1423 1424 int i; 1425 for( i = 0; i < numImages; i++ ) 1426 { 1427 double norm = cvNorm(matrsUk[i]); 1428 fprintf(file," test 6.01 post1 matrsUk=%lf\n",norm); 1429 } 1430 1431 for( i = 0; i < numPoints; i++ ) 1432 { 1433 double norm = cvNorm(matrsVi[i]); 1434 fprintf(file," test 6.01 post1 matrsVi=%lf\n",norm); 1435 } 1436 1437 fclose(file); 1438 } 1439#endif 1440 1441 /* ========== Solve normal equation for given alpha and Jacobian ============ */ 1442 1443 do 1444 { 1445 /* ---- Add alpha to matrices --- */ 1446 /* Add alpha to matrInvVi and make workMatrsInvVi */ 1447 1448 int currV; 1449 for( currV = 0; currV < numPoints; currV++ ) 1450 { 1451 cvCopy(matrsVi[currV],workMatrVi); 1452 1453 for( int i = 0; i < 4; i++ ) 1454 { 1455 cvmSet(workMatrVi,i,i,cvmGet(matrsVi[currV],i,i)*(1+alpha) ); 1456 } 1457 1458 cvInvert(workMatrVi,workMatrsInvVi[currV],CV_LU/*,&currV*/); 1459 } 1460 1461 /* Add alpha to matrUk and make matrix workMatrsUk */ 1462 for( currImage = 0; currImage< numImages; currImage++ ) 1463 { 1464 1465 for( i = 0; i < 12; i++ ) 1466 { 1467 cvmSet(workMatrsUk[currImage],i,i,cvmGet(matrsUk[currImage],i,i)*(1+alpha)); 1468 } 1469 1470 1471 } 1472 1473 /* Fill matrix to make system for computing delta P (matrTmpSys1 = inv(V)*tr(W) )*/ 1474 for( currV = 0; currV < numPoints; currV++ ) 1475 { 1476 int currRowV; 1477 for( currRowV = 0; currRowV < 4; currRowV++ ) 1478 { 1479 for( currImage = 0; currImage < numImages; currImage++ ) 1480 { 1481 for( int currCol = 0; currCol < 12; currCol++ )/* For each column of transposed matrix W */ 1482 { 1483 double sum = 0; 1484 for( i = 0; i < 4; i++ ) 1485 { 1486 sum += cvmGet(workMatrsInvVi[currV],currRowV,i) * 1487 cvmGet(matrW,currImage*12+currCol,currV*4+i); 1488 } 1489 cvmSet(matrTmpSys1,currV*4+currRowV,currImage*12+currCol,sum); 1490 } 1491 } 1492 } 1493 } 1494 1495 1496 /* Fill matrix to make system for computing delta P (matrTmpSys2 = W * matrTmpSys ) */ 1497 cvmMul(matrW,matrTmpSys1,matrSysDeltaP); 1498 1499 /* need to compute U-matrTmpSys2. But we compute matTmpSys2-U */ 1500 for( currImage = 0; currImage < numImages; currImage++ ) 1501 { 1502 CvMat subMatr; 1503 cvGetSubRect(matrSysDeltaP,&subMatr,cvRect(currImage*12,currImage*12,12,12)); 1504 cvSub(&subMatr,workMatrsUk[currImage],&subMatr); 1505 } 1506 1507 /* Compute right side of normal equation */ 1508 for( currV = 0; currV < numPoints; currV++ ) 1509 { 1510 CvMat subMatrErPnts; 1511 CvMat subMatr; 1512 cvGetSubRect(jacPointErr,&subMatrErPnts,cvRect(0,currV*4,1,4)); 1513 cvGetSubRect(vectTmpSys3,&subMatr,cvRect(0,currV*4,1,4)); 1514 cvmMul(workMatrsInvVi[currV],&subMatrErPnts,&subMatr); 1515 } 1516 1517 cvmMul(matrW,vectTmpSys3,vectSysDeltaP); 1518 cvSub(vectSysDeltaP,jacProjErr,vectSysDeltaP); 1519 1520 /* Now we can compute part of normal system - deltaP */ 1521 cvSolve(matrSysDeltaP ,vectSysDeltaP, deltaP, CV_SVD); 1522 1523 /* Print deltaP to file */ 1524 1525#ifdef TRACK_BUNDLE 1526 { 1527 FILE* file; 1528 file = fopen( TRACK_BUNDLE_FILE_DELTAP ,"w"); 1529 1530 int currImage; 1531 for( currImage = 0; currImage < numImages; currImage++ ) 1532 { 1533 fprintf(file,"\nImage=%d\n",currImage); 1534 int i; 1535 for( i = 0; i < 12; i++ ) 1536 { 1537 double val; 1538 val = cvmGet(deltaP,currImage*12+i,0); 1539 fprintf(file,"%lf\n",val); 1540 } 1541 fprintf(file,"\n"); 1542 } 1543 fclose(file); 1544 } 1545#endif 1546 /* We know deltaP and now we can compute system for deltaM */ 1547 for( i = 0; i < numPoints * 4; i++ ) 1548 { 1549 double sum = 0; 1550 for( int j = 0; j < numImages * 12; j++ ) 1551 { 1552 sum += cvmGet(matrW,j,i) * cvmGet(deltaP,j,0); 1553 } 1554 cvmSet(vectTmpSysM,i,0,cvmGet(jacPointErr,i,0)-sum); 1555 } 1556 1557 /* Compute deltaM */ 1558 for( currV = 0; currV < numPoints; currV++ ) 1559 { 1560 CvMat subMatr; 1561 CvMat subMatrM; 1562 cvGetSubRect(vectTmpSysM,&subMatr,cvRect(0,currV*4,1,4)); 1563 cvGetSubRect(deltaM,&subMatrM,cvRect(0,currV*4,1,4)); 1564 cvmMul(workMatrsInvVi[currV],&subMatr,&subMatrM); 1565 } 1566 1567 /* We know delta and compute new value of vector X: nextVectX = vectX + deltas */ 1568 1569 /* Compute new P */ 1570 for( currImage = 0; currImage < numImages; currImage++ ) 1571 { 1572 for( i = 0; i < 3; i++ ) 1573 { 1574 for( int j = 0; j < 4; j++ ) 1575 { 1576 cvmSet(newVectorX_projMatrs[currImage],i,j, 1577 cvmGet(vectorX_projMatrs[currImage],i,j) + cvmGet(deltaP,currImage*12+i*4+j,0)); 1578 } 1579 } 1580 } 1581 1582 /* Compute new M */ 1583 int currPoint; 1584 for( currPoint = 0; currPoint < numPoints; currPoint++ ) 1585 { 1586 for( i = 0; i < 4; i++ ) 1587 { 1588 cvmSet(newVectorX_points4D,i,currPoint, 1589 cvmGet(vectorX_points4D,i,currPoint) + cvmGet(deltaM,currPoint*4+i,0)); 1590 } 1591 } 1592 1593 /* ----- Compute errors for new vectorX ----- */ 1594 /* Project points using new vectorX and status of each point */ 1595 icvProjPointsStatusFunc(numImages, newVectorX_points4D, newVectorX_projMatrs, pointsPres, projVisPoints); 1596 /* Compute error with observed value and computed projection */ 1597 double newError = 0; 1598 for( currImage = 0; currImage < numImages; currImage++ ) 1599 { 1600 cvSub(observVisPoints[currImage],projVisPoints[currImage],errorProjPoints[currImage]); 1601 double currNorm = cvNorm(errorProjPoints[currImage]); 1602 1603//#ifdef TRACK_BUNDLE 1604#if 1 1605 { 1606 FILE *file; 1607 file = fopen( TRACK_BUNDLE_FILE ,"a"); 1608 fprintf(file,"\n----- test 13,01 currImage=%d currNorm=%lf -----\n",currImage,currNorm); 1609 fclose(file); 1610 } 1611#endif 1612 newError += currNorm * currNorm; 1613 } 1614 newError = sqrt(newError); 1615 1616 currIter++; 1617 1618 1619 1620 1621//#ifdef TRACK_BUNDLE 1622#if 1 1623 { 1624 /* Create file to track */ 1625 FILE* file; 1626 file = fopen( TRACK_BUNDLE_FILE ,"a"); 1627 fprintf(file,"\n========================================\n"); 1628 fprintf(file,"numPoints=%d\n",numPoints); 1629 fprintf(file,"Iter=%d\n",currIter); 1630 fprintf(file,"Error = %20.15lf\n",newError); 1631 fprintf(file,"Change = %20.15lf\n",change); 1632 1633 1634 /* Print all projection errors */ 1635#if 0 1636 fprintf(file,"projection errors\n"); 1637 int currImage; 1638 for( currImage = 0; currImage < numImages; currImage++) 1639 { 1640 fprintf(file,"\nImage=%d\n",currImage); 1641 int numPn = errorProjPoints[currImage]->cols; 1642 for( int currPoint = 0; currPoint < numPn; currPoint++ ) 1643 { 1644 double ex,ey; 1645 ex = cvmGet(errorProjPoints[currImage],0,currPoint); 1646 ey = cvmGet(errorProjPoints[currImage],1,currPoint); 1647 fprintf(file,"%lf,%lf\n",ex,ey); 1648 } 1649 } 1650 fprintf(file,"\n---- test 0 -----\n"); 1651#endif 1652 1653 fclose(file); 1654 } 1655#endif 1656 1657 1658 1659 /* Compare new error and last error */ 1660 if( newError < prevError ) 1661 {/* accept new value */ 1662 prevError = newError; 1663 /* Compute relative change of required parameter vectorX. change = norm(curr-prev) / norm(curr) ) */ 1664 { 1665 double normAll1 = 0; 1666 double normAll2 = 0; 1667 double currNorm1 = 0; 1668 double currNorm2 = 0; 1669 /* compute norm for projection matrices */ 1670 for( currImage = 0; currImage < numImages; currImage++ ) 1671 { 1672 currNorm1 = cvNorm(newVectorX_projMatrs[currImage],vectorX_projMatrs[currImage]); 1673 currNorm2 = cvNorm(newVectorX_projMatrs[currImage]); 1674 1675 normAll1 += currNorm1 * currNorm1; 1676 normAll2 += currNorm2 * currNorm2; 1677 } 1678 1679 /* compute norm for points */ 1680 currNorm1 = cvNorm(newVectorX_points4D,vectorX_points4D); 1681 currNorm2 = cvNorm(newVectorX_points4D); 1682 1683 normAll1 += currNorm1 * currNorm1; 1684 normAll2 += currNorm2 * currNorm2; 1685 1686 /* compute change */ 1687 change = sqrt(normAll1) / sqrt(normAll2); 1688 1689 1690//#ifdef TRACK_BUNDLE 1691#if 1 1692 { 1693 /* Create file to track */ 1694 FILE* file; 1695 file = fopen( TRACK_BUNDLE_FILE ,"a"); 1696 fprintf(file,"\nChange inside newVal change = %20.15lf\n",change); 1697 fprintf(file," normAll1= %20.15lf\n",sqrt(normAll1)); 1698 fprintf(file," normAll2= %20.15lf\n",sqrt(normAll2)); 1699 1700 fclose(file); 1701 } 1702#endif 1703 1704 } 1705 1706 alpha /= 10; 1707 for( currImage = 0; currImage < numImages; currImage++ ) 1708 { 1709 cvCopy(newVectorX_projMatrs[currImage],vectorX_projMatrs[currImage]); 1710 } 1711 cvCopy(newVectorX_points4D,vectorX_points4D); 1712 1713 break; 1714 } 1715 else 1716 { 1717 alpha *= 10; 1718 } 1719 1720 } while( change > epsilon && currIter < maxIter );/* solve normal equation using current alpha */ 1721 1722//#ifdef TRACK_BUNDLE 1723#if 1 1724 { 1725 FILE* file; 1726 file = fopen( TRACK_BUNDLE_FILE ,"a"); 1727 fprintf(file,"\nBest error = %40.35lf\n",prevError); 1728 fclose(file); 1729 } 1730 1731#endif 1732 1733 1734 } while( change > epsilon && currIter < maxIter ); 1735 1736 /*--------------------------------------------*/ 1737 /* Optimization complete copy computed params */ 1738 /* Copy projection matrices */ 1739 for( currImage = 0; currImage < numImages; currImage++ ) 1740 { 1741 cvCopy(newVectorX_projMatrs[currImage],resultProjMatrs[currImage]); 1742 } 1743 /* Copy 4D points */ 1744 cvCopy(newVectorX_points4D,resultPoints4D); 1745 1746// free(memory); 1747 1748 __END__; 1749 1750 /* Free allocated memory */ 1751 1752 /* Free simple matrices */ 1753 cvFree(&vectorX_points4D); 1754 cvFree(&newVectorX_points4D); 1755 cvFree(&changeVectorX_points4D); 1756 cvFree(&changeVectorX_projMatrs); 1757 cvFree(&matrW); 1758 cvFree(&workMatrVi); 1759 cvFree(&jacProjErr); 1760 cvFree(&jacPointErr); 1761 cvFree(&matrTmpSys1); 1762 cvFree(&matrSysDeltaP); 1763 cvFree(&vectTmpSys3); 1764 cvFree(&vectSysDeltaP); 1765 cvFree(&deltaP); 1766 cvFree(&deltaM); 1767 cvFree(&vectTmpSysM); 1768 1769 /* Free arrays of matrices */ 1770 icvFreeMatrixArray(&vectorX_projMatrs,numImages); 1771 icvFreeMatrixArray(&newVectorX_projMatrs,numImages); 1772 icvFreeMatrixArray(&observVisPoints,numImages); 1773 icvFreeMatrixArray(&projVisPoints,numImages); 1774 icvFreeMatrixArray(&errorProjPoints,numImages); 1775 icvFreeMatrixArray(&DerivProj,numImages); 1776 icvFreeMatrixArray(&DerivPoint,numImages); 1777 icvFreeMatrixArray(&matrsUk,numImages); 1778 icvFreeMatrixArray(&workMatrsUk,numImages); 1779 icvFreeMatrixArray(&matrsVi,numPoints); 1780 icvFreeMatrixArray(&workMatrsInvVi,numPoints); 1781 1782 return; 1783} 1784