| 593 | } |
| 594 | |
| 595 | int solve_scale_shift_pose_two_focal(const Eigen::Matrix3x4d &x_homo, const Eigen::Matrix3x4d &y_homo, |
| 596 | const Eigen::Vector4d &depth_x, const Eigen::Vector4d &depth_y, |
| 597 | std::vector<PoseScaleOffsetTwoFocal> *output, bool scale_on_x) { |
| 598 | std::vector<Eigen::Vector<double, 6>> solutions; |
| 599 | if (scale_on_x) |
| 600 | solutions = solve_scale_and_shift_two_focal(y_homo, x_homo, depth_y, depth_x); |
| 601 | else |
| 602 | solutions = solve_scale_and_shift_two_focal(x_homo, y_homo, depth_x, depth_y); |
| 603 | output->clear(); |
| 604 | |
| 605 | int sol_count = 0; |
| 606 | for (auto &sol : solutions) { |
| 607 | Eigen::Vector4d d1, d2; |
| 608 | if (scale_on_x) { |
| 609 | d1 = depth_x.array() * sol(2) + sol(3); |
| 610 | d2 = depth_y.array() + sol(1); |
| 611 | } else { |
| 612 | d1 = depth_x.array() + sol(1); |
| 613 | d2 = depth_y.array() * sol(2) + sol(3); |
| 614 | } |
| 615 | if (d1.minCoeff() <= 0 || d2.minCoeff() <= 0) |
| 616 | continue; |
| 617 | |
| 618 | double focal1 = sol(4), focal2 = sol(5); |
| 619 | Eigen::Matrix3x4d xu = x_homo; |
| 620 | Eigen::Matrix3x4d yu = y_homo; |
| 621 | xu.block<2, 4>(0, 0) /= focal1; |
| 622 | yu.block<2, 4>(0, 0) /= focal2; |
| 623 | |
| 624 | Eigen::Matrix3x4d X = xu.array().rowwise() * d1.transpose().array(); |
| 625 | Eigen::Matrix3x4d Y = yu.array().rowwise() * d2.transpose().array(); |
| 626 | |
| 627 | Eigen::Vector3d centroid_X = X.rowwise().mean(); |
| 628 | Eigen::Vector3d centroid_Y = Y.rowwise().mean(); |
| 629 | |
| 630 | Eigen::MatrixXd X_centered = X.colwise() - centroid_X; |
| 631 | Eigen::MatrixXd Y_centered = Y.colwise() - centroid_Y; |
| 632 | |
| 633 | Eigen::Matrix3d S = Y_centered * X_centered.transpose(); |
| 634 | |
| 635 | Eigen::JacobiSVD<Eigen::MatrixXd> svd(S, Eigen::ComputeFullU | Eigen::ComputeFullV); |
| 636 | Eigen::Matrix3d U = svd.matrixU(); |
| 637 | Eigen::Matrix3d V = svd.matrixV(); |
| 638 | |
| 639 | if (U.determinant() * V.determinant() < 0) { |
| 640 | U.col(2) *= -1; |
| 641 | } |
| 642 | Eigen::Matrix3d R = U * V.transpose(); |
| 643 | Eigen::Vector3d t = centroid_Y - R * centroid_X; |
| 644 | |
| 645 | double b2 = sol(1), a1 = sol(2), b1 = sol(3), f1 = sol(4), f2 = sol(5); |
| 646 | if (!scale_on_x) |
| 647 | std::swap(b1, b2); |
| 648 | output->push_back(PoseScaleOffsetTwoFocal(R, t, a1, b1, b2, f1, f2)); |
| 649 | sol_count++; |
| 650 | } |
| 651 | return sol_count; |
| 652 | } |
no test coverage detected