Merge pull request #1053 from senselessDev/develop

borglab · Feb 1, 2022 · ad0d7e4 · ad0d7e4
2 parents 236dbcb + 7526329
commit ad0d7e4
Show file tree

Hide file tree

Showing 16 changed files with 40 additions and 53 deletions.
diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h
@@ -300,50 +300,23 @@ struct GTSAM_EXPORT ISAM2Params {
   RelinearizationThreshold getRelinearizeThreshold() const {
     return relinearizeThreshold;
   }
-  int getRelinearizeSkip() const { return relinearizeSkip; }
-  bool isEnableRelinearization() const { return enableRelinearization; }
-  bool isEvaluateNonlinearError() const { return evaluateNonlinearError; }
   std::string getFactorization() const {
     return factorizationTranslator(factorization);
   }
-  bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; }
   KeyFormatter getKeyFormatter() const { return keyFormatter; }
-  bool isEnableDetailedResults() const { return enableDetailedResults; }
-  bool isEnablePartialRelinearizationCheck() const {
-    return enablePartialRelinearizationCheck;
-  }
 
   void setOptimizationParams(OptimizationParams optimizationParams) {
     this->optimizationParams = optimizationParams;
   }
   void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) {
     this->relinearizeThreshold = relinearizeThreshold;
   }
-  void setRelinearizeSkip(int relinearizeSkip) {
-    this->relinearizeSkip = relinearizeSkip;
-  }
-  void setEnableRelinearization(bool enableRelinearization) {
-    this->enableRelinearization = enableRelinearization;
-  }
-  void setEvaluateNonlinearError(bool evaluateNonlinearError) {
-    this->evaluateNonlinearError = evaluateNonlinearError;
-  }
   void setFactorization(const std::string& factorization) {
     this->factorization = factorizationTranslator(factorization);
   }
-  void setCacheLinearizedFactors(bool cacheLinearizedFactors) {
-    this->cacheLinearizedFactors = cacheLinearizedFactors;
-  }
   void setKeyFormatter(KeyFormatter keyFormatter) {
     this->keyFormatter = keyFormatter;
   }
-  void setEnableDetailedResults(bool enableDetailedResults) {
-    this->enableDetailedResults = enableDetailedResults;
-  }
-  void setEnablePartialRelinearizationCheck(
-      bool enablePartialRelinearizationCheck) {
-    this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck;
-  }
 
   GaussianFactorGraph::Eliminate getEliminationFunction() const {
     return factorization == CHOLESKY

diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
@@ -588,21 +588,19 @@ class ISAM2Params {
   void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params);
   void setRelinearizeThreshold(double threshold);
   void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map);
-  int getRelinearizeSkip() const;
-  void setRelinearizeSkip(int relinearizeSkip);
-  bool isEnableRelinearization() const;
-  void setEnableRelinearization(bool enableRelinearization);
-  bool isEvaluateNonlinearError() const;
-  void setEvaluateNonlinearError(bool evaluateNonlinearError);
   string getFactorization() const;
   void setFactorization(string factorization);
-  bool isCacheLinearizedFactors() const;
-  void setCacheLinearizedFactors(bool cacheLinearizedFactors);
-  bool isEnableDetailedResults() const;
-  void setEnableDetailedResults(bool enableDetailedResults);
-  bool isEnablePartialRelinearizationCheck() const;
-  void setEnablePartialRelinearizationCheck(
-      bool enablePartialRelinearizationCheck);
+
+  int relinearizeSkip;
+  bool enableRelinearization;
+  bool evaluateNonlinearError;
+  bool cacheLinearizedFactors;
+  bool enableDetailedResults;
+  bool enablePartialRelinearizationCheck;
+  bool findUnusedFactorSlots;
+
+  enum Factorization { CHOLESKY, QR };
+  Factorization factorization;
 };
 
 class ISAM2Clique {

diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i
@@ -566,7 +566,13 @@ virtual class FixedLagSmoother {
   gtsam::FixedLagSmootherKeyTimestampMap timestamps() const;
   double smootherLag() const;
 
-  gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps);
+  gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors,
+                                       const gtsam::Values &newTheta,
+                                       const gtsam::FixedLagSmootherKeyTimestampMap &timestamps);
+  gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors,
+                                       const gtsam::Values &newTheta,
+                                       const gtsam::FixedLagSmootherKeyTimestampMap &timestamps,
+                                       const gtsam::FactorIndices &factorsToRemove);
   gtsam::Values calculateEstimate() const;
 };
 
@@ -576,6 +582,8 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
   BatchFixedLagSmoother(double smootherLag);
   BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params);
 
+  void print(string s = "BatchFixedLagSmoother:\n") const;
+
   gtsam::LevenbergMarquardtParams params() const;
   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
@@ -589,7 +597,12 @@ virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
   IncrementalFixedLagSmoother(double smootherLag);
   IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);
 
+  void print(string s = "IncrementalFixedLagSmoother:\n") const;
+
   gtsam::ISAM2Params params() const;
+
+  gtsam::NonlinearFactorGraph getFactors() const;
+  gtsam::ISAM2 getISAM2() const;
 };
 
 #include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>

diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
@@ -113,6 +113,9 @@ class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother
   /// Get results of latest isam2 update
   const ISAM2Result& getISAM2Result() const{ return isamResult_; }
 
+  /// Get the iSAM2 object which is used for the inference internally
+  const ISAM2& getISAM2() const { return isam_; }
+
 protected:
 
   /** Create default parameters */

