feat: non-shrinking motion prior error

Summary:
This Diff changes the linear motion prior from additive to multiplicative form, in order to avoid shrinking effects that could arise.

However, we need to switch back to additive from in case of coincident centers (is breaking error function continuity).

Reviewed By: fabianschenk

Differential Revision: D37345489

fbshipit-source-id: f69e763f6f797c40b181ceb576bb6ad3be6e547f

opensfm/src/bundle/error/motion_prior_errors.h

@@ -37,14 +37,29 @@ struct LinearMotionError {
     Eigen::Map<Eigen::Matrix<T, 6, 1> > residual(r);
 
     // Position residual : op is translation
-    residual.segment(0, 3) =
-        T(position_scale_) * (T(alpha_) * (t2 - t0) + (t0 - t1));
+    const auto eps = T(1e-15);
+    const auto t2_t0 = (t2 - t0);
+    const auto t1_t0 = (t1 - t0);
+    const auto t2_t0_norm = t2_t0.norm();
+    const auto t1_t0_norm = t1_t0.norm();
+    for (int i = 0; i < 3; ++i) {
+      // in case of non-zero speed, use speed ratio as it isn't subject
+      //  to collapse when used together with position variance estimation
+      if (t2_t0_norm > eps) {
+        residual(i) =
+            T(position_scale_) * (T(alpha_) - t1_t0_norm / t2_t0_norm);
+        // otherwise, use classic difference between speeds, so one can still
+        // drag away position if they're coincident
+      } else {
+        residual(i) = T(position_scale_) * (T(alpha_) * t2_t0(i) - t1_t0(i));
+      }
+    }
 
     // Rotation residual : op is rotation
     const Vec3<T> R2_R0t = T(alpha_) * MultRotations(R2.eval(), (-R0).eval());
     const Vec3<T> R0_R1t = MultRotations(R0.eval(), (-R1).eval());
     residual.segment(3, 3) =
-        T(position_scale_) * MultRotations(R2_R0t.eval(), R0_R1t);
+        T(orientation_scale_) * MultRotations(R2_R0t.eval(), R0_R1t);
     return true;
   }
   const double alpha_;

opensfm/synthetic_data/synthetic_generator.py

@@ -165,10 +165,6 @@ def generate_exifs(
     causal_gps_noise: bool = False,
 ) -> Dict[str, Any]:
     """Generate fake exif metadata from the reconstruction."""
-    speed_ms = 10.0
-    previous_pose = None
-    previous_time = 0
-    exifs = {}
 
     def _gps_dop(shot: pymap.Shot) -> float:
         gps_dop = 15.0
@@ -178,6 +174,7 @@ def _gps_dop(shot: pymap.Shot) -> float:
             gps_dop = gps_noise[shot.camera.id]
         return gps_dop
 
+    exifs = {}
     per_sequence = defaultdict(list)
     for shot_name in sorted(reconstruction.shots.keys()):
         shot = reconstruction.shots[shot_name]
@@ -193,13 +190,20 @@ def _gps_dop(shot: pymap.Shot) -> float:
         if shot.camera.projection_type in ["perspective", "fisheye"]:
             exif["focal_ratio"] = shot.camera.focal
 
-        pose = shot.pose.get_origin()
+        exifs[shot_name] = exif
+
+    speed_ms = 10.0
+    previous_pose = None
+    previous_time = 0
+    for rig_instance in sorted(
+        reconstruction.rig_instances.values(), key=lambda x: x.id
+    ):
+        pose = rig_instance.pose.get_origin()
         if previous_pose is not None:
             previous_time += np.linalg.norm(pose - previous_pose) / speed_ms
         previous_pose = pose
-        exif["capture_time"] = previous_time
-
-        exifs[shot_name] = exif
+        for shot_id in rig_instance.shots:
+            exifs[shot_id]["capture_time"] = previous_time
 
     for sequence_images in per_sequence.values():
         if causal_gps_noise: