Blender  V3.3
euclidean_resection.cc
Go to the documentation of this file.
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 
22 
23 #include <cmath>
24 #include <limits>
25 
26 #include <Eigen/Geometry>
27 #include <Eigen/SVD>
28 
29 #include "libmv/base/vector.h"
30 #include "libmv/logging/logging.h"
32 
33 namespace libmv {
34 namespace euclidean_resection {
35 
36 typedef unsigned int uint;
37 
38 bool EuclideanResection(const Mat2X& x_camera,
39  const Mat3X& X_world,
40  Mat3* R,
41  Vec3* t,
42  ResectionMethod method) {
43  switch (method) {
45  EuclideanResectionAnsarDaniilidis(x_camera, X_world, R, t);
46  break;
47  case RESECTION_EPNP:
48  return EuclideanResectionEPnP(x_camera, X_world, R, t);
49  break;
50  case RESECTION_PPNP:
51  return EuclideanResectionPPnP(x_camera, X_world, R, t);
52  break;
53  default: LOG(FATAL) << "Unknown resection method.";
54  }
55  return false;
56 }
57 
58 bool EuclideanResection(const Mat& x_image,
59  const Mat3X& X_world,
60  const Mat3& K,
61  Mat3* R,
62  Vec3* t,
63  ResectionMethod method) {
64  CHECK(x_image.rows() == 2 || x_image.rows() == 3)
65  << "Invalid size for x_image: " << x_image.rows() << "x"
66  << x_image.cols();
67 
68  Mat2X x_camera;
69  if (x_image.rows() == 2) {
70  EuclideanToNormalizedCamera(x_image, K, &x_camera);
71  } else if (x_image.rows() == 3) {
72  HomogeneousToNormalizedCamera(x_image, K, &x_camera);
73  }
74  return EuclideanResection(x_camera, X_world, R, t, method);
75 }
76 
77 void AbsoluteOrientation(const Mat3X& X, const Mat3X& Xp, Mat3* R, Vec3* t) {
78  int num_points = X.cols();
79  Vec3 C = X.rowwise().sum() / num_points; // Centroid of X.
80  Vec3 Cp = Xp.rowwise().sum() / num_points; // Centroid of Xp.
81 
82  // Normalize the two point sets.
83  Mat3X Xn(3, num_points), Xpn(3, num_points);
84  for (int i = 0; i < num_points; ++i) {
85  Xn.col(i) = X.col(i) - C;
86  Xpn.col(i) = Xp.col(i) - Cp;
87  }
88 
89  // Construct the N matrix (pg. 635).
90  double Sxx = Xn.row(0).dot(Xpn.row(0));
91  double Syy = Xn.row(1).dot(Xpn.row(1));
92  double Szz = Xn.row(2).dot(Xpn.row(2));
93  double Sxy = Xn.row(0).dot(Xpn.row(1));
94  double Syx = Xn.row(1).dot(Xpn.row(0));
95  double Sxz = Xn.row(0).dot(Xpn.row(2));
96  double Szx = Xn.row(2).dot(Xpn.row(0));
97  double Syz = Xn.row(1).dot(Xpn.row(2));
98  double Szy = Xn.row(2).dot(Xpn.row(1));
99 
100  Mat4 N;
101  // clang-format off
102  N << Sxx + Syy + Szz, Syz - Szy, Szx - Sxz, Sxy - Syx,
103  Syz - Szy, Sxx - Syy - Szz, Sxy + Syx, Szx + Sxz,
104  Szx - Sxz, Sxy + Syx, -Sxx + Syy - Szz, Syz + Szy,
105  Sxy - Syx, Szx + Sxz, Syz + Szy, -Sxx - Syy + Szz;
106  // clang-format on
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  // clang-format off
122  (*R) << qq(0) + qq(1) - qq(2) - qq(3),
123  2 * (q1q2 - q0q3),
124  2 * (q1q3 + q0q2),
125  2 * (q1q2+ q0q3),
126  qq(0) - qq(1) + qq(2) - qq(3),
127  2 * (q2q3 - q0q1),
128  2 * (q1q3 - q0q2),
129  2 * (q2q3 + q0q1),
130  qq(0) - qq(1) - qq(2) + qq(3);
131  // clang-format on
132 
133  // Fix the handedness of the R matrix.
134  if (R->determinant() < 0) {
135  R->row(2) = -R->row(2);
136  }
137  // Compute the final translation.
138  *t = Cp - *R * C;
139 }
140 
141 // Convert i and j indices of the original variables into their quadratic
142 // permutation single index. It follows that t_ij = t_ji.
143 static int IJToPointIndex(int i, int j, int num_points) {
144  // Always make sure that j is bigger than i. This handles t_ij = t_ji.
145  if (j < i) {
146  std::swap(i, j);
147  }
148  int idx;
149  int num_permutation_rows = num_points * (num_points - 1) / 2;
150 
151  // All t_ii's are located at the end of the t vector after all t_ij's.
152  if (j == i) {
153  idx = num_permutation_rows + i;
154  } else {
155  int offset = (num_points - i - 1) * (num_points - i) / 2;
156  idx = (num_permutation_rows - offset + j - i - 1);
157  }
158  return idx;
159 };
160 
161 // Convert i and j indexes of the solution for lambda to their linear indexes.
162 static int IJToIndex(int i, int j, int num_lambda) {
163  if (j < i) {
164  std::swap(i, j);
165  }
166  int A = num_lambda * (num_lambda + 1) / 2;
167  int B = num_lambda - i;
168  int C = B * (B + 1) / 2;
169  int idx = A - C + j - i;
170  return idx;
171 };
172 
173 static int Sign(double value) {
174  return (value < 0) ? -1 : 1;
175 };
176 
177 // Organizes a square matrix into a single row constraint on the elements of
178 // Lambda to create the constraints in equation (5) in "Linear Pose Estimation
179 // from Points or Lines", by Ansar, A. and Daniilidis, PAMI 2003. vol. 25, no.
180 // 5.
181 static Vec MatrixToConstraint(const Mat& A, int num_k_columns, 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.
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 
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)
250  .matrixV()
251  .block(0, num_m_rows, num_m_columns, num_lambda);
252 
253  // TODO(vess): The number of constraint equations in K (num_k_rows) must be
254  // (num_points + 1) * (num_points + 2)/2. This creates a performance issue
255  // for more than 4 points. It is fine for 4 points at the moment with 18
256  // instead of 15 equations.
257  int num_k_rows =
258  num_m_rows +
259  num_points * (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 =
321  L_sq * Sign(L_sq(IJToIndex(max_L_sq_index, max_L_sq_index, num_lambda)));
322 
323  Vec L(num_lambda);
324  L(max_L_sq_index) =
325  sqrt(L_sq(IJToIndex(max_L_sq_index, max_L_sq_index, num_lambda)));
326 
327  for (int i = 0; i < num_lambda; ++i) {
328  if (i != max_L_sq_index) {
329  L(i) = L_sq(IJToIndex(max_L_sq_index, i, num_lambda)) / L(max_L_sq_index);
330  }
331  }
332 
333  // Correct the scale using the fact that the last constraint is equal to 1.
334  L = L / (V.row(num_m_columns - 1).dot(L));
335  Vec X = V * L;
336 
337  // Recover the distances from the camera center to the 3D points Q.
338  Vec d(num_points);
339  d.setZero();
340  for (int c_point = num_m_rows; c_point < num_tt_variables; ++c_point) {
341  d(c_point - num_m_rows) = sqrt(X(c_point));
342  }
343 
344  // Create the 3D points in the camera system.
345  Mat X_cam(3, num_points);
346  for (int c_point = 0; c_point < num_points; ++c_point) {
347  X_cam.col(c_point) = d(c_point) * x_camera_unit.col(c_point);
348  }
349  // Recover the camera translation and rotation.
350  AbsoluteOrientation(X_world, X_cam, R, t);
351 }
352 
353 // Selects 4 virtual control points using mean and PCA.
354 static void SelectControlPoints(const Mat3X& X_world,
355  Mat* X_centered,
356  Mat34* X_control_points) {
357  size_t num_points = X_world.cols();
358 
359  // The first virtual control point, C0, is the centroid.
360  Vec mean, variance;
361  MeanAndVarianceAlongRows(X_world, &mean, &variance);
362  X_control_points->col(0) = mean;
363 
364  // Computes PCA
365  X_centered->resize(3, num_points);
366  for (size_t c = 0; c < num_points; c++) {
367  X_centered->col(c) = X_world.col(c) - mean;
368  }
369  Mat3 X_centered_sq = (*X_centered) * X_centered->transpose();
370  Eigen::JacobiSVD<Mat3> X_centered_sq_svd(X_centered_sq, Eigen::ComputeFullU);
371  Vec3 w = X_centered_sq_svd.singularValues();
372  Mat3 u = X_centered_sq_svd.matrixU();
373  for (size_t c = 0; c < 3; c++) {
374  double k = sqrt(w(c) / num_points);
375  X_control_points->col(c + 1) = mean + k * u.col(c);
376  }
377 }
378 
379 // Computes the barycentric coordinates for all real points
380 static void ComputeBarycentricCoordinates(const Mat3X& X_world_centered,
381  const Mat34& X_control_points,
382  Mat4X* alphas) {
383  size_t num_points = X_world_centered.cols();
384  Mat3 C2;
385  for (size_t c = 1; c < 4; c++) {
386  C2.col(c - 1) = X_control_points.col(c) - X_control_points.col(0);
387  }
388 
389  Mat3 C2inv = C2.inverse();
390  Mat3X a = C2inv * X_world_centered;
391 
392  alphas->resize(4, num_points);
393  alphas->setZero();
394  alphas->block(1, 0, 3, num_points) = a;
395  for (size_t c = 0; c < num_points; c++) {
396  (*alphas)(0, c) = 1.0 - alphas->col(c).sum();
397  }
398 }
399 
400 // Estimates the coordinates of all real points in the camera coordinate frame
402  const Mat4X& alphas,
403  const Vec4& betas,
404  const Eigen::Matrix<double, 12, 12>& U,
405  Mat3X* X_camera) {
406  size_t num_points = alphas.cols();
407 
408  // Estimates the control points in the camera reference frame.
409  Mat34 C2b;
410  C2b.setZero();
411  for (size_t cu = 0; cu < 4; cu++) {
412  for (size_t c = 0; c < 4; c++) {
413  C2b.col(c) += betas(cu) * U.block(11 - cu, c * 3, 1, 3).transpose();
414  }
415  }
416 
417  // Estimates the 3D points in the camera reference frame
418  X_camera->resize(3, num_points);
419  for (size_t c = 0; c < num_points; c++) {
420  X_camera->col(c) = C2b * alphas.col(c);
421  }
422 
423  // Check the sign of the z coordinate of the points (should be positive)
424  uint num_z_neg = 0;
425  for (size_t i = 0; i < X_camera->cols(); ++i) {
426  if ((*X_camera)(2, i) < 0) {
427  num_z_neg++;
428  }
429  }
430 
431  // If more than 50% of z are negative, we change the signs
432  if (num_z_neg > 0.5 * X_camera->cols()) {
433  C2b = -C2b;
434  *X_camera = -(*X_camera);
435  }
436 }
437 
438 bool EuclideanResectionEPnP(const Mat2X& x_camera,
439  const Mat3X& X_world,
440  Mat3* R,
441  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  // clang-format off
466  M.block(2*c, 0, 2, 12) << a0, 0,
467  a0*(-ui), a1, 0,
468  a1*(-ui), a2, 0,
469  a2*(-ui), a3, 0,
470  a3*(-ui), 0,
471  a0, a0*(-vi), 0,
472  a1, a1*(-vi), 0,
473  a2, a2*(-vi), 0,
474  a3, a3*(-vi);
475  // clang-format on
476  }
477 
478  // TODO(julien): Avoid the transpose by rewriting the u2.block() calls.
479  Eigen::JacobiSVD<Mat> MtMsvd(M.transpose() * M, Eigen::ComputeFullU);
480  Eigen::Matrix<double, 12, 12> u2 = MtMsvd.matrixU().transpose();
481 
482  // Estimate the L matrix.
483  Eigen::Matrix<double, 6, 3> dv1;
484  Eigen::Matrix<double, 6, 3> dv2;
485  Eigen::Matrix<double, 6, 3> dv3;
486  Eigen::Matrix<double, 6, 3> dv4;
487 
488  dv1.row(0) = u2.block(11, 0, 1, 3) - u2.block(11, 3, 1, 3);
489  dv1.row(1) = u2.block(11, 0, 1, 3) - u2.block(11, 6, 1, 3);
490  dv1.row(2) = u2.block(11, 0, 1, 3) - u2.block(11, 9, 1, 3);
491  dv1.row(3) = u2.block(11, 3, 1, 3) - u2.block(11, 6, 1, 3);
492  dv1.row(4) = u2.block(11, 3, 1, 3) - u2.block(11, 9, 1, 3);
493  dv1.row(5) = u2.block(11, 6, 1, 3) - u2.block(11, 9, 1, 3);
494  dv2.row(0) = u2.block(10, 0, 1, 3) - u2.block(10, 3, 1, 3);
495  dv2.row(1) = u2.block(10, 0, 1, 3) - u2.block(10, 6, 1, 3);
496  dv2.row(2) = u2.block(10, 0, 1, 3) - u2.block(10, 9, 1, 3);
497  dv2.row(3) = u2.block(10, 3, 1, 3) - u2.block(10, 6, 1, 3);
498  dv2.row(4) = u2.block(10, 3, 1, 3) - u2.block(10, 9, 1, 3);
499  dv2.row(5) = u2.block(10, 6, 1, 3) - u2.block(10, 9, 1, 3);
500  dv3.row(0) = u2.block(9, 0, 1, 3) - u2.block(9, 3, 1, 3);
501  dv3.row(1) = u2.block(9, 0, 1, 3) - u2.block(9, 6, 1, 3);
502  dv3.row(2) = u2.block(9, 0, 1, 3) - u2.block(9, 9, 1, 3);
503  dv3.row(3) = u2.block(9, 3, 1, 3) - u2.block(9, 6, 1, 3);
504  dv3.row(4) = u2.block(9, 3, 1, 3) - u2.block(9, 9, 1, 3);
505  dv3.row(5) = u2.block(9, 6, 1, 3) - u2.block(9, 9, 1, 3);
506  dv4.row(0) = u2.block(8, 0, 1, 3) - u2.block(8, 3, 1, 3);
507  dv4.row(1) = u2.block(8, 0, 1, 3) - u2.block(8, 6, 1, 3);
508  dv4.row(2) = u2.block(8, 0, 1, 3) - u2.block(8, 9, 1, 3);
509  dv4.row(3) = u2.block(8, 3, 1, 3) - u2.block(8, 6, 1, 3);
510  dv4.row(4) = u2.block(8, 3, 1, 3) - u2.block(8, 9, 1, 3);
511  dv4.row(5) = u2.block(8, 6, 1, 3) - u2.block(8, 9, 1, 3);
512 
513  Eigen::Matrix<double, 6, 10> L;
514  for (size_t r = 0; r < 6; r++) {
515  // clang-format off
516  L.row(r) << dv1.row(r).dot(dv1.row(r)),
517  2.0 * dv1.row(r).dot(dv2.row(r)),
518  dv2.row(r).dot(dv2.row(r)),
519  2.0 * dv1.row(r).dot(dv3.row(r)),
520  2.0 * dv2.row(r).dot(dv3.row(r)),
521  dv3.row(r).dot(dv3.row(r)),
522  2.0 * dv1.row(r).dot(dv4.row(r)),
523  2.0 * dv2.row(r).dot(dv4.row(r)),
524  2.0 * dv3.row(r).dot(dv4.row(r)),
525  dv4.row(r).dot(dv4.row(r));
526  // clang-format on
527  }
528  Vec6 rho;
529  // clang-format off
530  rho << (X_control_points.col(0) - X_control_points.col(1)).squaredNorm(),
531  (X_control_points.col(0) - X_control_points.col(2)).squaredNorm(),
532  (X_control_points.col(0) - X_control_points.col(3)).squaredNorm(),
533  (X_control_points.col(1) - X_control_points.col(2)).squaredNorm(),
534  (X_control_points.col(1) - X_control_points.col(3)).squaredNorm(),
535  (X_control_points.col(2) - X_control_points.col(3)).squaredNorm();
536  // clang-format on
537 
538  // There are three possible solutions based on the three approximations of L
539  // (betas). Below, each one is solved for then the best one is chosen.
540  Mat3X X_camera;
541  Mat3 K;
542  K.setIdentity();
543  vector<Mat3> Rs(3);
544  vector<Vec3> ts(3);
545  Vec rmse(3);
546 
547  // At one point this threshold was 1e-3, and caused no end of problems in
548  // Blender by causing frames to not resect when they would have worked fine.
549  // When the resect failed, the projective followup is run leading to worse
550  // results, and often the dreaded "flipping" where objects get flipped
551  // between frames. Instead, disable the check for now, always succeeding. The
552  // ultimate check is always reprojection error anyway.
553  //
554  // TODO(keir): Decide if setting this to infinity, effectively disabling the
555  // check, is the right approach. So far this seems the case.
556  double kSuccessThreshold = std::numeric_limits<double>::max();
557 
558  // Find the first possible solution for R, t corresponding to:
559  // Betas = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33]
560  // Betas_approx_1 = [b00 b01 b02 b03]
561  Vec4 betas = Vec4::Zero();
562  Eigen::Matrix<double, 6, 4> l_6x4;
563  for (size_t r = 0; r < 6; r++) {
564  l_6x4.row(r) << L(r, 0), L(r, 1), L(r, 3), L(r, 6);
565  }
566  Eigen::JacobiSVD<Mat> svd_of_l4(l_6x4,
567  Eigen::ComputeFullU | Eigen::ComputeFullV);
568  Vec4 b4 = svd_of_l4.solve(rho);
569  if ((l_6x4 * b4).isApprox(rho, kSuccessThreshold)) {
570  if (b4(0) < 0) {
571  b4 = -b4;
572  }
573  b4(0) = std::sqrt(b4(0));
574  betas << b4(0), b4(1) / b4(0), b4(2) / b4(0), b4(3) / b4(0);
575  ComputePointsCoordinatesInCameraFrame(alphas, betas, u2, &X_camera);
576  AbsoluteOrientation(X_world, X_camera, &Rs[0], &ts[0]);
577  rmse(0) = RootMeanSquareError(x_camera, X_world, K, Rs[0], ts[0]);
578  } else {
579  LOG(ERROR) << "First approximation of beta not good enough.";
580  ts[0].setZero();
582  }
583 
584  // Find the second possible solution for R, t corresponding to:
585  // Betas = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33]
586  // Betas_approx_2 = [b00 b01 b11]
587  betas.setZero();
588  Eigen::Matrix<double, 6, 3> l_6x3;
589  l_6x3 = L.block(0, 0, 6, 3);
590  Eigen::JacobiSVD<Mat> svdOfL3(l_6x3,
591  Eigen::ComputeFullU | Eigen::ComputeFullV);
592  Vec3 b3 = svdOfL3.solve(rho);
593  VLOG(2) << " rho = " << rho;
594  VLOG(2) << " l_6x3 * b3 = " << l_6x3 * b3;
595  if ((l_6x3 * b3).isApprox(rho, kSuccessThreshold)) {
596  if (b3(0) < 0) {
597  betas(0) = std::sqrt(-b3(0));
598  betas(1) = (b3(2) < 0) ? std::sqrt(-b3(2)) : 0;
599  } else {
600  betas(0) = std::sqrt(b3(0));
601  betas(1) = (b3(2) > 0) ? std::sqrt(b3(2)) : 0;
602  }
603  if (b3(1) < 0) {
604  betas(0) = -betas(0);
605  }
606  betas(2) = 0;
607  betas(3) = 0;
608  ComputePointsCoordinatesInCameraFrame(alphas, betas, u2, &X_camera);
609  AbsoluteOrientation(X_world, X_camera, &Rs[1], &ts[1]);
610  rmse(1) = RootMeanSquareError(x_camera, X_world, K, Rs[1], ts[1]);
611  } else {
612  LOG(ERROR) << "Second approximation of beta not good enough.";
613  ts[1].setZero();
615  }
616 
617  // Find the third possible solution for R, t corresponding to:
618  // Betas = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33]
619  // Betas_approx_3 = [b00 b01 b11 b02 b12]
620  betas.setZero();
621  Eigen::Matrix<double, 6, 5> l_6x5;
622  l_6x5 = L.block(0, 0, 6, 5);
623  Eigen::JacobiSVD<Mat> svdOfL5(l_6x5,
624  Eigen::ComputeFullU | Eigen::ComputeFullV);
625  Vec5 b5 = svdOfL5.solve(rho);
626  if ((l_6x5 * b5).isApprox(rho, kSuccessThreshold)) {
627  if (b5(0) < 0) {
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  } else {
635  betas(0) = std::sqrt(b5(0));
636  if (b5(2) > 0) {
637  betas(1) = std::sqrt(b5(2));
638  } else {
639  b5(2) = 0;
640  }
641  }
642  if (b5(1) < 0) {
643  betas(0) = -betas(0);
644  }
645  betas(2) = b5(3) / betas(0);
646  betas(3) = 0;
647  ComputePointsCoordinatesInCameraFrame(alphas, betas, u2, &X_camera);
648  AbsoluteOrientation(X_world, X_camera, &Rs[2], &ts[2]);
649  rmse(2) = RootMeanSquareError(x_camera, X_world, K, Rs[2], ts[2]);
650  } else {
651  LOG(ERROR) << "Third approximation of beta not good enough.";
652  ts[2].setZero();
654  }
655 
656  // Finally, with all three solutions, select the (R, t) with the best RMSE.
657  VLOG(2) << "RMSE for solution 0: " << rmse(0);
658  VLOG(2) << "RMSE for solution 1: " << rmse(1);
659  VLOG(2) << "RMSE for solution 2: " << rmse(2);
660  size_t n = 0;
661  if (rmse(1) < rmse(0)) {
662  n = 1;
663  }
664  if (rmse(2) < rmse(n)) {
665  n = 2;
666  }
667  if (rmse(n) == std::numeric_limits<double>::max()) {
668  LOG(ERROR) << "All three possibilities failed. Reporting failure.";
669  return false;
670  }
671 
672  VLOG(1) << "RMSE for best solution #" << n << ": " << rmse(n);
673  *R = Rs[n];
674  *t = ts[n];
675 
676  // TODO(julien): Improve the solutions with non-linear refinement.
677  return true;
678 }
679 
680 /*
681 
682  Straight from the paper:
683  http://www.diegm.uniud.it/fusiello/papers/3dimpvt12-b.pdf
684 
685  function [R T] = ppnp(P,S,tol)
686  % input
687  % P : matrix (nx3) image coordinates in camera reference [u v 1]
688  % S : matrix (nx3) coordinates in world reference [X Y Z]
689  % tol: exit threshold
690  %
691  % output
692  % R : matrix (3x3) rotation (world-to-camera)
693  % T : vector (3x1) translation (world-to-camera)
694  %
695  n = size(P,1);
696  Z = zeros(n);
697  e = ones(n,1);
698  A = eye(n)-((e*e’)./n);
699  II = e./n;
700  err = +Inf;
701  E_old = 1000*ones(n,3);
702  while err>tol
703  [U,˜,V] = svd(P’*Z*A*S);
704  VT = V’;
705  R=U*[1 0 0; 0 1 0; 0 0 det(U*VT)]*VT;
706  PR = P*R;
707  c = (S-Z*PR)’*II;
708  Y = S-e*c’;
709  Zmindiag = diag(PR*Y’)./(sum(P.*P,2));
710  Zmindiag(Zmindiag<0)=0;
711  Z = diag(Zmindiag);
712  E = Y-Z*PR;
713  err = norm(E-E_old,’fro’);
714  E_old = E;
715  end
716  T = -R*c;
717  end
718 
719  */
720 // TODO(keir): Re-do all the variable names and add comments matching the paper.
721 // This implementation has too much of the terseness of the original. On the
722 // other hand, it did work on the first try.
723 bool EuclideanResectionPPnP(const Mat2X& x_camera,
724  const Mat3X& X_world,
725  Mat3* R,
726  Vec3* t) {
727  int n = x_camera.cols();
728  Mat Z = Mat::Zero(n, n);
729  Vec e = Vec::Ones(n);
730  Mat A = Mat::Identity(n, n) - (e * e.transpose() / n);
731  Vec II = e / n;
732 
733  Mat P(n, 3);
734  P.col(0) = x_camera.row(0);
735  P.col(1) = x_camera.row(1);
736  P.col(2).setConstant(1.0);
737 
738  Mat S = X_world.transpose();
739 
740  double error = std::numeric_limits<double>::infinity();
741  Mat E_old = 1000 * Mat::Ones(n, 3);
742 
743  Vec3 c;
744  Mat E(n, 3);
745 
746  int iteration = 0;
747  double tolerance = 1e-5;
748  // TODO(keir): The limit of 100 can probably be reduced, but this will require
749  // some investigation.
750  while (error > tolerance && iteration < 100) {
751  Mat3 tmp = P.transpose() * Z * A * S;
752  Eigen::JacobiSVD<Mat3> svd(tmp, Eigen::ComputeFullU | Eigen::ComputeFullV);
753  Mat3 U = svd.matrixU();
754  Mat3 VT = svd.matrixV().transpose();
755  Vec3 s;
756  s << 1, 1, (U * VT).determinant();
757  *R = U * s.asDiagonal() * VT;
758  Mat PR = P * *R; // n x 3
759  c = (S - Z * PR).transpose() * II;
760  Mat Y = S - e * c.transpose(); // n x 3
761  Vec Zmindiag = (PR * Y.transpose())
762  .diagonal()
763  .cwiseQuotient(P.rowwise().squaredNorm());
764  for (int i = 0; i < n; ++i) {
765  Zmindiag[i] = std::max(Zmindiag[i], 0.0);
766  }
767  Z = Zmindiag.asDiagonal();
768  E = Y - Z * PR;
769  error = (E - E_old).norm();
770  LG << "PPnP error(" << (iteration++) << "): " << error;
771  E_old = E;
772  }
773  *t = -*R * c;
774 
775  // TODO(keir): Figure out what the failure cases are. Is it too many
776  // iterations? Spend some time going through the math figuring out if there
777  // is some way to detect that the algorithm is going crazy, and return false.
778  return true;
779 }
780 
781 } // namespace euclidean_resection
782 } // namespace libmv
sqrt(x)+1/max(0
void swap(T &a, T &b)
Definition: Common.h:19
#define K(key)
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble GLdouble r _GL_VOID_RET _GL_VOID GLfloat GLfloat r _GL_VOID_RET _GL_VOID GLint GLint r _GL_VOID_RET _GL_VOID GLshort GLshort r _GL_VOID_RET _GL_VOID GLdouble GLdouble r
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble u2
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble t
#define X
Definition: GeomUtils.cpp:199
#define Z
Definition: GeomUtils.cpp:201
#define Y
Definition: GeomUtils.cpp:200
#define C
Definition: RandGen.cpp:25
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
#define A
unsigned int U
Definition: btGjkEpa3.h:78
btMatrix3x3 transpose() const
Return the transpose of the matrix.
btScalar determinant() const
Return the determinant of the matrix.
Definition: btMatrix3x3.h:1022
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:119
SIMD_FORCE_INLINE btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:263
#define CHECK(test_value, str, ofs, msg)
ccl_gpu_kernel_postfix ccl_global float int int int int float bool int offset
#define LG
#define VLOG(severity)
Definition: log.h:37
#define LOG(severity)
Definition: log.h:36
ccl_device_inline float2 fabs(const float2 &a)
Definition: math_float2.h:222
static float P(float k)
Definition: math_interp.c:25
#define M
#define N
#define B
#define R
#define L
static void error(const char *str)
Definition: meshlaplacian.c:51
static unsigned c
Definition: RandGen.cpp:83
static unsigned a[3]
Definition: RandGen.cpp:78
bool EuclideanResection(const Mat2X &x_camera, const Mat3X &X_world, Mat3 *R, Vec3 *t, ResectionMethod method)
void AbsoluteOrientation(const Mat3X &X, const Mat3X &Xp, Mat3 *R, Vec3 *t)
static void SelectControlPoints(const Mat3X &X_world, Mat *X_centered, Mat34 *X_control_points)
static int IJToIndex(int i, int j, int num_lambda)
static int IJToPointIndex(int i, int j, int num_points)
bool EuclideanResectionEPnP(const Mat2X &x_camera, const Mat3X &X_world, Mat3 *R, Vec3 *t)
static void ComputePointsCoordinatesInCameraFrame(const Mat4X &alphas, const Vec4 &betas, const Eigen::Matrix< double, 12, 12 > &U, Mat3X *X_camera)
bool EuclideanResectionPPnP(const Mat2X &x_camera, const Mat3X &X_world, Mat3 *R, Vec3 *t)
static Vec MatrixToConstraint(const Mat &A, int num_k_columns, int num_lambda)
static void ComputeBarycentricCoordinates(const Mat3X &X_world_centered, const Mat34 &X_control_points, Mat4X *alphas)
static int Sign(double value)
void EuclideanResectionAnsarDaniilidis(const Mat2X &x_camera, const Mat3X &X_world, Mat3 *R, Vec3 *t)
static void NormalizeColumnVectors(Mat3X *vectors)
Eigen::VectorXd Vec
Definition: numeric.h:61
Eigen::Vector4d Vec4
Definition: numeric.h:107
Eigen::Matrix< double, 6, 1 > Vec6
Definition: numeric.h:109
std::vector< ElementType, Eigen::aligned_allocator< ElementType > > vector
Eigen::Matrix< double, 3, 3 > Mat3
Definition: numeric.h:72
void HomogeneousToNormalizedCamera(const Mat3X &x, const Mat3 &K, Mat2X *n)
Definition: projection.cc:224
Eigen::MatrixXd Mat
Definition: numeric.h:60
void MeanAndVarianceAlongRows(const Mat &A, Vec *mean_pointer, Vec *variance_pointer)
Definition: numeric.cc:90
double RootMeanSquareError(const Mat2X &x_image, const Mat4X &X_world, const Mat34 &P)
Estimates the root mean square error (2D)
Eigen::Matrix< double, 3, 4 > Mat34
Definition: numeric.h:73
Eigen::Matrix< double, 5, 1 > Vec5
Definition: numeric.h:108
Eigen::Vector3d Vec3
Definition: numeric.h:106
Eigen::Matrix< double, 4, Eigen::Dynamic > Mat4X
Definition: numeric.h:93
Eigen::Matrix< double, 3, Eigen::Dynamic > Mat3X
Definition: numeric.h:92
Eigen::Matrix< unsigned int, Eigen::Dynamic, Eigen::Dynamic > Matu
Definition: numeric.h:66
Eigen::Matrix< double, 2, Eigen::Dynamic > Mat2X
Definition: numeric.h:91
void EuclideanToNormalizedCamera(const Mat2X &x, const Mat3 &K, Mat2X *n)
Definition: projection.cc:217
float max
CCL_NAMESPACE_BEGIN struct Window V