diff --git a/matlab/+gtsam/VisualISAMInitialize.m b/matlab/+gtsam/VisualISAMInitialize.m
@@ -7,7 +7,7 @@
 %% Initialize iSAM
 params = gtsam.ISAM2Params;
 if options.alwaysRelinearize
-    params.setRelinearizeSkip(1);
+    params.relinearizeSkip = 1;
 end
 isam = ISAM2(params);
 

diff --git a/matlab/gtsam_examples/IMUKittiExampleGPS.m b/matlab/gtsam_examples/IMUKittiExampleGPS.m
@@ -52,7 +52,7 @@
 %% Solver object
 isamParams = ISAM2Params;
 isamParams.setFactorization('CHOLESKY');
-isamParams.setRelinearizeSkip(10);
+isamParams.relinearizeSkip = 10;
 isam = gtsam.ISAM2(isamParams);
 newFactors = NonlinearFactorGraph;
 newValues = Values;

diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison.m b/matlab/unstable_examples/+imuSimulator/IMUComparison.m
@@ -46,7 +46,7 @@
 
 %% Solver object
 isamParams = ISAM2Params;
-isamParams.setRelinearizeSkip(1);
+isamParams.relinearizeSkip = 1;
 isam = gtsam.ISAM2(isamParams);
 
 initialValues = Values;

diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m
@@ -34,7 +34,7 @@
 
 %% Solver object
 isamParams = ISAM2Params;
-isamParams.setRelinearizeSkip(1);
+isamParams.relinearizeSkip = 1;
 isam = gtsam.ISAM2(isamParams);
 
 sigma_init_x = 1.0;

diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m
@@ -119,7 +119,7 @@
 % Solver object
 isamParams = ISAM2Params;
 isamParams.setFactorization('CHOLESKY');
-isamParams.setRelinearizeSkip(10);
+isamParams.relinearizeSkip = 10;
 isam = gtsam.ISAM2(isamParams);
 newFactors = NonlinearFactorGraph;
 newValues = Values;

diff --git a/matlab/unstable_examples/IMUKittiExampleAdvanced.m b/matlab/unstable_examples/IMUKittiExampleAdvanced.m
@@ -82,7 +82,7 @@
 %% Solver object
 isamParams = ISAM2Params;
 isamParams.setFactorization('QR');
-isamParams.setRelinearizeSkip(1);
+isamParams.relinearizeSkip = 1;
 isam = gtsam.ISAM2(isamParams);
 newFactors = NonlinearFactorGraph;
 newValues = Values;

diff --git a/matlab/unstable_examples/IMUKittiExampleVO.m b/matlab/unstable_examples/IMUKittiExampleVO.m
@@ -58,7 +58,7 @@
 %% Solver object
 isamParams = ISAM2Params;
 isamParams.setFactorization('CHOLESKY');
-isamParams.setRelinearizeSkip(10);
+isamParams.relinearizeSkip = 10;
 isam = gtsam.ISAM2(isamParams);
 newFactors = NonlinearFactorGraph;
 newValues = Values;

diff --git a/python/gtsam/examples/IMUKittiExampleGPS.py b/python/gtsam/examples/IMUKittiExampleGPS.py
@@ -203,7 +203,7 @@ def optimize(gps_measurements: List[GpsMeasurement],
     # Set ISAM2 parameters and create ISAM2 solver object
     isam_params = gtsam.ISAM2Params()
     isam_params.setFactorization("CHOLESKY")
-    isam_params.setRelinearizeSkip(10)
+    isam_params.relinearizeSkip = 10
 
     isam = gtsam.ISAM2(isam_params)
 

diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py
@@ -111,7 +111,7 @@ def Pose2SLAM_ISAM2_example():
     # update calls are required to perform the relinearization.
     parameters = gtsam.ISAM2Params()
     parameters.setRelinearizeThreshold(0.1)
-    parameters.setRelinearizeSkip(1)
+    parameters.relinearizeSkip = 1
     isam = gtsam.ISAM2(parameters)
 
     # Create the ground truth odometry measurements of the robot during the trajectory.

diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py
@@ -140,7 +140,7 @@ def Pose3_ISAM2_example():
     # update calls are required to perform the relinearization.
     parameters = gtsam.ISAM2Params()
     parameters.setRelinearizeThreshold(0.1)
-    parameters.setRelinearizeSkip(1)
+    parameters.relinearizeSkip = 1
     isam = gtsam.ISAM2(parameters)
 
     # Create the ground truth poses of the robot trajectory.

diff --git a/python/gtsam/examples/VisualISAM2Example.py b/python/gtsam/examples/VisualISAM2Example.py
@@ -81,7 +81,7 @@ def visual_ISAM2_example():
     # will approach the batch result.
     parameters = gtsam.ISAM2Params()
     parameters.setRelinearizeThreshold(0.01)
-    parameters.setRelinearizeSkip(1)
+    parameters.relinearizeSkip = 1
     isam = gtsam.ISAM2(parameters)
 
     # Create a Factor Graph and Values to hold the new data

diff --git a/python/gtsam/utils/visual_isam.py b/python/gtsam/utils/visual_isam.py
@@ -17,7 +17,7 @@ def initialize(data, truth, options):
     # Initialize iSAM
     params = gtsam.ISAM2Params()
     if options.alwaysRelinearize:
-        params.setRelinearizeSkip(1)
+        params.relinearizeSkip = 1
     isam = gtsam.ISAM2(params=params)
 
     # Add constraints/priors