Robustly handle undistortion failures (#58)

* Robustly handle undistortion failures

* f

* d

* f

---------

Co-authored-by: Linfei Pan <linpan@student.ethz.ch>

glomap/estimators/cost_function.h

@@ -14,27 +14,19 @@ namespace glomap {
 // from two positions such that t_ij - scale * (c_j - c_i) is minimized.
 struct BATAPairwiseDirectionError {
   BATAPairwiseDirectionError(const Eigen::Vector3d& translation_obs)
-      : translation_obs_(translation_obs){};
+      : translation_obs_(translation_obs) {}
 
   // The error is given by the position error described above.
   template <typename T>
   bool operator()(const T* position1,
                   const T* position2,
                   const T* scale,
                   T* residuals) const {
-    Eigen::Matrix<T, 3, 1> translation;
-    translation[0] = position2[0] - position1[0];
-    translation[1] = position2[1] - position1[1];
-    translation[2] = position2[2] - position1[2];
-
-    Eigen::Matrix<T, 3, 1> residual_vec;
-
-    residual_vec = translation_obs_.cast<T>() - scale[0] * translation;
-
-    residuals[0] = residual_vec(0);
-    residuals[1] = residual_vec(1);
-    residuals[2] = residual_vec(2);
-
+    Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals_vec(residuals);
+    residuals_vec =
+        translation_obs_.cast<T>() -
+        scale[0] * (Eigen::Map<const Eigen::Matrix<T, 3, 1>>(position2) -
+                    Eigen::Map<const Eigen::Matrix<T, 3, 1>>(position1));
     return true;
   }
 

glomap/estimators/global_positioning.cc

@@ -155,14 +155,15 @@ void GlobalPositioner::AddCameraToCameraConstraints(
     const image_t image_id1 = image_pair.image_id1;
     const image_t image_id2 = image_pair.image_id2;
     if (images.find(image_id1) == images.end() ||
-        images.find(image_id2) == images.end())
+        images.find(image_id2) == images.end()) {
       continue;
+    }
 
     CHECK_GT(scales_.capacity(), scales_.size())
         << "Not enough capacity was reserved for the scales.";
     double& scale = scales_.emplace_back(1);
 
-    Eigen::Vector3d translation =
+    const Eigen::Vector3d translation =
         -(images[image_id2].cam_from_world.rotation.inverse() *
           image_pair.cam2_from_cam1.translation);
     ceres::CostFunction* cost_function =
@@ -244,7 +245,7 @@ void GlobalPositioner::AddPointToCameraConstraints(
 }
 
 void GlobalPositioner::AddTrackToProblem(
-    const track_t& track_id,
+    track_t track_id,
     std::unordered_map<camera_t, Camera>& cameras,
     std::unordered_map<image_t, Image>& images,
     std::unordered_map<track_t, Track>& tracks) {
@@ -255,8 +256,19 @@ void GlobalPositioner::AddTrackToProblem(
     Image& image = images[observation.first];
     if (!image.is_registered) continue;
 
-    Eigen::Vector3d translation = image.cam_from_world.rotation.inverse() *
-                                  image.features_undist[observation.second];
+    const Eigen::Vector3d& feature_undist =
+        image.features_undist[observation.second];
+    if (feature_undist.array().isNaN().any()) {
+      LOG(WARNING)
+          << "Ignoring feature because it failed to undistort: track_id="
+          << track_id << ", image_id=" << observation.first
+          << ", feature_id=" << observation.second;
+      continue;
+    }
+
+    const Eigen::Vector3d translation =
+        image.cam_from_world.rotation.inverse() *
+        image.features_undist[observation.second];
     ceres::CostFunction* cost_function =
         BATAPairwiseDirectionError::Create(translation);
 
@@ -272,17 +284,19 @@ void GlobalPositioner::AddTrackToProblem(
 
     // For calibrated and uncalibrated cameras, use different loss functions
     // Down weight the uncalibrated cameras
-    (cameras[image.camera_id].has_prior_focal_length)
-        ? problem_->AddResidualBlock(cost_function,
-                                     loss_function_ptcam_calibrated_.get(),
-                                     image.cam_from_world.translation.data(),
-                                     tracks[track_id].xyz.data(),
-                                     &scale)
-        : problem_->AddResidualBlock(cost_function,
-                                     loss_function_ptcam_uncalibrated_.get(),
-                                     image.cam_from_world.translation.data(),
-                                     tracks[track_id].xyz.data(),
-                                     &scale);
+    if (cameras[image.camera_id].has_prior_focal_length) {
+      problem_->AddResidualBlock(cost_function,
+                                 loss_function_ptcam_calibrated_.get(),
+                                 image.cam_from_world.translation.data(),
+                                 tracks[track_id].xyz.data(),
+                                 &scale);
+    } else {
+      problem_->AddResidualBlock(cost_function,
+                                 loss_function_ptcam_uncalibrated_.get(),
+                                 image.cam_from_world.translation.data(),
+                                 tracks[track_id].xyz.data(),
+                                 &scale);
+    }
 
     problem_->SetParameterLowerBound(&scale, 0, 1e-5);
   }

glomap/estimators/global_positioning.h

@@ -80,7 +80,7 @@ class GlobalPositioner {
       std::unordered_map<track_t, Track>& tracks);
 
   // Add a single track to the problem
-  void AddTrackToProblem(const track_t& track_id,
+  void AddTrackToProblem(track_t track_id,
                          std::unordered_map<camera_t, Camera>& cameras,
                          std::unordered_map<image_t, Image>& images,
                          std::unordered_map<track_t, Track>& tracks);

glomap/estimators/relpose_estimation.cc

@@ -47,15 +47,22 @@ void EstimateRelativePoses(ViewGraph& view_graph,
 
       inliers.clear();
       poselib::CameraPose pose_rel_calc;
-      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);
+      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);
+      } catch (const std::exception& e) {
+        LOG(ERROR) << "Error in relative pose estimation: " << e.what();
+        image_pair.is_valid = false;
+        continue;
+      }
+
       // Convert the relative pose to the glomap format
       for (int i = 0; i < 4; i++) {
         image_pair.cam2_from_cam1.rotation.coeffs()[i] =

glomap/processors/track_filter.cc

@@ -37,7 +37,7 @@ int TrackFilter::FilterTracksByReprojection(
 
       // If the reprojection error is smaller than the threshold, then keep it
       if (reprojection_error < max_reprojection_error) {
-        observation_new.emplace_back(std::make_pair(image_id, feature_id));
+        observation_new.emplace_back(image_id, feature_id);
       }
     }
     if (observation_new.size() != track.observations.size()) {

glomap/scene/image.h

@@ -47,11 +47,10 @@ struct Image {
   // Gravity information
   GravityInfo gravity_info;
 
-  // Features
+  // Distorted feature points in pixels.
   std::vector<Eigen::Vector2d> features;
-  std::vector<Eigen::Vector3d>
-      features_undist;  // store the normalized features, can be obtained by
-                        // calling UndistortImages
+  // Normalized feature rays, can be obtained by calling UndistortImages.
+  std::vector<Eigen::Vector3d> features_undist;
 
   // Methods
   inline Eigen::Vector3d Center() const;