Merge pull request #2335 from borglab/fix/NonlinearEquality

Deprecate NonlinearEquality and Fix Constrained Covariances

Author: dellaert

.github/copilot-instructions.md

@@ -1,5 +1,5 @@
 For reviewing PRs:
-* All functions in header files should have doxygen-style API docs
+* All functions in header files should have doxygen-style API docs, /** */ style, except small functions like getters which can have single line /// comments, no need for @brief, @params etc
 * Use /// for single-line comments rather than /** */
 * Use meaningful variable names, e.g. `measurement` not `msm`, avoid abbreviations.
 * Flag overly complex or long/functions: break up in smaller functions

gtsam/constrained/NonlinearEqualityConstraint.h

@@ -101,7 +101,7 @@ class ExpressionEqualityConstraint : public NonlinearEqualityConstraint {
 
 /** Equality constraint that enforce the cost factor with zero error. 
  * e.g., for a factor with unwhitened cost 2x-1, the constraint enforces the
- * equlity 2x-1=0.
+ * equality 2x-1=0.
  */
 class GTSAM_EXPORT ZeroCostConstraint : public NonlinearEqualityConstraint {
  public:

gtsam/linear/JacobianFactor.cpp

@@ -515,9 +515,13 @@ double JacobianFactor::error(const VectorValues& c) const {
 /* ************************************************************************* */
 Matrix JacobianFactor::augmentedInformation() const {
   if (model_) {
-    Matrix AbWhitened = Ab_.full();
-    model_->WhitenInPlace(AbWhitened);
-    return AbWhitened.transpose() * AbWhitened;
+    Matrix Ab = Ab_.full();
+    if (model_->isConstrained()) {
+      auto constrained = std::static_pointer_cast<noiseModel::Constrained>(model_);
+      return constrained->informationFromA(Ab);
+    }
+    model_->WhitenInPlace(Ab);
+    return Ab.transpose() * Ab;
   } else {
     return Ab_.full().transpose() * Ab_.full();
   }
@@ -526,9 +530,13 @@ Matrix JacobianFactor::augmentedInformation() const {
 /* ************************************************************************* */
 Matrix JacobianFactor::information() const {
   if (model_) {
-    Matrix AWhitened = this->getA();
-    model_->WhitenInPlace(AWhitened);
-    return AWhitened.transpose() * AWhitened;
+    Matrix A = this->getA();
+    if (model_->isConstrained()) {
+      auto constrained = std::static_pointer_cast<noiseModel::Constrained>(model_);
+      return constrained->informationFromA(A);
+    }
+    model_->WhitenInPlace(A);
+    return A.transpose() * A;
   } else {
     return this->getA().transpose() * this->getA();
   }

gtsam/linear/NoiseModel.cpp

@@ -341,12 +341,16 @@ double Diagonal::logDetR() const {
 /* ************************************************************************* */
 
 namespace internal {
-// switch precisions and invsigmas to finite value
-// TODO: why?? And, why not just ask s==0.0 below ?
+// Keep invsigmas finite for constrained entries while preserving infinite
+// precision so downstream information matrices reflect determinism.
 static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) {
+  static const double kInfinity = std::numeric_limits<double>::infinity();
   for (Vector::Index i = 0; i < sigmas.size(); ++i)
     if (!std::isfinite(1. / sigmas[i])) {
-      precisions[i] = 0.0;
+      // Preserve the infinite precision so downstream information matrices
+      // reflect deterministic constraints. We still zero invsigmas to avoid
+      // scaling constraint rows during whitening.
+      precisions[i] = kInfinity;
       invsigmas[i] = 0.0;
     }
 }
@@ -457,6 +461,40 @@ void Constrained::WhitenInPlace(Eigen::Block<Matrix> H) const {
       H.row(i) *= invsigmas_(i);
 }
 
+/* ************************************************************************* */
+Matrix Constrained::informationFromA(const Matrix& A) const {
+  const double kZeroTol = 1e-12;
+  const double kInfinity = std::numeric_limits<double>::infinity();
+  Matrix info = Matrix::Zero(A.cols(), A.cols());
+  assert(static_cast<DenseIndex>(precisions_.size()) == A.rows());
+  // Accumulate row-wise contributions so constrained rows can mark infinite entries.
+  for (DenseIndex row = 0; row < A.rows(); ++row) {
+    const double precision = precisions_(row);
+    if (precision == 0.0) {
+      continue;
+    }
+    const auto a = A.row(row);
+    if (std::isinf(precision)) {
+      // Constrained rows force infinite information wherever they have support.
+      for (DenseIndex i = 0; i < a.cols(); ++i) {
+        if (std::abs(a(i)) <= kZeroTol) {
+          continue;
+        }
+        for (DenseIndex j = 0; j < a.cols(); ++j) {
+          if (std::abs(a(j)) <= kZeroTol) {
+            continue;
+          }
+          info(i, j) = kInfinity;
+        }
+      }
+    } else {
+      // Finite precisions reduce to the standard weighted outer product.
+      info.noalias() += precision * a.transpose() * a;
+    }
+  }
+  return info;
+}
+
 /* ************************************************************************* */
 Constrained::shared_ptr Constrained::unit() const {
   Vector sigmas = Vector::Ones(dim());

gtsam/linear/NoiseModel.h

@@ -408,8 +408,8 @@ namespace gtsam {
       Vector mu_; ///< Penalty function weight - needs to be large enough to dominate soft constraints
 
       /**
-       * Constructor that prevents any inf values
-       * from appearing in invsigmas or precisions.
+       * Constructor that prevents inf values from appearing in invsigmas,
+       * while preserving infinite precisions for constrained entries.
        * Allows for specifying mu.
        */
       Constrained(const Vector& mu, const Vector& sigmas);
@@ -439,25 +439,25 @@ namespace gtsam {
 
       /**
        * A diagonal noise model created by specifying a Vector of
-       * standard devations, some of which might be zero
+       * standard deviations, some of which might be zero
        */
       static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas);
 
       /**
        * A diagonal noise model created by specifying a Vector of
-       * standard devations, some of which might be zero
+       * standard deviations, some of which might be zero
        */
       static shared_ptr MixedSigmas(const Vector& sigmas);
 
       /**
        * A diagonal noise model created by specifying a Vector of
-       * standard devations, some of which might be zero
+       * standard deviations, some of which might be zero
        */
       static shared_ptr MixedSigmas(double m, const Vector& sigmas);
 
       /**
        * A diagonal noise model created by specifying a Vector of
-       * standard devations, some of which might be zero
+       * standard deviations, some of which might be zero
        */
       static shared_ptr MixedVariances(const Vector& mu, const Vector& variances);
       static shared_ptr MixedVariances(const Vector& variances);
@@ -470,7 +470,7 @@ namespace gtsam {
       static shared_ptr MixedPrecisions(const Vector& precisions);
 
       /**
-       * The squaredMahalanobisDistance function for a constrained noisemodel,
+       * The squaredMahalanobisDistance function for a constrained noise model,
        * for non-constrained versions, uses sigmas, otherwise
        * uses the penalty function with mu
        */
@@ -502,6 +502,12 @@ namespace gtsam {
       void WhitenInPlace(Matrix& H) const override;
       void WhitenInPlace(Eigen::Block<Matrix> H) const override;
 
+      /**
+       * Compute A' * diag(precisions) * A using constrained precisions.
+       * Infinite precisions yield infinite entries where the row has support.
+       */
+      Matrix informationFromA(const Matrix& A) const;
+
       /**
        * Apply QR factorization to the system [A b], taking into account constraints
        *               Q'  *   [A b]  =  [R d]
@@ -514,7 +520,7 @@ namespace gtsam {
       Diagonal::shared_ptr QR(Matrix& Ab) const override;
 
       /**
-       * Returns a Unit version of a constrained noisemodel in which
+       * Returns a Unit version of a constrained noise model in which
        * constrained sigmas remain constrained and the rest are unit scaled
        */
       shared_ptr unit() const;
@@ -559,7 +565,7 @@ namespace gtsam {
       typedef std::shared_ptr<Isotropic> shared_ptr;
 
       /**
-       * An isotropic noise model created by specifying a standard devation sigma
+       * An isotropic noise model created by specifying a standard deviation sigma
        */
       static shared_ptr Sigma(size_t dim, double sigma, bool smart = true);
 
@@ -719,7 +725,7 @@ namespace gtsam {
         return robust_->loss(std::sqrt(squared_distance));
       }
 
-      // NOTE: This is special because in whiten the base version will do the reweighting
+      // NOTE: This is special because in whiten the base version will do the re-weighting
       // which is incorrect!
       double squaredMahalanobisDistance(const Vector& v) const override {
         return noise_->squaredMahalanobisDistance(v);
@@ -773,5 +779,3 @@ namespace gtsam {
   template<> struct traits<noiseModel::Unit> : public Testable<noiseModel::Unit> {};
 
 } //\ namespace gtsam
-
-

gtsam/linear/tests/testNoiseModel.cpp

@@ -23,6 +23,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <cmath>
 #include <iostream>
 #include <limits>
 
@@ -129,9 +130,9 @@ TEST(NoiseModel, equals)
 ///* ************************************************************************* */
 //TEST(NoiseModel, ConstrainedSmart )
 //{
-//  Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vector3(sigma, 0.0, sigma), true);
-//  Diagonal::shared_ptr n1 = std::dynamic_pointer_cast<Diagonal>(nonconstrained);
-//  Constrained::shared_ptr n2 = std::dynamic_pointer_cast<Constrained>(nonconstrained);
+//  Gaussian::shared_ptr unconstrained = Constrained::MixedSigmas((Vector3(sigma, 0.0, sigma), true);
+//  Diagonal::shared_ptr n1 = std::dynamic_pointer_cast<Diagonal>(unconstrained);
+//  Constrained::shared_ptr n2 = std::dynamic_pointer_cast<Constrained>(unconstrained);
 //  EXPECT(n1);
 //  EXPECT(!n2);
 //
@@ -148,14 +149,15 @@ TEST(NoiseModel, ConstrainedConstructors )
   Constrained::shared_ptr actual;
   size_t d = 3;
   double m = 100.0;
+  const double kInfinity = std::numeric_limits<double>::infinity();
   Vector3 sigmas(kSigma, 0.0, 0.0);
   Vector3 mu(200.0, 300.0, 400.0);
   actual = Constrained::All(d);
   // TODO: why should this be a thousand ??? Dummy variable?
   EXPECT(assert_equal(Vector::Constant(d, 1000.0), actual->mu()));
   EXPECT(assert_equal(Vector::Constant(d, 0), actual->sigmas()));
   EXPECT(assert_equal(Vector::Constant(d, 0), actual->invsigmas())); // Actually zero as dummy value
-  EXPECT(assert_equal(Vector::Constant(d, 0), actual->precisions())); // Actually zero as dummy value
+  EXPECT(assert_equal(Vector::Constant(d, kInfinity), actual->precisions())); // Infinite precision for hard constraints
 
   actual = Constrained::All(d, m);
   EXPECT(assert_equal(Vector::Constant(d, m), actual->mu()));
@@ -201,6 +203,34 @@ TEST(NoiseModel, ConstrainedAll )
   DOUBLES_EQUAL(0.0, i->loss(0.0),1e-9);
 }
 
+/* ************************************************************************* */
+TEST(NoiseModel, ConstrainedInformationFromA) {
+  // Use one constrained row and one finite-precision row.
+  Vector2 sigmas(0.0, 2.0);
+  Constrained::shared_ptr model = Constrained::MixedSigmas(sigmas);
+
+  Matrix A(2, 2);
+  A << 1.0, 0.0, 0.0, 2.0;
+
+  Matrix info = model->informationFromA(A);
+
+  EXPECT(std::isinf(info(0, 0)));
+  DOUBLES_EQUAL(0.0, info(0, 1), 1e-12);
+  DOUBLES_EQUAL(0.0, info(1, 0), 1e-12);
+  DOUBLES_EQUAL(1.0, info(1, 1), 1e-12);
+
+  // Constrained row with support in multiple columns should mark cross-terms.
+  Matrix A_dense(2, 2);
+  A_dense << 1.0, 1.0, 0.0, 2.0;
+
+  Matrix info_dense = model->informationFromA(A_dense);
+
+  EXPECT(std::isinf(info_dense(0, 0)));
+  EXPECT(std::isinf(info_dense(0, 1)));
+  EXPECT(std::isinf(info_dense(1, 0)));
+  EXPECT(std::isinf(info_dense(1, 1)));
+}
+
 /* ************************************************************************* */
 namespace exampleQR {
   // create a matrix to eliminate

gtsam/nonlinear/Marginals.cpp

@@ -131,7 +131,15 @@ Matrix Marginals::marginalInformation(Key variable) const {
 
 /* ************************************************************************* */
 Matrix Marginals::marginalCovariance(Key variable) const {
-  return marginalInformation(variable).inverse();
+  Matrix info = marginalInformation(variable);
+
+  // Deterministic constraints (e.g., NonlinearEquality) produce infinite
+  // information for the constrained variable. Treat those as zero covariance
+  // rather than attempting to invert an infinite matrix.
+  if (!info.allFinite())
+    return Matrix::Zero(info.rows(), info.cols());
+
+  return info.inverse();
 }
 
 /* ************************************************************************* */