| 14 | using namespace PLPSLAM; |
| 15 | |
| 16 | TEST(pnp_solver, without_ransac) |
| 17 | { |
| 18 | // Create four landmarks which needed the least number for solve problem |
| 19 | const unsigned int num_landmarks = 10; |
| 20 | eigen_alloc_vector<Vec3_t> landmarks; |
| 21 | landmarks.emplace_back(Vec3_t{-19.283677, -18.130606, 82.329830}); |
| 22 | landmarks.emplace_back(Vec3_t{-88.230105, 61.669552, -52.896303}); |
| 23 | landmarks.emplace_back(Vec3_t{-46.048140, 47.097662, -92.047191}); |
| 24 | landmarks.emplace_back(Vec3_t{-91.468185, -56.584450, -35.762650}); |
| 25 | landmarks.emplace_back(Vec3_t{-82.214075, 11.124351, -0.022995}); |
| 26 | landmarks.emplace_back(Vec3_t{-88.117582, 84.359816, -55.239983}); |
| 27 | landmarks.emplace_back(Vec3_t{26.433690, -95.957955, -81.737696}); |
| 28 | landmarks.emplace_back(Vec3_t{43.528656, -0.451698, 84.015400}); |
| 29 | landmarks.emplace_back(Vec3_t{94.330351, 56.546052, 94.311132}); |
| 30 | landmarks.emplace_back(Vec3_t{6.822799, -5.799954, -88.622470}); |
| 31 | |
| 32 | // Create single-view pose |
| 33 | const Mat33_t rot_gt = util::converter::to_rot_mat(97.37 * M_PI / 180 * Vec3_t{9.0, -8.5, 1.1}.normalized()); |
| 34 | const Vec3_t trans_gt = Vec3_t(-67.5, 84.6, -68.0); |
| 35 | |
| 36 | // Create bearing vectors from pose and landmarks |
| 37 | eigen_alloc_vector<Vec3_t> bearings; |
| 38 | create_bearing_vectors(rot_gt, trans_gt, landmarks, bearings); |
| 39 | |
| 40 | // keypts and scale_factor are required of solver |
| 41 | // In this test, octave is 0 and scale factor is 1 for each keypoint |
| 42 | std::vector<cv::KeyPoint> keypts; |
| 43 | const std::vector<float> scale_factor{1}; |
| 44 | for (unsigned int i = 0; i < num_landmarks; i++) |
| 45 | { |
| 46 | keypts.emplace_back(cv::KeyPoint{}); |
| 47 | } |
| 48 | |
| 49 | // Compute the camera pose by pnp_solver |
| 50 | auto solver = std::unique_ptr<solve::pnp_solver>(new solve::pnp_solver(bearings, keypts, landmarks, scale_factor, 0)); |
| 51 | solver->find_via_ransac(30); |
| 52 | |
| 53 | EXPECT_TRUE(solver->solution_is_valid()); |
| 54 | |
| 55 | // Compute error of estimated pose |
| 56 | const auto estimated_pose = solver->get_best_cam_pose(); |
| 57 | |
| 58 | const auto rot = estimated_pose.block<3, 3>(0, 0); |
| 59 | const auto trans = estimated_pose.block<3, 1>(0, 3); |
| 60 | |
| 61 | const auto rot_err = util::converter::to_angle_axis(rot_gt * rot.transpose()).norm(); |
| 62 | const auto trans_err = (trans_gt - trans).norm(); |
| 63 | |
| 64 | EXPECT_LT(rot_err, 1e-2); |
| 65 | EXPECT_LT(trans_err, 1); |
| 66 | } |
| 67 | |
| 68 | TEST(pnp_solver, without_noise) |
| 69 | { |
nothing calls this directly
no test coverage detected