Merge pull request #997 from borglab/force-nonnegative-scale-Sim3

Force nonnegative scale for Sim(3)
borglab · Dec 30, 2021 · 0ba1d3b · 0ba1d3b
2 parents 2f7ebb4 + fa38b29
commit 0ba1d3b
Show file tree

Hide file tree

Showing 2 changed files with 585 additions and 1 deletion.
diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp
@@ -40,6 +40,8 @@ static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
 }
 
 /// Form inner products x and y and calculate scale.
+// We force the scale to be a non-negative quantity
+// (see Section 10.1 of https://ethaneade.com/lie_groups.pdf)
 static double calculateScale(const Point3Pairs &d_abPointPairs,
                              const Rot3 &aRb) {
   double x = 0, y = 0;
@@ -50,7 +52,7 @@ static double calculateScale(const Point3Pairs &d_abPointPairs,
     y += da.transpose() * da_prime;
     x += da_prime.transpose() * da_prime;
   }
-  const double s = y / x;
+  const double s = std::fabs(y / x);
   return s;
 }