fix the bug that relative pose estimation fails when camera model is not supported by poselib

Author: lpanaf

glomap/estimators/relpose_estimation.cc

@@ -44,26 +44,53 @@ void EstimateRelativePoses(ViewGraph& view_graph,
         const Image& image2 = images[image_pair.image_id2];
         const Eigen::MatrixXi& matches = image_pair.matches;
 
+        const Camera& camera1 = cameras[image1.camera_id];
+        const Camera& camera2 = cameras[image2.camera_id];
+        poselib::Camera camera_poselib1 = ColmapCameraToPoseLibCamera(camera1);
+        poselib::Camera camera_poselib2 = ColmapCameraToPoseLibCamera(camera2);
+        bool valid_camera_model =
+            (camera_poselib1.model_id >= 0 && camera_poselib2.model_id >= 0);
+
         // Collect the original 2D points
         points2D_1.clear();
         points2D_2.clear();
         for (size_t idx = 0; idx < matches.rows(); idx++) {
           points2D_1.push_back(image1.features[matches(idx, 0)]);
           points2D_2.push_back(image2.features[matches(idx, 1)]);
         }
-
+        // If the camera model is not supported by poselib
+        if (!valid_camera_model) {
+          // Undistort points
+          // Note that here, we still rescale by the focal length (to avoid
+          // change the RANSAC threshold)
+          for (size_t idx = 0; idx < matches.rows(); idx++) {
+            points2D_1[idx] =
+                camera1.Focal() * camera1.CamFromImg(points2D_1[idx]);
+            points2D_2[idx] =
+                camera2.Focal() * camera2.CamFromImg(points2D_2[idx]);
+          }
+          // Reset the camera to be the pinhole camera with original focal
+          // length and zero principal point
+          camera_poselib1 = poselib::Camera("SIMPLE_PINHOLE",
+                                            {camera1.Focal(), 0., 0.},
+                                            camera1.width,
+                                            camera1.height);
+          camera_poselib2 = poselib::Camera("SIMPLE_PINHOLE",
+                                            {camera2.Focal(), 0., 0.},
+                                            camera2.width,
+                                            camera2.height);
+        }
         inliers.clear();
         poselib::CameraPose pose_rel_calc;
         try {
-          poselib::estimate_relative_pose(
-              points2D_1,
-              points2D_2,
-              ColmapCameraToPoseLibCamera(cameras[image1.camera_id]),
-              ColmapCameraToPoseLibCamera(cameras[image2.camera_id]),
-              options.ransac_options,
-              options.bundle_options,
-              &pose_rel_calc,
-              &inliers);
+          poselib::estimate_relative_pose(points2D_1,
+                                          points2D_2,
+                                          camera_poselib1,
+                                          camera_poselib2,
+                                          options.ransac_options,
+                                          options.bundle_options,
+                                          &pose_rel_calc,
+                                          &inliers);
         } catch (const std::exception& e) {
           LOG(ERROR) << "Error in relative pose estimation: " << e.what();
           image_pair.is_valid = false;