1 #ifndef CAMERAPROJECTIONMATH_H 2 #define CAMERAPROJECTIONMATH_H 10 template<
typename Type>
11 Eigen::Matrix<Type, 3, 3> dropColumn(
const Eigen::Matrix<Type, 3, 4> &projectionMatrix,
unsigned col)
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);
26 template<
typename Type>
27 Eigen::Matrix<Type, 3, 1> computeFocalPoint(
const Eigen::Matrix<Type, 3, 4> &projectionMatrix)
30 Eigen::Matrix<Type, 3, 3> M = projectionMatrix.block(0, 0, 3, 3);
32 Type lambda = M.row(2).norm();
33 if (M.determinant() < 0.0f)
36 Eigen::Matrix<Type, 3, 4> scaledProjectionMatrix = projectionMatrix;
37 scaledProjectionMatrix *= (1.0f / lambda);
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());
47 Eigen::Matrix<Type, 3, 1> cameraPosition(
53 return cameraPosition;
57 template<
typename Type,
unsigned Rows,
unsigned Cols>
58 Eigen::Matrix<Type, Rows, Cols> flipud(
const Eigen::Matrix<Type, Rows, Cols> &A)
60 Eigen::Matrix<Type, Rows, Cols> result;
61 for (
unsigned i = 0; i < Rows; i++)
62 result.row(Rows-1-i) = A.row(i);
66 extern void RQDecompose(
const Eigen::Matrix<float, 3, 3> &A, Eigen::Matrix<float, 3, 3> &Q, Eigen::Matrix<float, 3, 3> &R);
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)
98 Eigen::Matrix<Type, 3, 3> M = projectionMatrix.block(0, 0, 3, 3);
100 Type lambda = M.row(2).norm();
101 if (M.determinant() < 0.0f)
104 Eigen::Matrix<Type, 3, 4> scaledProjectionMatrix = projectionMatrix;
105 scaledProjectionMatrix *= (1.0f / lambda);
106 M *= (1.0f / lambda);
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());
114 cameraPosition = Eigen::Matrix<Type, 3, 1>(
120 Eigen::Matrix<Type, 3, 3> R, Q;
121 RQDecompose(M, Q, R);
123 for (
unsigned i = 0; i < 3; i++) {
125 for (
unsigned j = 0; j < 3; j++) {
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)
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;
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;
151 augmentedInternal(2, 0) = 0.0f;
152 augmentedInternal(2, 1) = 0.0f;
153 augmentedInternal(2, 2) = 0.0f;
154 augmentedInternal(2, 3) = 1.0f;
156 augmentedInternal(3, 0) = 0.0f;
157 augmentedInternal(3, 1) = 0.0f;
158 augmentedInternal(3, 2) = 1.0f;
159 augmentedInternal(3, 3) = 0.0f;
161 return augmentedInternal * externalMatrix;
168 template<
typename Type>
169 Eigen::Matrix<Type, 4, 4> augmentProjectionMatrix(
const Eigen::Matrix<Type, 3, 4> &projectionMatrix)
171 Eigen::Matrix<Type, 3, 3> internalMatrix;
172 Eigen::Matrix<Type, 3, 3> cameraRotation;
173 Eigen::Matrix<Type, 3, 1> cameraPosition;
175 decomposeProjectionMatrix(projectionMatrix, internalMatrix, cameraRotation, cameraPosition);
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);
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);
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);
194 external(3, 0) = 0.0f;
195 external(3, 1) = 0.0f;
196 external(3, 2) = 0.0f;
197 external(3, 3) = 1.0f;
199 return composeAugmentedProjectionMatrix<Type>(internalMatrix, external);
205 #endif // CAMERAPROJECTIONMATH_H Definition: CameraPathEvaluation.cpp:10