Fixed lag and python

gtsam/nonlinear/FixedLagSmoother.h

@@ -51,6 +51,9 @@ class GTSAM_EXPORT FixedLagSmoother {
     size_t nonlinearVariables; ///< The number of variables that can be relinearized
     size_t linearVariables; ///< The number of variables that must keep a constant linearization point
     double error; ///< The final factor graph error
+    FactorIndices marginalFactorIndices; ///< Indices added during the marginalizeLeaves step
+    FactorIndices deletedFactorIndices; ///< Indices removed during the marginalizeLeaves step
+    KeySet keysOfDeletedNodes; ///< Keys of nodes removed during the marginalizeLeaves step
     Result() : iterations(0), intermediateSteps(0), nonlinearVariables(0), linearVariables(0), error(0) {}
 
     /// Getter methods
@@ -59,6 +62,9 @@ class GTSAM_EXPORT FixedLagSmoother {
     size_t getNonlinearVariables() const { return nonlinearVariables; }
     size_t getLinearVariables() const { return linearVariables; }
     double getError() const { return error; }
+    FactorIndices getMarginalFactorIndices() const { return marginalFactorIndices; }
+    FactorIndices getDeletedFactorIndices() const { return deletedFactorIndices; }
+    KeySet getKeysOfDeletedNodes() const { return keysOfDeletedNodes; }
     void print() const;
   };
 
@@ -86,6 +92,11 @@ class GTSAM_EXPORT FixedLagSmoother {
     return smootherLag_;
   }
 
+  /** Write to the current smoother lag. Made for python bindings. */
+  void setSmootherLag(double smootherLag) {
+    smootherLag_ = smootherLag;
+  }
+
   /** Access the current set of timestamps associated with each variable */
   const KeyTimestampMap& timestamps() const {
     return keyTimestampMap_;

gtsam/nonlinear/ISAM2.cpp

@@ -768,6 +768,22 @@ void ISAM2::marginalizeLeaves(
   }
 }
 
+/** An added function specifically to return the marginalFactorIndices
+ * and deletedFactorIndices. Made for the python wrapping
+ * of marginalizeLeaves
+ */
+std::pair<FactorIndices, FactorIndices> ISAM2::marginalizeLeavesWithIndices(const FastList<Key>& leafKeys) {
+  FactorIndices marginal;
+  FactorIndices deleted;
+
+  marginalizeLeaves(
+      leafKeys,
+      &marginal,
+      &deleted);
+
+  return {marginal, deleted};
+}
+
 /* ************************************************************************* */
 // Marked const but actually changes mutable delta
 void ISAM2::updateDelta(bool forceFullSolve) const {

gtsam/nonlinear/ISAM2.h

@@ -216,6 +216,12 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
           marginalizeLeaves(leafKeys, (&optArgs)...);
       }
 
+  /** An added function specifically to return the marginalFactorIndices
+   * and deletedFactorIndices. Made for the python wrapping
+   * of marginalizeLeaves
+   */
+  std::pair<FactorIndices, FactorIndices> marginalizeLeavesWithIndices(const FastList<Key>& leafKeys);
+
   /// Access the current linearization point
   const Values& getLinearizationPoint() const { return theta_; }
 

gtsam/nonlinear/IncrementalFixedLagSmoother.cpp

@@ -138,10 +138,12 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
   }
 
   // Marginalize out any needed variables
+  FactorIndices marginalFactorsIndices;  
+  FactorIndices deletedFactorsIndices; 
   if (marginalizableKeys.size() > 0) {
     FastList<Key> leafKeys(marginalizableKeys.begin(),
         marginalizableKeys.end());
-    isam_.marginalizeLeaves(leafKeys);
+    isam_.marginalizeLeaves(leafKeys, &marginalFactorsIndices, &deletedFactorsIndices);
   }
 
   // Remove marginalized keys from the KeyTimestampMap
@@ -158,6 +160,9 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
   result.linearVariables = 0;
   result.nonlinearVariables = 0;
   result.error = 0;
+  result.marginalFactorIndices = marginalFactorsIndices;
+  result.deletedFactorIndices = deletedFactorsIndices;
+  result.keysOfDeletedNodes = KeySet(marginalizableKeys);
 
   if (debug)
     std::cout << "IncrementalFixedLagSmoother::update() Finish" << std::endl;

gtsam/nonlinear/nonlinear.i

@@ -621,6 +621,9 @@ class ISAM2 {
                  gtsam::DefaultKeyFormatter) const;
   void saveGraph(string s, const gtsam::KeyFormatter& keyFormatter =
                                gtsam::DefaultKeyFormatter) const;
+  void marginalizeLeaves(const gtsam::FastList<gtsam::Key>& leafKeys);
+  std::pair<gtsam::FactorIndices, gtsam::FactorIndices>
+  marginalizeLeavesWithIndices(const gtsam::FastList<gtsam::Key>& leafKeys);
 };
 
 #include <gtsam/nonlinear/NonlinearISAM.h>
@@ -963,6 +966,10 @@ class FixedLagSmootherResult {
   size_t getNonlinearVariables() const;
   size_t getLinearVariables() const;
   double getError() const;
+  FactorIndices getMarginalFactorIndices() const;
+  FactorIndices getDeletedFactorIndices() const;
+  KeySet getKeysOfDeletedNodes() const;
+  void print() const;
 };
 
 virtual class FixedLagSmoother {
@@ -971,6 +978,7 @@ virtual class FixedLagSmoother {
 
   gtsam::FixedLagSmootherKeyTimestampMap timestamps() const;
   double smootherLag() const;
+  void setSmootherLag(double smootherLag);
 
   gtsam::FixedLagSmootherResult update(
       const gtsam::NonlinearFactorGraph& newFactors,
@@ -1018,6 +1026,7 @@ virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
 
   gtsam::NonlinearFactorGraph getFactors() const;
   gtsam::ISAM2 getISAM2() const;
+  ISAM2Result& getISAM2Result() const;
 };
 
 #include <gtsam/nonlinear/ExtendedKalmanFilter.h>