Function TEST

test/PLPSLAM/solve/essential_solver.cc:12–45

using namespace PLPSLAM;

TEST(essential_solver, linear_solve)
{
    // create 3D points
    const unsigned int num_landmarks = 100;
    const auto landmarks = create_random_landmarks_in_space(num_landmarks, 100);

    // create two-view poses
    const Mat33_t rot_1 = util::converter::to_rot_mat(205.0 * M_PI / 180.0 * Vec3_t{4, -6, 2}.normalized());
    const Vec3_t trans_1 = Vec3_t(-28.1, -63.3, 43.4);
    const Mat33_t rot_2 = util::converter::to_rot_mat(-15.0 * M_PI / 180.0 * Vec3_t{5, 1, -3}.normalized());
    const Vec3_t trans_2 = Vec3_t(-30.4, -45.5, -49.6);

    // create bearing vectors from two-view poses and 3D points
    eigen_alloc_vector<Vec3_t> bearings_1;
    eigen_alloc_vector<Vec3_t> bearings_2;
    create_bearing_vectors(rot_1, trans_1, landmarks, bearings_1);
    create_bearing_vectors(rot_2, trans_2, landmarks, bearings_2);

    // create a true essential matrix
    Mat33_t true_E_21 = solve::essential_solver::create_E_21(rot_1, trans_1, rot_2, trans_2);

    // solve with SVD
    Mat33_t E_21 = solve::essential_solver::compute_E_21(bearings_1, bearings_2);

    // align scale and sign
    true_E_21 /= true_E_21.norm();
    E_21 /= E_21.norm();
    if (true_E_21(0, 0) * E_21(0, 0) < 0)
    {
        true_E_21 *= -1.0;
    }

    EXPECT_LT((true_E_21 - E_21).norm(), 1e-4);
}

TEST(essential_solver, ransac_solve_without_noise)
{