Function solve_scale_shift_pose_shared_focal

src/solver.cpp:536–593

}

int solve_scale_shift_pose_shared_focal(const Eigen::Matrix3x4d &x_homo, const Eigen::Matrix3x4d &y_homo,
                                        const Eigen::Vector4d &depth_x, const Eigen::Vector4d &depth_y,
                                        std::vector<PoseScaleOffsetSharedFocal> *output, bool scale_on_x) {
    std::vector<Eigen::Vector<double, 5>> solutions;
    if (scale_on_x)
        solutions = solve_scale_and_shift_shared_focal(y_homo, x_homo, depth_y, depth_x);
    else
        solutions = solve_scale_and_shift_shared_focal(x_homo, y_homo, depth_x, depth_y);
    output->clear();

    int sol_count = 0;
    for (auto &sol : solutions) {
        Eigen::Vector4d d1, d2;
        if (scale_on_x) {
            d1 = depth_x.array() * sol(2) + sol(3);
            d2 = depth_y.array() + sol(1);
        } else {
            d1 = depth_x.array() + sol(1);
            d2 = depth_y.array() * sol(2) + sol(3);
        }
        if (d1.minCoeff() <= 0 || d2.minCoeff() <= 0)
            continue;

        double focal = sol(4);
        Eigen::Matrix3x4d xu = x_homo;
        Eigen::Matrix3x4d yu = y_homo;
        xu.block<2, 4>(0, 0) /= focal;
        yu.block<2, 4>(0, 0) /= focal;

        Eigen::Matrix3x4d X = xu.array().rowwise() * d1.transpose().array();
        Eigen::Matrix3x4d Y = yu.array().rowwise() * d2.transpose().array();

        Eigen::Vector3d centroid_X = X.rowwise().mean();
        Eigen::Vector3d centroid_Y = Y.rowwise().mean();

        Eigen::MatrixXd X_centered = X.colwise() - centroid_X;
        Eigen::MatrixXd Y_centered = Y.colwise() - centroid_Y;

        Eigen::Matrix3d S = Y_centered * X_centered.transpose();

        Eigen::JacobiSVD<Eigen::MatrixXd> svd(S, Eigen::ComputeFullU | Eigen::ComputeFullV);
        Eigen::Matrix3d U = svd.matrixU();
        Eigen::Matrix3d V = svd.matrixV();

        if (U.determinant() * V.determinant() < 0) {
            U.col(2) *= -1;
        }
        Eigen::Matrix3d R = U * V.transpose();
        Eigen::Vector3d t = centroid_Y - R * centroid_X;

        double b2 = sol(1), a1 = sol(2), b1 = sol(3), f = sol(4);
        if (!scale_on_x)
            std::swap(b1, b2);
        output->push_back(PoseScaleOffsetSharedFocal(R, t, a1, b1, b2, f));
        sol_count++;
    }
    return sol_count;
}