26 #include <Eigen/Geometry>
34 namespace euclidean_resection {
53 default:
LOG(FATAL) <<
"Unknown resection method.";
64 CHECK(x_image.rows() == 2 || x_image.rows() == 3)
65 <<
"Invalid size for x_image: " << x_image.rows() <<
"x"
69 if (x_image.rows() == 2) {
71 }
else if (x_image.rows() == 3) {
78 int num_points =
X.cols();
79 Vec3 C =
X.rowwise().sum() / num_points;
80 Vec3 Cp = Xp.rowwise().sum() / num_points;
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;
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));
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;
110 Vec4 q =
N.jacobiSvd(Eigen::ComputeFullU).matrixU().col(0);
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);
122 (*R) << qq(0) + qq(1) - qq(2) - qq(3),
126 qq(0) - qq(1) + qq(2) - qq(3),
130 qq(0) - qq(1) - qq(2) + qq(3);
134 if (
R->determinant() < 0) {
135 R->row(2) = -
R->row(2);
149 int num_permutation_rows = num_points * (num_points - 1) / 2;
153 idx = num_permutation_rows + i;
155 int offset = (num_points - i - 1) * (num_points - i) / 2;
156 idx = (num_permutation_rows -
offset + j - i - 1);
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;
173 static int Sign(
double value) {
174 return (value < 0) ? -1 : 1;
182 Vec C(num_k_columns);
185 for (
int i = 0; i < num_lambda; ++i) {
186 for (
int j = i; j < num_lambda; ++j) {
199 int num_columns = vectors->cols();
200 for (
int i = 0; i < num_columns; ++i) {
201 vectors->col(i).normalize();
206 const Mat3X& X_world,
209 CHECK(x_camera.cols() == X_world.cols());
210 CHECK(x_camera.cols() > 3);
212 int num_points = x_camera.cols();
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();
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);
226 Matu ij_index(num_tt_variables, 2);
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;
244 ij_index(i + num_m_rows, 0) = i;
245 ij_index(i + num_m_rows, 1) = i;
248 int num_lambda = num_points + 1;
249 Mat V =
M.jacobiSvd(Eigen::ComputeFullV)
251 .block(0, num_m_rows, num_m_columns, num_lambda);
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);
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);
273 if (i != j && i != k) {
277 K.row(counter_k_row) =
279 V.row(idx3).transpose() *
V.row(idx4),
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);
298 K.row(counter_k_row) =
300 V.row(idx3).transpose() *
V.row(idx4),
306 Vec L_sq =
K.jacobiSvd(Eigen::ComputeFullV).matrixV().col(num_k_columns - 1);
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;
321 L_sq *
Sign(L_sq(
IJToIndex(max_L_sq_index, max_L_sq_index, num_lambda)));
325 sqrt(L_sq(
IJToIndex(max_L_sq_index, max_L_sq_index, num_lambda)));
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);
334 L =
L / (
V.row(num_m_columns - 1).dot(
L));
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));
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);
356 Mat34* X_control_points) {
357 size_t num_points = X_world.cols();
362 X_control_points->col(0) = mean;
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;
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);
381 const Mat34& X_control_points,
383 size_t num_points = X_world_centered.cols();
385 for (
size_t c = 1;
c < 4;
c++) {
386 C2.col(
c - 1) = X_control_points.col(
c) - X_control_points.col(0);
389 Mat3 C2inv = C2.inverse();
390 Mat3X a = C2inv * X_world_centered;
392 alphas->resize(4, num_points);
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();
404 const Eigen::Matrix<double, 12, 12>&
U,
406 size_t num_points = alphas.cols();
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();
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);
425 for (
size_t i = 0; i < X_camera->cols(); ++i) {
426 if ((*X_camera)(2, i) < 0) {
432 if (num_z_neg > 0.5 * X_camera->cols()) {
434 *X_camera = -(*X_camera);
439 const Mat3X& X_world,
442 CHECK(x_camera.cols() == X_world.cols());
443 CHECK(x_camera.cols() > 3);
444 size_t num_points = X_world.cols();
447 Mat34 X_control_points;
452 Mat4X alphas(4, num_points);
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);
466 M.block(2*
c, 0, 2, 12) << a0, 0,
479 Eigen::JacobiSVD<Mat> MtMsvd(
M.transpose() *
M, Eigen::ComputeFullU);
480 Eigen::Matrix<double, 12, 12>
u2 = MtMsvd.matrixU().transpose();
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;
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);
513 Eigen::Matrix<double, 6, 10>
L;
514 for (
size_t r = 0;
r < 6;
r++) {
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));
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();
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);
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)) {
574 betas << b4(0), b4(1) / b4(0), b4(2) / b4(0), b4(3) / b4(0);
579 LOG(ERROR) <<
"First approximation of beta not good enough.";
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)) {
598 betas(1) = (b3(2) < 0) ?
std::sqrt(-b3(2)) : 0;
601 betas(1) = (b3(2) > 0) ?
std::sqrt(b3(2)) : 0;
604 betas(0) = -betas(0);
612 LOG(ERROR) <<
"Second approximation of beta not good enough.";
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)) {
643 betas(0) = -betas(0);
645 betas(2) = b5(3) / betas(0);
651 LOG(ERROR) <<
"Third approximation of beta not good enough.";
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);
661 if (rmse(1) < rmse(0)) {
664 if (rmse(2) < rmse(n)) {
668 LOG(ERROR) <<
"All three possibilities failed. Reporting failure.";
672 VLOG(1) <<
"RMSE for best solution #" << n <<
": " << rmse(n);
724 const Mat3X& X_world,
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);
734 P.col(0) = x_camera.row(0);
735 P.col(1) = x_camera.row(1);
736 P.col(2).setConstant(1.0);
738 Mat S = X_world.transpose();
740 double error = std::numeric_limits<double>::infinity();
741 Mat E_old = 1000 * Mat::Ones(n, 3);
747 double tolerance = 1
e-5;
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();
757 *
R =
U * s.asDiagonal() * VT;
760 Mat Y = S -
e *
c.transpose();
761 Vec Zmindiag = (PR *
Y.transpose())
763 .cwiseQuotient(
P.rowwise().squaredNorm());
764 for (
int i = 0; i < n; ++i) {
765 Zmindiag[i] =
std::max(Zmindiag[i], 0.0);
767 Z = Zmindiag.asDiagonal();
770 LG <<
"PPnP error(" << (iteration++) <<
"): " <<
error;
_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
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
btMatrix3x3 transpose() const
Return the transpose of the matrix.
btScalar determinant() const
Return the determinant of the matrix.
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
SIMD_FORCE_INLINE btScalar norm() const
Return the norm (length) of the vector.
#define CHECK(test_value, str, ofs, msg)
ccl_gpu_kernel_postfix ccl_global float int int int int float bool int offset
ccl_device_inline float2 fabs(const float2 &a)
static void error(const char *str)
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)
@ RESECTION_ANSAR_DANIILIDIS
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::Matrix< double, 6, 1 > Vec6
std::vector< ElementType, Eigen::aligned_allocator< ElementType > > vector
Eigen::Matrix< double, 3, 3 > Mat3
void HomogeneousToNormalizedCamera(const Mat3X &x, const Mat3 &K, Mat2X *n)
void MeanAndVarianceAlongRows(const Mat &A, Vec *mean_pointer, Vec *variance_pointer)
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
Eigen::Matrix< double, 5, 1 > Vec5
Eigen::Matrix< double, 4, Eigen::Dynamic > Mat4X
Eigen::Matrix< double, 3, Eigen::Dynamic > Mat3X
Eigen::Matrix< unsigned int, Eigen::Dynamic, Eigen::Dynamic > Matu
Eigen::Matrix< double, 2, Eigen::Dynamic > Mat2X
void EuclideanToNormalizedCamera(const Mat2X &x, const Mat3 &K, Mat2X *n)
CCL_NAMESPACE_BEGIN struct Window V