1 // Copyright (c) 2009 libmv authors.
2 //
3 // Permission is hereby granted, free of charge, to any person obtaining a copy
4 // of this software and associated documentation files (the "Software"), to
5 // deal in the Software without restriction, including without limitation the
6 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7 // sell copies of the Software, and to permit persons to whom the Software is
8 // furnished to do so, subject to the following conditions:
9 //
10 // The above copyright notice and this permission notice shall be included in
11 // all copies or substantial portions of the Software.
12 //
13 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
18 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
19 // IN THE SOFTWARE.
20
21 #include "libmv/multiview/euclidean_resection.h"
22
23 #include <cmath>
24 #include <limits>
25
26 #include <Eigen/SVD>
27 #include <Eigen/Geometry>
28
29 #include "libmv/base/vector.h"
30 #include "libmv/logging/logging.h"
31 #include "libmv/multiview/projection.h"
32
33 namespace libmv {
34 namespace euclidean_resection {
35
36 typedef unsigned int uint;
37
EuclideanResection(const Mat2X & x_camera,const Mat3X & X_world,Mat3 * R,Vec3 * t,ResectionMethod method)38 bool EuclideanResection(const Mat2X &x_camera,
39 const Mat3X &X_world,
40 Mat3 *R, Vec3 *t,
41 ResectionMethod method) {
42 switch (method) {
43 case RESECTION_ANSAR_DANIILIDIS:
44 EuclideanResectionAnsarDaniilidis(x_camera, X_world, R, t);
45 break;
46 case RESECTION_EPNP:
47 return EuclideanResectionEPnP(x_camera, X_world, R, t);
48 break;
49 case RESECTION_PPNP:
50 return EuclideanResectionPPnP(x_camera, X_world, R, t);
51 break;
52 default:
53 LOG(FATAL) << "Unknown resection method.";
54 }
55 return false;
56 }
57
EuclideanResection(const Mat & x_image,const Mat3X & X_world,const Mat3 & K,Mat3 * R,Vec3 * t,ResectionMethod method)58 bool EuclideanResection(const Mat &x_image,
59 const Mat3X &X_world,
60 const Mat3 &K,
61 Mat3 *R, Vec3 *t,
62 ResectionMethod method) {
63 CHECK(x_image.rows() == 2 || x_image.rows() == 3)
64 << "Invalid size for x_image: "
65 << x_image.rows() << "x" << x_image.cols();
66
67 Mat2X x_camera;
68 if (x_image.rows() == 2) {
69 EuclideanToNormalizedCamera(x_image, K, &x_camera);
70 } else if (x_image.rows() == 3) {
71 HomogeneousToNormalizedCamera(x_image, K, &x_camera);
72 }
73 return EuclideanResection(x_camera, X_world, R, t, method);
74 }
75
AbsoluteOrientation(const Mat3X & X,const Mat3X & Xp,Mat3 * R,Vec3 * t)76 void AbsoluteOrientation(const Mat3X &X,
77 const Mat3X &Xp,
78 Mat3 *R,
79 Vec3 *t) {
80 int num_points = X.cols();
81 Vec3 C = X.rowwise().sum() / num_points; // Centroid of X.
82 Vec3 Cp = Xp.rowwise().sum() / num_points; // Centroid of Xp.
83
84 // Normalize the two point sets.
85 Mat3X Xn(3, num_points), Xpn(3, num_points);
86 for (int i = 0; i < num_points; ++i) {
87 Xn.col(i) = X.col(i) - C;
88 Xpn.col(i) = Xp.col(i) - Cp;
89 }
90
91 // Construct the N matrix (pg. 635).
92 double Sxx = Xn.row(0).dot(Xpn.row(0));
93 double Syy = Xn.row(1).dot(Xpn.row(1));
94 double Szz = Xn.row(2).dot(Xpn.row(2));
95 double Sxy = Xn.row(0).dot(Xpn.row(1));
96 double Syx = Xn.row(1).dot(Xpn.row(0));
97 double Sxz = Xn.row(0).dot(Xpn.row(2));
98 double Szx = Xn.row(2).dot(Xpn.row(0));
99 double Syz = Xn.row(1).dot(Xpn.row(2));
100 double Szy = Xn.row(2).dot(Xpn.row(1));
101
102 Mat4 N;
103 N << Sxx + Syy + Szz, Syz - Szy, Szx - Sxz, Sxy - Syx,
104 Syz - Szy, Sxx - Syy - Szz, Sxy + Syx, Szx + Sxz,
105 Szx - Sxz, Sxy + Syx, -Sxx + Syy - Szz, Syz + Szy,
106 Sxy - Syx, Szx + Sxz, Syz + Szy, -Sxx - Syy + Szz;
107
108 // Find the unit quaternion q that maximizes qNq. It is the eigenvector
109 // corresponding to the lagest eigenvalue.
110 Vec4 q = N.jacobiSvd(Eigen::ComputeFullU).matrixU().col(0);
111
112 // Retrieve the 3x3 rotation matrix.
113 Vec4 qq = q.array() * q.array();
114 double q0q1 = q(0) * q(1);
115 double q0q2 = q(0) * q(2);
116 double q0q3 = q(0) * q(3);
117 double q1q2 = q(1) * q(2);
118 double q1q3 = q(1) * q(3);
119 double q2q3 = q(2) * q(3);
120
121 (*R) << qq(0) + qq(1) - qq(2) - qq(3),
122 2 * (q1q2 - q0q3),
123 2 * (q1q3 + q0q2),
124 2 * (q1q2+ q0q3),
125 qq(0) - qq(1) + qq(2) - qq(3),
126 2 * (q2q3 - q0q1),
127 2 * (q1q3 - q0q2),
128 2 * (q2q3 + q0q1),
129 qq(0) - qq(1) - qq(2) + qq(3);
130
131 // Fix the handedness of the R matrix.
132 if (R->determinant() < 0) {
133 R->row(2) = -R->row(2);
134 }
135 // Compute the final translation.
136 *t = Cp - *R * C;
137 }
138
139 // Convert i and j indices of the original variables into their quadratic
140 // permutation single index. It follows that t_ij = t_ji.
IJToPointIndex(int i,int j,int num_points)141 static int IJToPointIndex(int i, int j, int num_points) {
142 // Always make sure that j is bigger than i. This handles t_ij = t_ji.
143 if (j < i) {
144 std::swap(i, j);
145 }
146 int idx;
147 int num_permutation_rows = num_points * (num_points - 1) / 2;
148
149 // All t_ii's are located at the end of the t vector after all t_ij's.
150 if (j == i) {
151 idx = num_permutation_rows + i;
152 } else {
153 int offset = (num_points - i - 1) * (num_points - i) / 2;
154 idx = (num_permutation_rows - offset + j - i - 1);
155 }
156 return idx;
157 };
158
159 // Convert i and j indexes of the solution for lambda to their linear indexes.
IJToIndex(int i,int j,int num_lambda)160 static int IJToIndex(int i, int j, int num_lambda) {
161 if (j < i) {
162 std::swap(i, j);
163 }
164 int A = num_lambda * (num_lambda + 1) / 2;
165 int B = num_lambda - i;
166 int C = B * (B + 1) / 2;
167 int idx = A - C + j - i;
168 return idx;
169 };
170
Sign(double value)171 static int Sign(double value) {
172 return (value < 0) ? -1 : 1;
173 };
174
175 // Organizes a square matrix into a single row constraint on the elements of
176 // Lambda to create the constraints in equation (5) in "Linear Pose Estimation
177 // from Points or Lines", by Ansar, A. and Daniilidis, PAMI 2003. vol. 25, no.
178 // 5.
MatrixToConstraint(const Mat & A,int num_k_columns,int num_lambda)179 static Vec MatrixToConstraint(const Mat &A,
180 int num_k_columns,
181 int num_lambda) {
182 Vec C(num_k_columns);
183 C.setZero();
184 int idx = 0;
185 for (int i = 0; i < num_lambda; ++i) {
186 for (int j = i; j < num_lambda; ++j) {
187 C(idx) = A(i, j);
188 if (i != j) {
189 C(idx) += A(j, i);
190 }
191 ++idx;
192 }
193 }
194 return C;
195 }
196
197 // Normalizes the columns of vectors.
NormalizeColumnVectors(Mat3X * vectors)198 static void NormalizeColumnVectors(Mat3X *vectors) {
199 int num_columns = vectors->cols();
200 for (int i = 0; i < num_columns; ++i) {
201 vectors->col(i).normalize();
202 }
203 }
204
EuclideanResectionAnsarDaniilidis(const Mat2X & x_camera,const Mat3X & X_world,Mat3 * R,Vec3 * t)205 void EuclideanResectionAnsarDaniilidis(const Mat2X &x_camera,
206 const Mat3X &X_world,
207 Mat3 *R,
208 Vec3 *t) {
209 CHECK(x_camera.cols() == X_world.cols());
210 CHECK(x_camera.cols() > 3);
211
212 int num_points = x_camera.cols();
213
214 // Copy the normalized camera coords into 3 vectors and normalize them so
215 // that they are unit vectors from the camera center.
216 Mat3X x_camera_unit(3, num_points);
217 x_camera_unit.block(0, 0, 2, num_points) = x_camera;
218 x_camera_unit.row(2).setOnes();
219 NormalizeColumnVectors(&x_camera_unit);
220
221 int num_m_rows = num_points * (num_points - 1) / 2;
222 int num_tt_variables = num_points * (num_points + 1) / 2;
223 int num_m_columns = num_tt_variables + 1;
224 Mat M(num_m_columns, num_m_columns);
225 M.setZero();
226 Matu ij_index(num_tt_variables, 2);
227
228 // Create the constraint equations for the t_ij variables (7) and arrange
229 // them into the M matrix (8). Also store the initial (i, j) indices.
230 int row = 0;
231 for (int i = 0; i < num_points; ++i) {
232 for (int j = i+1; j < num_points; ++j) {
233 M(row, row) = -2 * x_camera_unit.col(i).dot(x_camera_unit.col(j));
234 M(row, num_m_rows + i) = x_camera_unit.col(i).dot(x_camera_unit.col(i));
235 M(row, num_m_rows + j) = x_camera_unit.col(j).dot(x_camera_unit.col(j));
236 Vec3 Xdiff = X_world.col(i) - X_world.col(j);
237 double center_to_point_distance = Xdiff.norm();
238 M(row, num_m_columns - 1) =
239 - center_to_point_distance * center_to_point_distance;
240 ij_index(row, 0) = i;
241 ij_index(row, 1) = j;
242 ++row;
243 }
244 ij_index(i + num_m_rows, 0) = i;
245 ij_index(i + num_m_rows, 1) = i;
246 }
247
248 int num_lambda = num_points + 1; // Dimension of the null space of M.
249 Mat V = M.jacobiSvd(Eigen::ComputeFullV).matrixV().block(0,
250 num_m_rows,
251 num_m_columns,
252 num_lambda);
253
254 // TODO(vess): The number of constraint equations in K (num_k_rows) must be
255 // (num_points + 1) * (num_points + 2)/2. This creates a performance issue
256 // for more than 4 points. It is fine for 4 points at the moment with 18
257 // instead of 15 equations.
258 int num_k_rows = num_m_rows + num_points *
259 (num_points*(num_points-1)/2 - num_points+1);
260 int num_k_columns = num_lambda * (num_lambda + 1) / 2;
261 Mat K(num_k_rows, num_k_columns);
262 K.setZero();
263
264 // Construct the first part of the K matrix corresponding to (t_ii, t_jk) for
265 // i != j.
266 int counter_k_row = 0;
267 for (int idx1 = num_m_rows; idx1 < num_tt_variables; ++idx1) {
268 for (int idx2 = 0; idx2 < num_m_rows; ++idx2) {
269 unsigned int i = ij_index(idx1, 0);
270 unsigned int j = ij_index(idx2, 0);
271 unsigned int k = ij_index(idx2, 1);
272
273 if (i != j && i != k) {
274 int idx3 = IJToPointIndex(i, j, num_points);
275 int idx4 = IJToPointIndex(i, k, num_points);
276
277 K.row(counter_k_row) =
278 MatrixToConstraint(V.row(idx1).transpose() * V.row(idx2)-
279 V.row(idx3).transpose() * V.row(idx4),
280 num_k_columns,
281 num_lambda);
282 ++counter_k_row;
283 }
284 }
285 }
286
287 // Construct the second part of the K matrix corresponding to (t_ii,t_jk) for
288 // j==k.
289 for (int idx1 = num_m_rows; idx1 < num_tt_variables; ++idx1) {
290 for (int idx2 = idx1 + 1; idx2 < num_tt_variables; ++idx2) {
291 unsigned int i = ij_index(idx1, 0);
292 unsigned int j = ij_index(idx2, 0);
293 unsigned int k = ij_index(idx2, 1);
294
295 int idx3 = IJToPointIndex(i, j, num_points);
296 int idx4 = IJToPointIndex(i, k, num_points);
297
298 K.row(counter_k_row) =
299 MatrixToConstraint(V.row(idx1).transpose() * V.row(idx2)-
300 V.row(idx3).transpose() * V.row(idx4),
301 num_k_columns,
302 num_lambda);
303 ++counter_k_row;
304 }
305 }
306 Vec L_sq = K.jacobiSvd(Eigen::ComputeFullV).matrixV().col(num_k_columns - 1);
307
308 // Pivot on the largest element for numerical stability. Afterwards recover
309 // the sign of the lambda solution.
310 double max_L_sq_value = fabs(L_sq(IJToIndex(0, 0, num_lambda)));
311 int max_L_sq_index = 1;
312 for (int i = 1; i < num_lambda; ++i) {
313 double abs_sq_value = fabs(L_sq(IJToIndex(i, i, num_lambda)));
314 if (max_L_sq_value < abs_sq_value) {
315 max_L_sq_value = abs_sq_value;
316 max_L_sq_index = i;
317 }
318 }
319 // Ensure positiveness of the largest value corresponding to lambda_ii.
320 L_sq = L_sq * Sign(L_sq(IJToIndex(max_L_sq_index,
321 max_L_sq_index,
322 num_lambda)));
323
324 Vec L(num_lambda);
325 L(max_L_sq_index) = sqrt(L_sq(IJToIndex(max_L_sq_index,
326 max_L_sq_index,
327 num_lambda)));
328
329 for (int i = 0; i < num_lambda; ++i) {
330 if (i != max_L_sq_index) {
331 L(i) = L_sq(IJToIndex(max_L_sq_index, i, num_lambda)) / L(max_L_sq_index);
332 }
333 }
334
335 // Correct the scale using the fact that the last constraint is equal to 1.
336 L = L / (V.row(num_m_columns - 1).dot(L));
337 Vec X = V * L;
338
339 // Recover the distances from the camera center to the 3D points Q.
340 Vec d(num_points);
341 d.setZero();
342 for (int c_point = num_m_rows; c_point < num_tt_variables; ++c_point) {
343 d(c_point - num_m_rows) = sqrt(X(c_point));
344 }
345
346 // Create the 3D points in the camera system.
347 Mat X_cam(3, num_points);
348 for (int c_point = 0; c_point < num_points; ++c_point) {
349 X_cam.col(c_point) = d(c_point) * x_camera_unit.col(c_point);
350 }
351 // Recover the camera translation and rotation.
352 AbsoluteOrientation(X_world, X_cam, R, t);
353 }
354
355 // Selects 4 virtual control points using mean and PCA.
SelectControlPoints(const Mat3X & X_world,Mat * X_centered,Mat34 * X_control_points)356 static void SelectControlPoints(const Mat3X &X_world,
357 Mat *X_centered,
358 Mat34 *X_control_points) {
359 size_t num_points = X_world.cols();
360
361 // The first virtual control point, C0, is the centroid.
362 Vec mean, variance;
363 MeanAndVarianceAlongRows(X_world, &mean, &variance);
364 X_control_points->col(0) = mean;
365
366 // Computes PCA
367 X_centered->resize(3, num_points);
368 for (size_t c = 0; c < num_points; c++) {
369 X_centered->col(c) = X_world.col(c) - mean;
370 }
371 Mat3 X_centered_sq = (*X_centered) * X_centered->transpose();
372 Eigen::JacobiSVD<Mat3> X_centered_sq_svd(X_centered_sq, Eigen::ComputeFullU);
373 Vec3 w = X_centered_sq_svd.singularValues();
374 Mat3 u = X_centered_sq_svd.matrixU();
375 for (size_t c = 0; c < 3; c++) {
376 double k = sqrt(w(c) / num_points);
377 X_control_points->col(c + 1) = mean + k * u.col(c);
378 }
379 }
380
381 // Computes the barycentric coordinates for all real points
ComputeBarycentricCoordinates(const Mat3X & X_world_centered,const Mat34 & X_control_points,Mat4X * alphas)382 static void ComputeBarycentricCoordinates(const Mat3X &X_world_centered,
383 const Mat34 &X_control_points,
384 Mat4X *alphas) {
385 size_t num_points = X_world_centered.cols();
386 Mat3 C2;
387 for (size_t c = 1; c < 4; c++) {
388 C2.col(c-1) = X_control_points.col(c) - X_control_points.col(0);
389 }
390
391 Mat3 C2inv = C2.inverse();
392 Mat3X a = C2inv * X_world_centered;
393
394 alphas->resize(4, num_points);
395 alphas->setZero();
396 alphas->block(1, 0, 3, num_points) = a;
397 for (size_t c = 0; c < num_points; c++) {
398 (*alphas)(0, c) = 1.0 - alphas->col(c).sum();
399 }
400 }
401
402 // Estimates the coordinates of all real points in the camera coordinate frame
ComputePointsCoordinatesInCameraFrame(const Mat4X & alphas,const Vec4 & betas,const Eigen::Matrix<double,12,12> & U,Mat3X * X_camera)403 static void ComputePointsCoordinatesInCameraFrame(
404 const Mat4X &alphas,
405 const Vec4 &betas,
406 const Eigen::Matrix<double, 12, 12> &U,
407 Mat3X *X_camera) {
408 size_t num_points = alphas.cols();
409
410 // Estimates the control points in the camera reference frame.
411 Mat34 C2b; C2b.setZero();
412 for (size_t cu = 0; cu < 4; cu++) {
413 for (size_t c = 0; c < 4; c++) {
414 C2b.col(c) += betas(cu) * U.block(11 - cu, c * 3, 1, 3).transpose();
415 }
416 }
417
418 // Estimates the 3D points in the camera reference frame
419 X_camera->resize(3, num_points);
420 for (size_t c = 0; c < num_points; c++) {
421 X_camera->col(c) = C2b * alphas.col(c);
422 }
423
424 // Check the sign of the z coordinate of the points (should be positive)
425 uint num_z_neg = 0;
426 for (size_t i = 0; i < X_camera->cols(); ++i) {
427 if ((*X_camera)(2, i) < 0) {
428 num_z_neg++;
429 }
430 }
431
432 // If more than 50% of z are negative, we change the signs
433 if (num_z_neg > 0.5 * X_camera->cols()) {
434 C2b = -C2b;
435 *X_camera = -(*X_camera);
436 }
437 }
438
EuclideanResectionEPnP(const Mat2X & x_camera,const Mat3X & X_world,Mat3 * R,Vec3 * t)439 bool EuclideanResectionEPnP(const Mat2X &x_camera,
440 const Mat3X &X_world,
441 Mat3 *R, Vec3 *t) {
442 CHECK(x_camera.cols() == X_world.cols());
443 CHECK(x_camera.cols() > 3);
444 size_t num_points = X_world.cols();
445
446 // Select the control points.
447 Mat34 X_control_points;
448 Mat X_centered;
449 SelectControlPoints(X_world, &X_centered, &X_control_points);
450
451 // Compute the barycentric coordinates.
452 Mat4X alphas(4, num_points);
453 ComputeBarycentricCoordinates(X_centered, X_control_points, &alphas);
454
455 // Estimates the M matrix with the barycentric coordinates
456 Mat M(2 * num_points, 12);
457 Eigen::Matrix<double, 2, 12> sub_M;
458 for (size_t c = 0; c < num_points; c++) {
459 double a0 = alphas(0, c);
460 double a1 = alphas(1, c);
461 double a2 = alphas(2, c);
462 double a3 = alphas(3, c);
463 double ui = x_camera(0, c);
464 double vi = x_camera(1, c);
465 M.block(2*c, 0, 2, 12) << a0, 0,
466 a0*(-ui), a1, 0,
467 a1*(-ui), a2, 0,
468 a2*(-ui), a3, 0,
469 a3*(-ui), 0,
470 a0, a0*(-vi), 0,
471 a1, a1*(-vi), 0,
472 a2, a2*(-vi), 0,
473 a3, a3*(-vi);
474 }
475
476 // TODO(julien): Avoid the transpose by rewriting the u2.block() calls.
477 Eigen::JacobiSVD<Mat> MtMsvd(M.transpose()*M, Eigen::ComputeFullU);
478 Eigen::Matrix<double, 12, 12> u2 = MtMsvd.matrixU().transpose();
479
480 // Estimate the L matrix.
481 Eigen::Matrix<double, 6, 3> dv1;
482 Eigen::Matrix<double, 6, 3> dv2;
483 Eigen::Matrix<double, 6, 3> dv3;
484 Eigen::Matrix<double, 6, 3> dv4;
485
486 dv1.row(0) = u2.block(11, 0, 1, 3) - u2.block(11, 3, 1, 3);
487 dv1.row(1) = u2.block(11, 0, 1, 3) - u2.block(11, 6, 1, 3);
488 dv1.row(2) = u2.block(11, 0, 1, 3) - u2.block(11, 9, 1, 3);
489 dv1.row(3) = u2.block(11, 3, 1, 3) - u2.block(11, 6, 1, 3);
490 dv1.row(4) = u2.block(11, 3, 1, 3) - u2.block(11, 9, 1, 3);
491 dv1.row(5) = u2.block(11, 6, 1, 3) - u2.block(11, 9, 1, 3);
492 dv2.row(0) = u2.block(10, 0, 1, 3) - u2.block(10, 3, 1, 3);
493 dv2.row(1) = u2.block(10, 0, 1, 3) - u2.block(10, 6, 1, 3);
494 dv2.row(2) = u2.block(10, 0, 1, 3) - u2.block(10, 9, 1, 3);
495 dv2.row(3) = u2.block(10, 3, 1, 3) - u2.block(10, 6, 1, 3);
496 dv2.row(4) = u2.block(10, 3, 1, 3) - u2.block(10, 9, 1, 3);
497 dv2.row(5) = u2.block(10, 6, 1, 3) - u2.block(10, 9, 1, 3);
498 dv3.row(0) = u2.block(9, 0, 1, 3) - u2.block(9, 3, 1, 3);
499 dv3.row(1) = u2.block(9, 0, 1, 3) - u2.block(9, 6, 1, 3);
500 dv3.row(2) = u2.block(9, 0, 1, 3) - u2.block(9, 9, 1, 3);
501 dv3.row(3) = u2.block(9, 3, 1, 3) - u2.block(9, 6, 1, 3);
502 dv3.row(4) = u2.block(9, 3, 1, 3) - u2.block(9, 9, 1, 3);
503 dv3.row(5) = u2.block(9, 6, 1, 3) - u2.block(9, 9, 1, 3);
504 dv4.row(0) = u2.block(8, 0, 1, 3) - u2.block(8, 3, 1, 3);
505 dv4.row(1) = u2.block(8, 0, 1, 3) - u2.block(8, 6, 1, 3);
506 dv4.row(2) = u2.block(8, 0, 1, 3) - u2.block(8, 9, 1, 3);
507 dv4.row(3) = u2.block(8, 3, 1, 3) - u2.block(8, 6, 1, 3);
508 dv4.row(4) = u2.block(8, 3, 1, 3) - u2.block(8, 9, 1, 3);
509 dv4.row(5) = u2.block(8, 6, 1, 3) - u2.block(8, 9, 1, 3);
510
511 Eigen::Matrix<double, 6, 10> L;
512 for (size_t r = 0; r < 6; r++) {
513 L.row(r) << dv1.row(r).dot(dv1.row(r)),
514 2.0 * dv1.row(r).dot(dv2.row(r)),
515 dv2.row(r).dot(dv2.row(r)),
516 2.0 * dv1.row(r).dot(dv3.row(r)),
517 2.0 * dv2.row(r).dot(dv3.row(r)),
518 dv3.row(r).dot(dv3.row(r)),
519 2.0 * dv1.row(r).dot(dv4.row(r)),
520 2.0 * dv2.row(r).dot(dv4.row(r)),
521 2.0 * dv3.row(r).dot(dv4.row(r)),
522 dv4.row(r).dot(dv4.row(r));
523 }
524 Vec6 rho;
525 rho << (X_control_points.col(0) - X_control_points.col(1)).squaredNorm(),
526 (X_control_points.col(0) - X_control_points.col(2)).squaredNorm(),
527 (X_control_points.col(0) - X_control_points.col(3)).squaredNorm(),
528 (X_control_points.col(1) - X_control_points.col(2)).squaredNorm(),
529 (X_control_points.col(1) - X_control_points.col(3)).squaredNorm(),
530 (X_control_points.col(2) - X_control_points.col(3)).squaredNorm();
531
532 // There are three possible solutions based on the three approximations of L
533 // (betas). Below, each one is solved for then the best one is chosen.
534 Mat3X X_camera;
535 Mat3 K; K.setIdentity();
536 vector<Mat3> Rs(3);
537 vector<Vec3> ts(3);
538 Vec rmse(3);
539
540 // At one point this threshold was 1e-3, and caused no end of problems in
541 // Blender by causing frames to not resect when they would have worked fine.
542 // When the resect failed, the projective followup is run leading to worse
543 // results, and often the dreaded "flipping" where objects get flipped
544 // between frames. Instead, disable the check for now, always succeeding. The
545 // ultimate check is always reprojection error anyway.
546 //
547 // TODO(keir): Decide if setting this to infinity, effectively disabling the
548 // check, is the right approach. So far this seems the case.
549 double kSuccessThreshold = std::numeric_limits<double>::max();
550
551 // Find the first possible solution for R, t corresponding to:
552 // Betas = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33]
553 // Betas_approx_1 = [b00 b01 b02 b03]
554 Vec4 betas = Vec4::Zero();
555 Eigen::Matrix<double, 6, 4> l_6x4;
556 for (size_t r = 0; r < 6; r++) {
557 l_6x4.row(r) << L(r, 0), L(r, 1), L(r, 3), L(r, 6);
558 }
559 Eigen::JacobiSVD<Mat> svd_of_l4(l_6x4,
560 Eigen::ComputeFullU | Eigen::ComputeFullV);
561 Vec4 b4 = svd_of_l4.solve(rho);
562 if ((l_6x4 * b4).isApprox(rho, kSuccessThreshold)) {
563 if (b4(0) < 0) {
564 b4 = -b4;
565 }
566 b4(0) = std::sqrt(b4(0));
567 betas << b4(0), b4(1) / b4(0), b4(2) / b4(0), b4(3) / b4(0);
568 ComputePointsCoordinatesInCameraFrame(alphas, betas, u2, &X_camera);
569 AbsoluteOrientation(X_world, X_camera, &Rs[0], &ts[0]);
570 rmse(0) = RootMeanSquareError(x_camera, X_world, K, Rs[0], ts[0]);
571 } else {
572 LOG(ERROR) << "First approximation of beta not good enough.";
573 ts[0].setZero();
574 rmse(0) = std::numeric_limits<double>::max();
575 }
576
577 // Find the second possible solution for R, t corresponding to:
578 // Betas = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33]
579 // Betas_approx_2 = [b00 b01 b11]
580 betas.setZero();
581 Eigen::Matrix<double, 6, 3> l_6x3;
582 l_6x3 = L.block(0, 0, 6, 3);
583 Eigen::JacobiSVD<Mat> svdOfL3(l_6x3,
584 Eigen::ComputeFullU | Eigen::ComputeFullV);
585 Vec3 b3 = svdOfL3.solve(rho);
586 VLOG(2) << " rho = " << rho;
587 VLOG(2) << " l_6x3 * b3 = " << l_6x3 * b3;
588 if ((l_6x3 * b3).isApprox(rho, kSuccessThreshold)) {
589 if (b3(0) < 0) {
590 betas(0) = std::sqrt(-b3(0));
591 betas(1) = (b3(2) < 0) ? std::sqrt(-b3(2)) : 0;
592 } else {
593 betas(0) = std::sqrt(b3(0));
594 betas(1) = (b3(2) > 0) ? std::sqrt(b3(2)) : 0;
595 }
596 if (b3(1) < 0) {
597 betas(0) = -betas(0);
598 }
599 betas(2) = 0;
600 betas(3) = 0;
601 ComputePointsCoordinatesInCameraFrame(alphas, betas, u2, &X_camera);
602 AbsoluteOrientation(X_world, X_camera, &Rs[1], &ts[1]);
603 rmse(1) = RootMeanSquareError(x_camera, X_world, K, Rs[1], ts[1]);
604 } else {
605 LOG(ERROR) << "Second approximation of beta not good enough.";
606 ts[1].setZero();
607 rmse(1) = std::numeric_limits<double>::max();
608 }
609
610 // Find the third possible solution for R, t corresponding to:
611 // Betas = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33]
612 // Betas_approx_3 = [b00 b01 b11 b02 b12]
613 betas.setZero();
614 Eigen::Matrix<double, 6, 5> l_6x5;
615 l_6x5 = L.block(0, 0, 6, 5);
616 Eigen::JacobiSVD<Mat> svdOfL5(l_6x5,
617 Eigen::ComputeFullU | Eigen::ComputeFullV);
618 Vec5 b5 = svdOfL5.solve(rho);
619 if ((l_6x5 * b5).isApprox(rho, kSuccessThreshold)) {
620 if (b5(0) < 0) {
621 betas(0) = std::sqrt(-b5(0));
622 if (b5(2) < 0) {
623 betas(1) = std::sqrt(-b5(2));
624 } else {
625 b5(2) = 0;
626 }
627 } else {
628 betas(0) = std::sqrt(b5(0));
629 if (b5(2) > 0) {
630 betas(1) = std::sqrt(b5(2));
631 } else {
632 b5(2) = 0;
633 }
634 }
635 if (b5(1) < 0) {
636 betas(0) = -betas(0);
637 }
638 betas(2) = b5(3) / betas(0);
639 betas(3) = 0;
640 ComputePointsCoordinatesInCameraFrame(alphas, betas, u2, &X_camera);
641 AbsoluteOrientation(X_world, X_camera, &Rs[2], &ts[2]);
642 rmse(2) = RootMeanSquareError(x_camera, X_world, K, Rs[2], ts[2]);
643 } else {
644 LOG(ERROR) << "Third approximation of beta not good enough.";
645 ts[2].setZero();
646 rmse(2) = std::numeric_limits<double>::max();
647 }
648
649 // Finally, with all three solutions, select the (R, t) with the best RMSE.
650 VLOG(2) << "RMSE for solution 0: " << rmse(0);
651 VLOG(2) << "RMSE for solution 1: " << rmse(1);
652 VLOG(2) << "RMSE for solution 2: " << rmse(2);
653 size_t n = 0;
654 if (rmse(1) < rmse(0)) {
655 n = 1;
656 }
657 if (rmse(2) < rmse(n)) {
658 n = 2;
659 }
660 if (rmse(n) == std::numeric_limits<double>::max()) {
661 LOG(ERROR) << "All three possibilities failed. Reporting failure.";
662 return false;
663 }
664
665 VLOG(1) << "RMSE for best solution #" << n << ": " << rmse(n);
666 *R = Rs[n];
667 *t = ts[n];
668
669 // TODO(julien): Improve the solutions with non-linear refinement.
670 return true;
671 }
672
673 /*
674
675 Straight from the paper:
676 http://www.diegm.uniud.it/fusiello/papers/3dimpvt12-b.pdf
677
678 function [R T] = ppnp(P,S,tol)
679 % input
680 % P : matrix (nx3) image coordinates in camera reference [u v 1]
681 % S : matrix (nx3) coordinates in world reference [X Y Z]
682 % tol: exit threshold
683 %
684 % output
685 % R : matrix (3x3) rotation (world-to-camera)
686 % T : vector (3x1) translation (world-to-camera)
687 %
688 n = size(P,1);
689 Z = zeros(n);
690 e = ones(n,1);
691 A = eye(n)-((e*e’)./n);
692 II = e./n;
693 err = +Inf;
694 E_old = 1000*ones(n,3);
695 while err>tol
696 [U,˜,V] = svd(P’*Z*A*S);
697 VT = V’;
698 R=U*[1 0 0; 0 1 0; 0 0 det(U*VT)]*VT;
699 PR = P*R;
700 c = (S-Z*PR)’*II;
701 Y = S-e*c’;
702 Zmindiag = diag(PR*Y’)./(sum(P.*P,2));
703 Zmindiag(Zmindiag<0)=0;
704 Z = diag(Zmindiag);
705 E = Y-Z*PR;
706 err = norm(E-E_old,’fro’);
707 E_old = E;
708 end
709 T = -R*c;
710 end
711
712 */
713 // TODO(keir): Re-do all the variable names and add comments matching the paper.
714 // This implementation has too much of the terseness of the original. On the
715 // other hand, it did work on the first try.
EuclideanResectionPPnP(const Mat2X & x_camera,const Mat3X & X_world,Mat3 * R,Vec3 * t)716 bool EuclideanResectionPPnP(const Mat2X &x_camera,
717 const Mat3X &X_world,
718 Mat3 *R, Vec3 *t) {
719 int n = x_camera.cols();
720 Mat Z = Mat::Zero(n, n);
721 Vec e = Vec::Ones(n);
722 Mat A = Mat::Identity(n, n) - (e * e.transpose() / n);
723 Vec II = e / n;
724
725 Mat P(n, 3);
726 P.col(0) = x_camera.row(0);
727 P.col(1) = x_camera.row(1);
728 P.col(2).setConstant(1.0);
729
730 Mat S = X_world.transpose();
731
732 double error = std::numeric_limits<double>::infinity();
733 Mat E_old = 1000 * Mat::Ones(n, 3);
734
735 Vec3 c;
736 Mat E(n, 3);
737
738 int iteration = 0;
739 double tolerance = 1e-5;
740 // TODO(keir): The limit of 100 can probably be reduced, but this will require
741 // some investigation.
742 while (error > tolerance && iteration < 100) {
743 Mat3 tmp = P.transpose() * Z * A * S;
744 Eigen::JacobiSVD<Mat3> svd(tmp, Eigen::ComputeFullU | Eigen::ComputeFullV);
745 Mat3 U = svd.matrixU();
746 Mat3 VT = svd.matrixV().transpose();
747 Vec3 s;
748 s << 1, 1, (U * VT).determinant();
749 *R = U * s.asDiagonal() * VT;
750 Mat PR = P * *R; // n x 3
751 c = (S - Z*PR).transpose() * II;
752 Mat Y = S - e*c.transpose(); // n x 3
753 Vec Zmindiag = (PR * Y.transpose()).diagonal()
754 .cwiseQuotient(P.rowwise().squaredNorm());
755 for (int i = 0; i < n; ++i) {
756 Zmindiag[i] = std::max(Zmindiag[i], 0.0);
757 }
758 Z = Zmindiag.asDiagonal();
759 E = Y - Z*PR;
760 error = (E - E_old).norm();
761 LOG(INFO) << "PPnP error(" << (iteration++) << "): " << error;
762 E_old = E;
763 }
764 *t = -*R*c;
765
766 // TODO(keir): Figure out what the failure cases are. Is it too many
767 // iterations? Spend some time going through the math figuring out if there
768 // is some way to detect that the algorithm is going crazy, and return false.
769 return true;
770 }
771
772
773 } // namespace resection
774 } // namespace libmv
775