| 10 | using namespace PLPSLAM; |
| 11 | |
| 12 | TEST(homography_solver, linear_solve) |
| 13 | { |
| 14 | // create 3D points |
| 15 | const unsigned int num_landmarks = 100; |
| 16 | const Vec4_t plane_coeffs{-3.0, 5.0, 2.0, 23.0}; |
| 17 | const auto landmarks = create_random_landmarks_on_plane(num_landmarks, 100, plane_coeffs); |
| 18 | |
| 19 | // create two-view poses |
| 20 | const Mat33_t rot_1 = util::converter::to_rot_mat(205.0 * M_PI / 180.0 * Vec3_t{4, -6, 2}.normalized()); |
| 21 | const Vec3_t trans_1 = Vec3_t(-28.1, -63.3, 43.4); |
| 22 | const Mat33_t rot_2 = util::converter::to_rot_mat(-15.0 * M_PI / 180.0 * Vec3_t{5, 1, -3}.normalized()); |
| 23 | const Vec3_t trans_2 = Vec3_t(-30.4, -45.5, -49.6); |
| 24 | // create camera matrices |
| 25 | Mat33_t cam_matrix_1 = Mat33_t::Identity(); |
| 26 | cam_matrix_1(0, 0) = 143.0; |
| 27 | cam_matrix_1(0, 2) = 400.0; |
| 28 | cam_matrix_1(1, 1) = 140.0; |
| 29 | cam_matrix_1(1, 2) = 700.0; |
| 30 | Mat33_t cam_matrix_2 = Mat33_t::Identity(); |
| 31 | cam_matrix_2(0, 0) = 113.0; |
| 32 | cam_matrix_2(0, 2) = 200.0; |
| 33 | cam_matrix_2(1, 1) = 108.0; |
| 34 | cam_matrix_2(1, 2) = 500.0; |
| 35 | |
| 36 | // create keypoints from two-view poses and 3D points |
| 37 | std::vector<cv::Point2f> keypts_1; |
| 38 | std::vector<cv::Point2f> keypts_2; |
| 39 | create_keypoints(rot_1, trans_1, cam_matrix_1, landmarks, keypts_1); |
| 40 | create_keypoints(rot_2, trans_2, cam_matrix_2, landmarks, keypts_2); |
| 41 | |
| 42 | // solve with SVD |
| 43 | const Mat33_t H_21 = solve::homography_solver::compute_H_21(keypts_1, keypts_2); |
| 44 | const Mat33_t H_12 = H_21.inverse(); |
| 45 | |
| 46 | // check symmetric transform error |
| 47 | for (unsigned int i = 0; i < num_landmarks; ++i) |
| 48 | { |
| 49 | const Vec3_t keypt_1 = util::converter::to_homogeneous(keypts_1.at(i)); |
| 50 | const Vec3_t keypt_2 = util::converter::to_homogeneous(keypts_2.at(i)); |
| 51 | // transform each other |
| 52 | Vec3_t keypt_1_in_2 = H_21 * keypt_1; |
| 53 | keypt_1_in_2 /= keypt_1_in_2(2); |
| 54 | Vec3_t keypt_2_in_1 = H_12 * keypt_2; |
| 55 | keypt_2_in_1 /= keypt_2_in_1(2); |
| 56 | // check errors |
| 57 | EXPECT_LT((keypt_1 - keypt_2_in_1).norm(), 5.0); |
| 58 | EXPECT_LT((keypt_2 - keypt_1_in_2).norm(), 5.0); |
| 59 | } |
| 60 | } |
| 61 | |
| 62 | TEST(homography_solver, ransac_solve_without_noise) |
| 63 | { |
nothing calls this directly
no test coverage detected