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
51 void 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 ================= */
icvComputeDerivateProj(CvMat * points4D,CvMat * projMatr,CvMat * status,CvMat * derivProj)70 void 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
icvComputeDerivateProjAll(CvMat * points4D,CvMat ** projMatrs,CvMat ** pointPres,int numImages,CvMat ** projDerives)204 void 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
icvComputeDerivatePoints(CvMat * points4D,CvMat * projMatr,CvMat * presPoints,CvMat * derivPoint)231 void 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 /*======================================================================================*/
icvComputeDerivatePointsAll(CvMat * points4D,CvMat ** projMatrs,CvMat ** pointPres,int numImages,CvMat ** pointDerives)341 void 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 /*======================================================================================*/
icvComputeMatrixVAll(int numImages,CvMat ** pointDeriv,CvMat ** presPoints,CvMat ** matrV)367 void 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 /*======================================================================================*/
icvComputeMatrixUAll(int numImages,CvMat ** projDeriv,CvMat ** matrU)436 void 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 /*======================================================================================*/
icvComputeMatrixW(int numImages,CvMat ** projDeriv,CvMat ** pointDeriv,CvMat ** presPoints,CvMat * matrW)463 void 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 */
icvComputeJacErrorProj(int numImages,CvMat ** projDeriv,CvMat ** projErrors,CvMat * jacProjErr)565 void 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 */
icvComputeJacErrorPoint(int numImages,CvMat ** pointDeriv,CvMat ** projErrors,CvMat ** presPoints,CvMat * jacPointErr)632 void 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 */
icvReconstructPoints4DStatus(CvMat ** projPoints,CvMat ** projMatrs,CvMat ** presPoints,CvMat * points4D,int numImages,CvMat ** projError)738 void 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
icvProjPointsStatusFunc(int numImages,CvMat * points4D,CvMat ** projMatrs,CvMat ** pointsPres,CvMat ** projPoints)900 void 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 /*======================================================================================*/
icvFreeMatrixArray(CvMat *** matrArray,int numMatr)1001 void 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 /*======================================================================================*/
icvClearAlloc(int size)1018 void *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
1037 void 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
1043 int 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 */
cvOptimizeLevenbergMarquardtBundle(CvMat ** projMatrs,CvMat ** observProjPoints,CvMat ** pointsPres,int numImages,CvMat ** resultProjMatrs,CvMat * resultPoints4D,int maxIter,double epsilon)1053 void 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