New FastNoiseModelFactor with more efficient linearization

Author: ProfFan

gtsam/base/OptionalJacobian.h

@@ -120,7 +120,27 @@ class OptionalJacobian {
                       "Expected: ") +
           "(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")");
     }
-  } 
+  }
+
+  /**
+   * @brief Constructor from an Eigen::Ref *value*. Will not usurp if dimension is wrong
+   * @note This is important so we don't overwrite someone else's memory!
+   */
+  template<class MATRIX>
+  OptionalJacobian(Eigen::Ref<MATRIX>* dynamic_optional) :
+      map_(nullptr) {
+    if (!dynamic_optional) return; // stay empty
+    Eigen::Ref<MATRIX>& dynamic_ref = *dynamic_optional;
+    if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && !dynamic_ref.IsRowMajor) {
+      usurp(dynamic_ref.data());
+    } else {
+      throw std::invalid_argument(
+          std::string("OptionalJacobian called with wrong dimensions or "
+                      "storage order.\n"
+                      "Expected: ") +
+          "(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")");
+    }
+  }
 
   /// Constructor with std::nullopt just makes empty
   OptionalJacobian(std::nullopt_t /*none*/) :

gtsam/linear/GaussianConditional.cpp

@@ -322,7 +322,7 @@ namespace gtsam {
     for (auto&& key : parents()) newKeys.push_back(key);
 
     // Hopefully second newAb copy below is optimized out...
-    return std::make_shared<JacobianFactor>(newKeys, newAb, model_);
+    return std::make_shared<JacobianFactor>(newKeys, std::move(newAb), model_);
   }
 
   /* **************************************************************************/

gtsam/linear/JacobianFactor-inl.h

@@ -34,20 +34,15 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  template <typename KEYS>
-  JacobianFactor::JacobianFactor(const KEYS& keys,
-                                 const VerticalBlockMatrix& augmentedMatrix,
+  template <
+      typename KEYS, typename MATRIX,
+      std::enable_if_t<
+          std::is_same<std::decay_t<MATRIX>, VerticalBlockMatrix>::value, bool>>
+  JacobianFactor::JacobianFactor(KEYS&& keys, MATRIX&& augmentedMatrix,
                                  const SharedDiagonal& model)
-      : Base(keys), Ab_(augmentedMatrix), model_(model) {
-    checkAb(model, augmentedMatrix);
-  }
-
-  /* ************************************************************************* */
-  template <typename KEYS>
-  JacobianFactor::JacobianFactor(const KEYS& keys,
-                                 VerticalBlockMatrix&& augmentedMatrix,
-                                 const SharedDiagonal& model)
-      : Base(keys), Ab_(std::move(augmentedMatrix)), model_(model) {
+      : Base(std::forward<KEYS>(keys)),
+        Ab_(std::forward<MATRIX>(augmentedMatrix)),
+        model_(model) {
     checkAb(model, Ab_);
   }
 

gtsam/linear/JacobianFactor.h

@@ -145,17 +145,13 @@ namespace gtsam {
     template<typename TERMS>
     JacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal());
 
-    /** Constructor with arbitrary number keys, and where the augmented matrix
-     * is given all together instead of in block terms.
-     */
-    template <typename KEYS>
-    JacobianFactor(const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix,
-                   const SharedDiagonal& sigmas = SharedDiagonal());
-
     /** Construct with an rvalue VerticalBlockMatrix, to allow std::move. */
-    template <typename KEYS>
-    JacobianFactor(const KEYS& keys, VerticalBlockMatrix&& augmentedMatrix,
-                   const SharedDiagonal& model);
+    template <typename KEYS, typename MATRIX,
+              std::enable_if_t<std::is_same<std::decay_t<MATRIX>,
+                                            VerticalBlockMatrix>::value,
+                               bool> = true>
+    JacobianFactor(KEYS&& keys, MATRIX&& augmentedMatrix,
+                   const SharedDiagonal& model = SharedDiagonal());
 
     /**
      * Build a dense joint factor from all the factors in a factor graph.  If a VariableSlots

gtsam/linear/NoiseModel.h

@@ -123,12 +123,12 @@ namespace gtsam {
       }
 
       /** in-place whiten, override if can be done more efficiently */
-      virtual void whitenInPlace(Eigen::Block<Vector>& v) const {
-        v = whiten(v);
+      virtual void whitenInPlace(Eigen::Block<Matrix> v) const {
+        v = Whiten(v);
       }
 
       /** in-place unwhiten, override if can be done more efficiently */
-      virtual void unwhitenInPlace(Eigen::Block<Vector>& v) const {
+      virtual void unwhitenInPlace(Eigen::Block<Matrix> v) const {
         v = unwhiten(v);
       }
 
@@ -644,8 +644,8 @@ namespace gtsam {
       void WhitenInPlace(Eigen::Block<Matrix> /*H*/) const override {}
       void whitenInPlace(Vector& /*v*/) const override {}
       void unwhitenInPlace(Vector& /*v*/) const override {}
-      void whitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
-      void unwhitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
+      void whitenInPlace(Eigen::Block<Matrix> /*v*/) const override {}
+      void unwhitenInPlace(Eigen::Block<Matrix> /*v*/) const override {}
 
     private:
 #if GTSAM_ENABLE_BOOST_SERIALIZATION

gtsam/navigation/ImuBias.cpp

@@ -66,8 +66,8 @@ namespace imuBias {
 //    }
 /// ostream operator
 std::ostream& operator<<(std::ostream& os, const ConstantBias& bias) {
-  os << "acc = " << bias.accelerometer().transpose();
-  os << " gyro = " << bias.gyroscope().transpose();
+  os << "acc = " << bias.accelerometer().transpose().eval();
+  os << " gyro = " << bias.gyroscope().transpose().eval();
   return os;
 }