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>().matrixV().block(
250 0, num_m_rows, num_m_columns, num_lambda);
258 num_points * (num_points * (num_points - 1) / 2 - num_points + 1);
259 int num_k_columns = num_lambda * (num_lambda + 1) / 2;
260 Mat K(num_k_rows, num_k_columns);
265 int counter_k_row = 0;
266 for (
int idx1 = num_m_rows; idx1 < num_tt_variables; ++idx1) {
267 for (
int idx2 = 0; idx2 < num_m_rows; ++idx2) {
268 unsigned int i = ij_index(idx1, 0);
269 unsigned int j = ij_index(idx2, 0);
270 unsigned int k = ij_index(idx2, 1);
272 if (i != j && i != k) {
276 K.row(counter_k_row) =
278 V.row(idx3).transpose() *
V.row(idx4),
288 for (
int idx1 = num_m_rows; idx1 < num_tt_variables; ++idx1) {
289 for (
int idx2 = idx1 + 1; idx2 < num_tt_variables; ++idx2) {
290 unsigned int i = ij_index(idx1, 0);
291 unsigned int j = ij_index(idx2, 0);
292 unsigned int k = ij_index(idx2, 1);
297 K.row(counter_k_row) =
299 V.row(idx3).transpose() *
V.row(idx4),
306 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);
455 const Mat3X& X_world,
458 CHECK(x_camera.cols() == X_world.cols());
459 CHECK(x_camera.cols() > 3);
460 size_t num_points = X_world.cols();
463 Mat34 X_control_points;
468 Mat4X alphas(4, num_points);
474 Mat M(2 * num_points, 12);
475 for (
size_t c = 0; c < num_points; c++) {
476 double a0 = alphas(0, c);
477 double a1 = alphas(1, c);
478 double a2 = alphas(2, c);
479 double a3 = alphas(3, c);
480 double ui = x_camera(0, c);
481 double vi = x_camera(1, c);
483 M.block(2*c, 0, 2, 12) << a0, 0,
496 Eigen::JacobiSVD<Mat> MtMsvd(
M.transpose() *
M, Eigen::ComputeFullU);
497 Eigen::Matrix<double, 12, 12> u2 = MtMsvd.matrixU().transpose();
500 Eigen::Matrix<double, 6, 3> dv1;
501 Eigen::Matrix<double, 6, 3> dv2;
502 Eigen::Matrix<double, 6, 3> dv3;
503 Eigen::Matrix<double, 6, 3> dv4;
505 dv1.row(0) = u2.block(11, 0, 1, 3) - u2.block(11, 3, 1, 3);
506 dv1.row(1) = u2.block(11, 0, 1, 3) - u2.block(11, 6, 1, 3);
507 dv1.row(2) = u2.block(11, 0, 1, 3) - u2.block(11, 9, 1, 3);
508 dv1.row(3) = u2.block(11, 3, 1, 3) - u2.block(11, 6, 1, 3);
509 dv1.row(4) = u2.block(11, 3, 1, 3) - u2.block(11, 9, 1, 3);
510 dv1.row(5) = u2.block(11, 6, 1, 3) - u2.block(11, 9, 1, 3);
511 dv2.row(0) = u2.block(10, 0, 1, 3) - u2.block(10, 3, 1, 3);
512 dv2.row(1) = u2.block(10, 0, 1, 3) - u2.block(10, 6, 1, 3);
513 dv2.row(2) = u2.block(10, 0, 1, 3) - u2.block(10, 9, 1, 3);
514 dv2.row(3) = u2.block(10, 3, 1, 3) - u2.block(10, 6, 1, 3);
515 dv2.row(4) = u2.block(10, 3, 1, 3) - u2.block(10, 9, 1, 3);
516 dv2.row(5) = u2.block(10, 6, 1, 3) - u2.block(10, 9, 1, 3);
517 dv3.row(0) = u2.block(9, 0, 1, 3) - u2.block(9, 3, 1, 3);
518 dv3.row(1) = u2.block(9, 0, 1, 3) - u2.block(9, 6, 1, 3);
519 dv3.row(2) = u2.block(9, 0, 1, 3) - u2.block(9, 9, 1, 3);
520 dv3.row(3) = u2.block(9, 3, 1, 3) - u2.block(9, 6, 1, 3);
521 dv3.row(4) = u2.block(9, 3, 1, 3) - u2.block(9, 9, 1, 3);
522 dv3.row(5) = u2.block(9, 6, 1, 3) - u2.block(9, 9, 1, 3);
523 dv4.row(0) = u2.block(8, 0, 1, 3) - u2.block(8, 3, 1, 3);
524 dv4.row(1) = u2.block(8, 0, 1, 3) - u2.block(8, 6, 1, 3);
525 dv4.row(2) = u2.block(8, 0, 1, 3) - u2.block(8, 9, 1, 3);
526 dv4.row(3) = u2.block(8, 3, 1, 3) - u2.block(8, 6, 1, 3);
527 dv4.row(4) = u2.block(8, 3, 1, 3) - u2.block(8, 9, 1, 3);
528 dv4.row(5) = u2.block(8, 6, 1, 3) - u2.block(8, 9, 1, 3);
530 Eigen::Matrix<double, 6, 10>
L;
531 for (
size_t r = 0; r < 6; r++) {
533 L.row(r) << dv1.row(r).dot(dv1.row(r)),
534 2.0 * dv1.row(r).dot(dv2.row(r)),
535 dv2.row(r).dot(dv2.row(r)),
536 2.0 * dv1.row(r).dot(dv3.row(r)),
537 2.0 * dv2.row(r).dot(dv3.row(r)),
538 dv3.row(r).dot(dv3.row(r)),
539 2.0 * dv1.row(r).dot(dv4.row(r)),
540 2.0 * dv2.row(r).dot(dv4.row(r)),
541 2.0 * dv3.row(r).dot(dv4.row(r)),
542 dv4.row(r).dot(dv4.row(r));
547 rho << (X_control_points.col(0) - X_control_points.col(1)).squaredNorm(),
548 (X_control_points.col(0) - X_control_points.col(2)).squaredNorm(),
549 (X_control_points.col(0) - X_control_points.col(3)).squaredNorm(),
550 (X_control_points.col(1) - X_control_points.col(2)).squaredNorm(),
551 (X_control_points.col(1) - X_control_points.col(3)).squaredNorm(),
552 (X_control_points.col(2) - X_control_points.col(3)).squaredNorm();
573 double kSuccessThreshold = std::numeric_limits<double>::max();
578 Vec4 betas = Vec4::Zero();
579 Eigen::Matrix<double, 6, 4> l_6x4;
580 for (
size_t r = 0; r < 6; r++) {
581 l_6x4.row(r) <<
L(r, 0),
L(r, 1),
L(r, 3),
L(r, 6);
583 Eigen::JacobiSVD<Mat> svd_of_l4(l_6x4,
584 Eigen::ComputeFullU | Eigen::ComputeFullV);
585 Vec4 b4 = svd_of_l4.solve(rho);
586 if ((l_6x4 * b4).isApprox(rho, kSuccessThreshold)) {
590 b4(0) = std::sqrt(b4(0));
591 betas << b4(0), b4(1) / b4(0), b4(2) / b4(0), b4(3) / b4(0);
596 LOG(ERROR) <<
"First approximation of beta not good enough.";
598 rmse(0) = std::numeric_limits<double>::max();
605 Eigen::Matrix<double, 6, 3> l_6x3;
606 l_6x3 =
L.block(0, 0, 6, 3);
607 Eigen::JacobiSVD<Mat> svdOfL3(l_6x3,
608 Eigen::ComputeFullU | Eigen::ComputeFullV);
609 Vec3 b3 = svdOfL3.solve(rho);
610 VLOG(2) <<
" rho = " << rho;
611 VLOG(2) <<
" l_6x3 * b3 = " << l_6x3 * b3;
612 if ((l_6x3 * b3).isApprox(rho, kSuccessThreshold)) {
614 betas(0) = std::sqrt(-b3(0));
615 betas(1) = (b3(2) < 0) ? std::sqrt(-b3(2)) : 0;
617 betas(0) = std::sqrt(b3(0));
618 betas(1) = (b3(2) > 0) ? std::sqrt(b3(2)) : 0;
621 betas(0) = -betas(0);
629 LOG(ERROR) <<
"Second approximation of beta not good enough.";
631 rmse(1) = std::numeric_limits<double>::max();
638 Eigen::Matrix<double, 6, 5> l_6x5;
639 l_6x5 =
L.block(0, 0, 6, 5);
640 Eigen::JacobiSVD<Mat> svdOfL5(l_6x5,
641 Eigen::ComputeFullU | Eigen::ComputeFullV);
642 Vec5 b5 = svdOfL5.solve(rho);
643 if ((l_6x5 * b5).isApprox(rho, kSuccessThreshold)) {
645 betas(0) = std::sqrt(-b5(0));
647 betas(1) = std::sqrt(-b5(2));
652 betas(0) = std::sqrt(b5(0));
654 betas(1) = std::sqrt(b5(2));
660 betas(0) = -betas(0);
662 betas(2) = b5(3) / betas(0);
668 LOG(ERROR) <<
"Third approximation of beta not good enough.";
670 rmse(2) = std::numeric_limits<double>::max();
674 VLOG(2) <<
"RMSE for solution 0: " << rmse(0);
675 VLOG(2) <<
"RMSE for solution 1: " << rmse(1);
676 VLOG(2) <<
"RMSE for solution 2: " << rmse(2);
678 if (rmse(1) < rmse(0)) {
681 if (rmse(2) < rmse(n)) {
684 if (rmse(n) == std::numeric_limits<double>::max()) {
685 LOG(ERROR) <<
"All three possibilities failed. Reporting failure.";
689 VLOG(1) <<
"RMSE for best solution #" << n <<
": " << rmse(n);