Fix StdDeviationConstraint singularity and add lower bound for std deviation parameter

Differential Revision: D100588139

fbshipit-source-id: b894b27a0c99380d23fe2843dd51f8fe2acba19d

Author: Fabian Schenk

opensfm/src/bundle/error/parameters_errors.h

@@ -9,8 +9,9 @@ struct StdDeviationConstraint {
 
   template <typename T>
   bool operator()(const T* const std_deviation, T* residuals) const {
-    T std = std_deviation[0];
-    residuals[0] = ceres::log(T(1.0) / ceres::sqrt(T(2.0 * M_PI) * std * std));
+    constexpr double kEps = 1e-20;
+    T std_sq = std_deviation[0] * std_deviation[0] + T(kEps);
+    residuals[0] = ceres::log(T(1.0) / ceres::sqrt(T(2.0 * M_PI) * std_sq));
     return true;
   }
 };

opensfm/src/bundle/src/bundle_adjuster.cc

@@ -732,6 +732,7 @@ void BundleAdjuster::Run() {
               new StdDeviationConstraint());
       problem.AddResidualBlock(std_dev_cost_function, nullptr,
                                &std_deviations[i]);
+      problem.SetParameterLowerBound(&std_deviations[i], 0, 1e-10);
     }
   } else {
     for (int i = 0; i < std_deviations.size(); ++i) {

opensfm/src/bundle/test/bundle_data_test.cc

@@ -1,5 +1,6 @@
 #include <bundle/bundle_adjuster.h>
 #include <bundle/error/absolute_motion_errors.h>
+#include <bundle/error/parameters_errors.h>
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
 
@@ -227,3 +228,21 @@ TEST(TranslationPriorError, ZeroPriorNormClamped) {
   EXPECT_TRUE(error(instance1.data(), instance2.data(), &residual));
   EXPECT_TRUE(std::isfinite(residual));
 }
+
+TEST(StdDeviationConstraint, FiniteResidualAtLowerBound) {
+  // Regression test: at the Ceres lower bound of 1e-10, the residual must
+  // be finite (not -inf or NaN from log(0)).
+  bundle::StdDeviationConstraint constraint;
+  constexpr double std_dev = 1e-10;
+  double residual = 0.0;
+  EXPECT_TRUE(constraint(&std_dev, &residual));
+  EXPECT_TRUE(std::isfinite(residual));
+}
+
+TEST(StdDeviationConstraint, FiniteResidualAtZero) {
+  bundle::StdDeviationConstraint constraint;
+  constexpr double std_dev = 0.0;
+  double residual = 0.0;
+  EXPECT_TRUE(constraint(&std_dev, &residual));
+  EXPECT_TRUE(std::isfinite(residual));
+}