SyB3R - Synthetic Benchmark for 3D Reconstruction
CameraProjectionMath.h
1 #ifndef CAMERAPROJECTIONMATH_H
2 #define CAMERAPROJECTIONMATH_H
3 
4 #include <Eigen/Dense>
5 //#include <iostream>
6 
7 namespace syb3r {
8 namespace math {
9 
10 template<typename Type>
11 Eigen::Matrix<Type, 3, 3> dropColumn(const Eigen::Matrix<Type, 3, 4> &projectionMatrix, unsigned col)
12 {
13  Eigen::Matrix<Type, 3, 3> P;
14  for (unsigned i = 0; i < 3; i++) {
15  for (unsigned j = 0; j < col; j++)
16  P(i, j) = projectionMatrix(i, j);
17  for (unsigned j = col+1; j < 4; j++)
18  P(i, j-1) = projectionMatrix(i, j);
19  }
20  return P;
21 }
22 
26 template<typename Type>
27 Eigen::Matrix<Type, 3, 1> computeFocalPoint(const Eigen::Matrix<Type, 3, 4> &projectionMatrix)
28 {
29 
30  Eigen::Matrix<Type, 3, 3> M = projectionMatrix.block(0, 0, 3, 3);
31 
32  Type lambda = M.row(2).norm();
33  if (M.determinant() < 0.0f)
34  lambda = -lambda;
35 
36  Eigen::Matrix<Type, 3, 4> scaledProjectionMatrix = projectionMatrix;
37  scaledProjectionMatrix *= (1.0f / lambda);
38  // M *= (1.0f / lambda);
39 
40 
41  Type Cx = (dropColumn(scaledProjectionMatrix, 0).determinant());
42  Type Cy = -(dropColumn(scaledProjectionMatrix, 1).determinant());
43  Type Cz = (dropColumn(scaledProjectionMatrix, 2).determinant());
44  Type Cw = -(dropColumn(scaledProjectionMatrix, 3).determinant());
45 
46 
47  Eigen::Matrix<Type, 3, 1> cameraPosition(
48  Cx / Cw,
49  Cy / Cw,
50  Cz / Cw
51  );
52 
53  return cameraPosition;
54 }
55 
56 
57 template<typename Type, unsigned Rows, unsigned Cols>
58 Eigen::Matrix<Type, Rows, Cols> flipud(const Eigen::Matrix<Type, Rows, Cols> &A)
59 {
60  Eigen::Matrix<Type, Rows, Cols> result;
61  for (unsigned i = 0; i < Rows; i++)
62  result.row(Rows-1-i) = A.row(i);
63  return result;
64 }
65 
66 extern void RQDecompose(const Eigen::Matrix<float, 3, 3> &A, Eigen::Matrix<float, 3, 3> &Q, Eigen::Matrix<float, 3, 3> &R);
67 /*{
68  Eigen::Matrix<float, 3, 3> A_ = flipud<float, 3, 3>(A).transpose();
69  Eigen::HouseholderQR<Eigen::Matrix<float, 3, 3>> qr(A_);
70  Eigen::Matrix<float, 3, 3> R_ = qr.matrixQR().triangularView<Eigen::Upper>();
71 
72  R = flipud<float, 3, 3>(flipud<float, 3, 3>(R_).transpose());
73  Q = flipud<float, 3, 3>(qr.householderQ().transpose());
74 }*/
75 
76 /*
77 
79 template<typename Type, unsigned Dim, class QRDecompose>// = Eigen::FullPivHouseholderQR<Eigen::Matrix<Type, Dim, Dim>>>
80 void RQDecompose(const Eigen::Matrix<Type, Dim, Dim> &A, Eigen::Matrix<Type, Dim, Dim> &Q, Eigen::Matrix<Type, Dim, Dim> &R)
81 {
82  Eigen::Matrix<Type, Dim, Dim> A_ = flipud<Type, Dim, Dim>(A.transpose());
83  //QRDecompose qr(A_);
84  Eigen::FullPivHouseholderQR<Eigen::Matrix<Type, Dim, Dim>> qr(A_);
85  Eigen::Matrix<Type, Dim, Dim> R_ = qr.matrixQR().triangularView<Eigen::Upper>();
86 
87  R = flipud<Type, Dim, Dim>(flipud<Type, Dim, Dim>(R_).transpose());
88  Q = qr.matrixQ().transpose();
89 }
90 */
91 
92 template<typename Type>
93 void decomposeProjectionMatrix(const Eigen::Matrix<Type, 3, 4> &projectionMatrix,
94  Eigen::Matrix<Type, 3, 3> &internalMatrix,
95  Eigen::Matrix<Type, 3, 3> &cameraRotation,
96  Eigen::Matrix<Type, 3, 1> &cameraPosition)
97 {
98  Eigen::Matrix<Type, 3, 3> M = projectionMatrix.block(0, 0, 3, 3);
99 
100  Type lambda = M.row(2).norm();
101  if (M.determinant() < 0.0f)
102  lambda = -lambda;
103 
104  Eigen::Matrix<Type, 3, 4> scaledProjectionMatrix = projectionMatrix;
105  scaledProjectionMatrix *= (1.0f / lambda);
106  M *= (1.0f / lambda);
107 
108 
109  Type Cx = (dropColumn(scaledProjectionMatrix, 0).determinant());
110  Type Cy = -(dropColumn(scaledProjectionMatrix, 1).determinant());
111  Type Cz = (dropColumn(scaledProjectionMatrix, 2).determinant());
112  Type Cw = -(dropColumn(scaledProjectionMatrix, 3).determinant());
113 
114  cameraPosition = Eigen::Matrix<Type, 3, 1>(
115  Cx / Cw,
116  Cy / Cw,
117  Cz / Cw
118  );
119 
120  Eigen::Matrix<Type, 3, 3> R, Q;
121  RQDecompose(M, Q, R);
122 
123  for (unsigned i = 0; i < 3; i++) {
124  if (R(i,i) < 0.0f) {
125  for (unsigned j = 0; j < 3; j++) {
126  R(j, i) = -R(j, i);
127  Q(i,j) = -Q(i,j);
128  }
129  }
130  }
131 
132  internalMatrix = R;
133  cameraRotation = Q;
134 }
135 
136 
137 template<typename Type>
138 Eigen::Matrix<Type, 4, 4> composeAugmentedProjectionMatrix(const Eigen::Matrix<Type, 3, 3> &internalMatrix, const Eigen::Matrix<Type, 4, 4> &externalMatrix)
139 {
140  Eigen::Matrix<Type, 4, 4> augmentedInternal;
141  augmentedInternal(0, 0) = internalMatrix(0, 0) * (1.0f / internalMatrix(2, 2));
142  augmentedInternal(0, 1) = internalMatrix(0, 1) * (1.0f / internalMatrix(2, 2));
143  augmentedInternal(0, 2) = internalMatrix(0, 2) * (1.0f / internalMatrix(2, 2));
144  augmentedInternal(0, 3) = 0.0f;
145 
146  augmentedInternal(1, 0) = internalMatrix(1, 0) * (1.0f / internalMatrix(2, 2));
147  augmentedInternal(1, 1) = internalMatrix(1, 1) * (1.0f / internalMatrix(2, 2));
148  augmentedInternal(1, 2) = internalMatrix(1, 2) * (1.0f / internalMatrix(2, 2));
149  augmentedInternal(1, 3) = 0.0f;
150 
151  augmentedInternal(2, 0) = 0.0f;
152  augmentedInternal(2, 1) = 0.0f;
153  augmentedInternal(2, 2) = 0.0f;
154  augmentedInternal(2, 3) = 1.0f;
155 
156  augmentedInternal(3, 0) = 0.0f;
157  augmentedInternal(3, 1) = 0.0f;
158  augmentedInternal(3, 2) = 1.0f;
159  augmentedInternal(3, 3) = 0.0f;
160 
161  return augmentedInternal * externalMatrix;
162 }
163 
164 
168 template<typename Type>
169 Eigen::Matrix<Type, 4, 4> augmentProjectionMatrix(const Eigen::Matrix<Type, 3, 4> &projectionMatrix)
170 {
171  Eigen::Matrix<Type, 3, 3> internalMatrix;
172  Eigen::Matrix<Type, 3, 3> cameraRotation;
173  Eigen::Matrix<Type, 3, 1> cameraPosition;
174 
175  decomposeProjectionMatrix(projectionMatrix, internalMatrix, cameraRotation, cameraPosition);
176 
177  Eigen::Matrix<Type, 3, 1> revCameraPosition = cameraRotation * cameraPosition;
178  Eigen::Matrix<Type, 4, 4> external;
179  external(0, 0) = cameraRotation(0, 0);
180  external(0, 1) = cameraRotation(0, 1);
181  external(0, 2) = cameraRotation(0, 2);
182  external(0, 3) = -revCameraPosition(0);
183 
184  external(1, 0) = cameraRotation(1, 0);
185  external(1, 1) = cameraRotation(1, 1);
186  external(1, 2) = cameraRotation(1, 2);
187  external(1, 3) = -revCameraPosition(1);
188 
189  external(2, 0) = cameraRotation(2, 0);
190  external(2, 1) = cameraRotation(2, 1);
191  external(2, 2) = cameraRotation(2, 2);
192  external(2, 3) = -revCameraPosition(2);
193 
194  external(3, 0) = 0.0f;
195  external(3, 1) = 0.0f;
196  external(3, 2) = 0.0f;
197  external(3, 3) = 1.0f;
198 
199  return composeAugmentedProjectionMatrix<Type>(internalMatrix, external);
200 }
201 
202 }
203 }
204 
205 #endif // CAMERAPROJECTIONMATH_H
Definition: CameraPathEvaluation.cpp:10