1 // This is brl/bbas/bgui3d/bgui3d_algo.cxx
2 #include "bgui3d_algo.h"
3 //:
4 // \file
5
6 #include <vnl/algo/vnl_qr.h>
7
8 //: decompose the camera into internal and external params
9 bool
bgui3d_decompose_camera(const vnl_double_3x4 & camera,vnl_double_3x3 & internal_calibration,vnl_double_3x3 & rotation,vnl_double_3 & translation)10 bgui3d_decompose_camera( const vnl_double_3x4& camera,
11 vnl_double_3x3& internal_calibration,
12 vnl_double_3x3& rotation,
13 vnl_double_3& translation )
14 {
15 // camera = [H t]
16 //
17 vnl_double_3x3 PermH;
18
19 for (int i = 0; i < 3; ++i)
20 for (int j = 0; j < 3; ++j)
21 PermH(i,j) = camera(2-j,2-i);
22
23 vnl_qr<double> qr(PermH.as_ref());
24
25 vnl_double_3x3 Q = qr.Q();
26 vnl_double_3x3 R = qr.R();
27
28 // Ensure all diagonal components of C are positive.
29 // Must insert a det(+1) or det(-1) mx between.
30 int r0pos = R(0,0) > 0 ? 1 : 0;
31 int r1pos = R(1,1) > 0 ? 1 : 0;
32 int r2pos = R(2,2) > 0 ? 1 : 0;
33 typedef double d3[3];
34 d3 diags[] = { // 1 2 3
35 { -1, -1, -1}, // - - -
36 { 1, -1, -1}, // + - -
37 { -1, 1, -1}, // - + -
38 { 1, 1, -1}, // + + -
39 { -1, -1, 1}, // - - +
40 { 1, -1, 1}, // + - +
41 { -1, 1, 1}, // - + +
42 { 1, 1, 1}, // + + +
43 };
44 int d = r0pos * 4 + r1pos * 2 + r2pos;
45 double* diag = &diags[d][0];
46
47 for (int i = 0; i < 3; ++i)
48 for (int j = 0; j < 3; ++j) {
49 internal_calibration(j,i) = diag[i] * R(2-i,2-j);
50 rotation(j,i) = diag[j] * Q(2-i,2-j);
51 }
52
53 // Compute t' = inv(C) t
54 vnl_double_3 t;
55 for (int i = 0; i < 3; ++i)
56 t[i] = camera(i,3);
57
58 t[2] /= internal_calibration(2,2);
59 t[1] = (t[1] - internal_calibration(1,2)*t[2])/internal_calibration(1,1);
60 t[0] = (t[0] - internal_calibration(0,1)*t[1] - internal_calibration(0,2)*t[2])/internal_calibration(0,0);
61
62 translation = t;
63
64 // Recompose the matrix and compare
65 vnl_double_3x4 Po;
66 Po.set_columns(0,rotation.as_ref());
67 Po.set_column(3,translation);
68 if (((internal_calibration * Po - camera).fro_norm() > 1e-4) ||
69 (internal_calibration(0,0) < 0) ||
70 (internal_calibration(1,1) < 0) ||
71 (internal_calibration(2,2) < 0)) {
72 return false;
73 }
74
75 // Scale the internal calibration matrix such that the bottom right is 1
76 internal_calibration /= internal_calibration(2,2);
77
78 return true;
79 }
80