| 480 | } |
| 481 | |
| 482 | int solve_scale_shift_pose(const Eigen::Matrix3d &x_homo, const Eigen::Matrix3d &y_homo, const Eigen::Vector3d &depth_x, |
| 483 | const Eigen::Vector3d &depth_y, std::vector<PoseScaleOffset> *output, bool scale_on_x) { |
| 484 | // X: 3 x 3, column vectors are homogeneous 2D points |
| 485 | // Y: 3 x 3, column vectors are homogeneous 2D points |
| 486 | std::vector<Eigen::Vector4d> solutions; |
| 487 | if (scale_on_x) |
| 488 | solutions = solve_scale_and_shift(y_homo, x_homo, depth_y, depth_x); |
| 489 | else |
| 490 | solutions = solve_scale_and_shift(x_homo, y_homo, depth_x, depth_y); |
| 491 | output->clear(); |
| 492 | |
| 493 | int sol_count = 0; |
| 494 | for (auto &sol : solutions) { |
| 495 | Eigen::Vector3d d1, d2; |
| 496 | if (scale_on_x) { |
| 497 | d1 = depth_x.array() * sol(2) + sol(3); |
| 498 | d2 = depth_y.array() + sol(1); |
| 499 | } else { |
| 500 | d1 = depth_x.array() + sol(1); |
| 501 | d2 = depth_y.array() * sol(2) + sol(3); |
| 502 | } |
| 503 | if (d1.minCoeff() <= 0 || d2.minCoeff() <= 0) |
| 504 | continue; |
| 505 | |
| 506 | Eigen::Matrix3d X = x_homo.array().rowwise() * d1.transpose().array(); |
| 507 | Eigen::Matrix3d Y = y_homo.array().rowwise() * d2.transpose().array(); |
| 508 | |
| 509 | Eigen::Vector3d centroid_X = X.rowwise().mean(); |
| 510 | Eigen::Vector3d centroid_Y = Y.rowwise().mean(); |
| 511 | |
| 512 | Eigen::MatrixXd X_centered = X.colwise() - centroid_X; |
| 513 | Eigen::MatrixXd Y_centered = Y.colwise() - centroid_Y; |
| 514 | |
| 515 | Eigen::Matrix3d S = Y_centered * X_centered.transpose(); |
| 516 | |
| 517 | Eigen::JacobiSVD<Eigen::MatrixXd> svd(S, Eigen::ComputeFullU | Eigen::ComputeFullV); |
| 518 | Eigen::Matrix3d U = svd.matrixU(); |
| 519 | Eigen::Matrix3d V = svd.matrixV(); |
| 520 | |
| 521 | if (U.determinant() * V.determinant() < 0) { |
| 522 | U.col(2) *= -1; |
| 523 | } |
| 524 | Eigen::Matrix3d R = U * V.transpose(); |
| 525 | Eigen::Vector3d t = centroid_Y - R * centroid_X; |
| 526 | |
| 527 | double b2 = sol(1), a1 = sol(2), b1 = sol(3); |
| 528 | if (!scale_on_x) |
| 529 | std::swap(b1, b2); |
| 530 | output->push_back(PoseScaleOffset(R, t, a1, b1, b2)); |
| 531 | sol_count++; |
| 532 | } |
| 533 | return sol_count; |
| 534 | } |
| 535 | |
| 536 | int solve_scale_shift_pose_shared_focal(const Eigen::Matrix3x4d &x_homo, const Eigen::Matrix3x4d &y_homo, |
| 537 | const Eigen::Vector4d &depth_x, const Eigen::Vector4d &depth_y, |
no test coverage detected