| 1285 | } |
| 1286 | |
| 1287 | void test6DPoseFitting( |
| 1288 | const std::string& intrinsics_path_, |
| 1289 | const std::string& ground_truth_pose_path_, |
| 1290 | const std::string& points_path_, |
| 1291 | const std::string& inlier_image_points_path_, |
| 1292 | const double confidence_, |
| 1293 | const double inlier_outlier_threshold_, |
| 1294 | const double spatial_coherence_weight_, |
| 1295 | const double sphere_radius_, |
| 1296 | const int fps_, |
| 1297 | const bool numerical_optimization_, // A flag to decide if numerical optimization should be applied as a post-processing step) |
| 1298 | const double minimum_inlier_ratio_for_sprt_) // An assumption about the minimum inlier ratio used for the SPRT test |
| 1299 | { |
| 1300 | // The image and world points stored as rows in the matrix. Each row is of format 'u v x y z'. |
| 1301 | cv::Mat points; |
| 1302 | |
| 1303 | // Loading the image and world points |
| 1304 | gcransac::utils::loadPointsFromFile<5>(points, // The points in the image |
| 1305 | points_path_.c_str()); // The path where the image points are stored |
| 1306 | |
| 1307 | // Load the intrinsic camera matrix |
| 1308 | Eigen::Matrix3d intrinsics; |
| 1309 | |
| 1310 | if (!utils::loadMatrix<double, 3, 3>(intrinsics_path_, |
| 1311 | intrinsics)) |
| 1312 | { |
| 1313 | printf("An error occured when loading the intrinsics camera matrix from '%s'\n", |
| 1314 | intrinsics_path_.c_str()); |
| 1315 | return; |
| 1316 | } |
| 1317 | |
| 1318 | // Load the ground truth pose |
| 1319 | Eigen::Matrix<double, 3, 4> reference_pose; |
| 1320 | if (!utils::loadMatrix<double, 3, 4>(ground_truth_pose_path_, |
| 1321 | reference_pose)) |
| 1322 | { |
| 1323 | printf("An error occured when loading the reference camera pose from '%s'\n", |
| 1324 | ground_truth_pose_path_.c_str()); |
| 1325 | return; |
| 1326 | } |
| 1327 | |
| 1328 | // The ground truth rotation matrix |
| 1329 | const Eigen::Matrix3d& gt_rotation = |
| 1330 | reference_pose.leftCols<3>(); |
| 1331 | |
| 1332 | // The ground truth translation matrix |
| 1333 | const Eigen::Vector3d& gt_translation = |
| 1334 | reference_pose.rightCols<1>(); |
| 1335 | |
| 1336 | // Normalize the point coordinate by the intrinsic matrix |
| 1337 | cv::Rect roi(0, 0, 2, points.rows); // A rectangle covering the 2D image points in the matrix |
| 1338 | cv::Mat normalized_points = points.clone(); // The 2D image points normalized by the intrinsic camera matrix |
| 1339 | // Normalizing the image points by the camera matrix |
| 1340 | utils::normalizeImagePoints( |
| 1341 | points(roi), // The loaded image points |
| 1342 | intrinsics, // The intrinsic camera matrix |
| 1343 | normalized_points); // The normalized points |
| 1344 |
no test coverage detected