From 0814efafd7be9a93e29911613c0e2089ec55de4a Mon Sep 17 00:00:00 2001 From: senselessDev Date: Sat, 22 Jan 2022 15:35:53 +0100 Subject: [PATCH 1/8] expose some iSAM2/FixedLagSmoothing functionality for the bindings --- gtsam/nonlinear/ISAM2Params.h | 7 +++++++ gtsam/nonlinear/nonlinear.i | 3 +++ gtsam_unstable/gtsam_unstable.i | 6 ++++++ 3 files changed, 16 insertions(+) diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h index c6e1001c4..a939ab691 100644 --- a/gtsam/nonlinear/ISAM2Params.h +++ b/gtsam/nonlinear/ISAM2Params.h @@ -313,6 +313,10 @@ struct GTSAM_EXPORT ISAM2Params { return enablePartialRelinearizationCheck; } + bool isFindUnusedFactorSlots() const { + return findUnusedFactorSlots; + } + void setOptimizationParams(OptimizationParams optimizationParams) { this->optimizationParams = optimizationParams; } @@ -344,6 +348,9 @@ struct GTSAM_EXPORT ISAM2Params { bool enablePartialRelinearizationCheck) { this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; } + void setFindUnusedFactorSlots(bool findUnusedFactorSlots) { + this->findUnusedFactorSlots = findUnusedFactorSlots; + } GaussianFactorGraph::Eliminate getEliminationFunction() const { return factorization == CHOLESKY diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 84c4939f4..bc07c334b 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -696,6 +696,9 @@ class ISAM2Params { bool isEnablePartialRelinearizationCheck() const; void setEnablePartialRelinearizationCheck( bool enablePartialRelinearizationCheck); + bool isFindUnusedFactorSlots() const; + void setFindUnusedFactorSlots(bool findUnusedFactorSlots); + }; class ISAM2Clique { diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 8c9345147..b7de97be3 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -567,6 +567,10 @@ virtual class FixedLagSmoother { 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 ×tamps, + const gtsam::FactorIndices &factorsToRemove); gtsam::Values calculateEstimate() const; }; @@ -590,6 +594,8 @@ virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); gtsam::ISAM2Params params() const; + + gtsam::NonlinearFactorGraph getFactors() const; }; #include From bcbd26c4fbc443dbef8823ca62df17853d3e5ad6 Mon Sep 17 00:00:00 2001 From: senselessDev Date: Sat, 22 Jan 2022 18:54:52 +0100 Subject: [PATCH 2/8] use direct memeber access for ISAM2Params --- gtsam/nonlinear/ISAM2Params.h | 33 --------------------------------- gtsam/nonlinear/nonlinear.i | 22 +++++++--------------- 2 files changed, 7 insertions(+), 48 deletions(-) diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h index a939ab691..996addd47 100644 --- a/gtsam/nonlinear/ISAM2Params.h +++ b/gtsam/nonlinear/ISAM2Params.h @@ -300,22 +300,11 @@ 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; - } - - bool isFindUnusedFactorSlots() const { - return findUnusedFactorSlots; - } void setOptimizationParams(OptimizationParams optimizationParams) { this->optimizationParams = optimizationParams; @@ -323,34 +312,12 @@ struct GTSAM_EXPORT ISAM2Params { 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; - } - void setFindUnusedFactorSlots(bool findUnusedFactorSlots) { - this->findUnusedFactorSlots = findUnusedFactorSlots; - } GaussianFactorGraph::Eliminate getEliminationFunction() const { return factorization == CHOLESKY diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index bc07c334b..bffe91eba 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -681,24 +681,16 @@ 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); - bool isFindUnusedFactorSlots() const; - void setFindUnusedFactorSlots(bool findUnusedFactorSlots); + int relinearizeSkip; + bool enableRelinearization; + bool evaluateNonlinearError; + bool cacheLinearizedFactors; + bool enableDetailedResults; + bool enablePartialRelinearizationCheck; + bool findUnusedFactorSlots; }; class ISAM2Clique { From 305300848bd2ab0adb502d85bb8c28681123f67c Mon Sep 17 00:00:00 2001 From: senselessDev Date: Sat, 22 Jan 2022 19:06:17 +0100 Subject: [PATCH 3/8] use default for parameter instead of overloading --- gtsam_unstable/gtsam_unstable.i | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index b7de97be3..35d2da40f 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -566,11 +566,10 @@ 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 ×tamps, - const gtsam::FactorIndices &factorsToRemove); + const gtsam::FactorIndices &factorsToRemove=gtsam::FactorIndices()); gtsam::Values calculateEstimate() const; }; From dbfc7bb4959f3c4a2217ba7d393f3475aa5c7a34 Mon Sep 17 00:00:00 2001 From: senselessDev Date: Sat, 29 Jan 2022 22:13:23 +0100 Subject: [PATCH 4/8] revert FactorIndices default argument to overloading for now * FactorIndices default argument is currently not easily available in binding code * see https://github.com/borglab/gtsam/pull/1053#issuecomment-1019345941 --- gtsam_unstable/gtsam_unstable.i | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 35d2da40f..7d5241771 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -566,10 +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 ×tamps); gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta, const gtsam::FixedLagSmootherKeyTimestampMap ×tamps, - const gtsam::FactorIndices &factorsToRemove=gtsam::FactorIndices()); + const gtsam::FactorIndices &factorsToRemove); gtsam::Values calculateEstimate() const; }; From 11b2135eb371988107777c9067fb4d0929087427 Mon Sep 17 00:00:00 2001 From: senselessDev Date: Sat, 29 Jan 2022 22:21:57 +0100 Subject: [PATCH 5/8] remove last overseen stub for bindings --- gtsam/nonlinear/ISAM2Params.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h index 996addd47..d88afd505 100644 --- a/gtsam/nonlinear/ISAM2Params.h +++ b/gtsam/nonlinear/ISAM2Params.h @@ -303,7 +303,6 @@ struct GTSAM_EXPORT ISAM2Params { std::string getFactorization() const { return factorizationTranslator(factorization); } - bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; } KeyFormatter getKeyFormatter() const { return keyFormatter; } void setOptimizationParams(OptimizationParams optimizationParams) { From 65d72ab7a1c6590bef77f051e7b7c69a49ff43f7 Mon Sep 17 00:00:00 2001 From: senselessDev Date: Sat, 29 Jan 2022 22:35:24 +0100 Subject: [PATCH 6/8] adapt examples to new direct member access --- matlab/+gtsam/VisualISAMInitialize.m | 2 +- matlab/gtsam_examples/IMUKittiExampleGPS.m | 2 +- matlab/unstable_examples/+imuSimulator/IMUComparison.m | 2 +- matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m | 2 +- matlab/unstable_examples/+imuSimulator/coriolisExample.m | 2 +- matlab/unstable_examples/IMUKittiExampleAdvanced.m | 2 +- matlab/unstable_examples/IMUKittiExampleVO.m | 2 +- python/gtsam/examples/IMUKittiExampleGPS.py | 2 +- python/gtsam/examples/Pose2ISAM2Example.py | 2 +- python/gtsam/examples/Pose3ISAM2Example.py | 2 +- python/gtsam/examples/VisualISAM2Example.py | 2 +- python/gtsam/utils/visual_isam.py | 2 +- 12 files changed, 12 insertions(+), 12 deletions(-) diff --git a/matlab/+gtsam/VisualISAMInitialize.m b/matlab/+gtsam/VisualISAMInitialize.m index 9b834e3e1..560503345 100644 --- a/matlab/+gtsam/VisualISAMInitialize.m +++ b/matlab/+gtsam/VisualISAMInitialize.m @@ -7,7 +7,7 @@ import gtsam.* %% 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 index 530c3382c..b350618e5 100755 --- a/matlab/gtsam_examples/IMUKittiExampleGPS.m +++ b/matlab/gtsam_examples/IMUKittiExampleGPS.m @@ -52,7 +52,7 @@ IMU_params.setOmegaCoriolis(w_coriolis); %% 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 index ccc975d84..b753916c6 100644 --- a/matlab/unstable_examples/+imuSimulator/IMUComparison.m +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison.m @@ -46,7 +46,7 @@ posesIMUbody(1).R = poses(1).R; %% 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 index 6adc8e9dc..689d8a3f5 100644 --- a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m @@ -34,7 +34,7 @@ poses(1).R = currentPoseGlobal.rotation.matrix; %% 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 index 61dc78d96..dd276e2c1 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -119,7 +119,7 @@ h = figure; % 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 index cb13eacee..b09764ec0 100644 --- a/matlab/unstable_examples/IMUKittiExampleAdvanced.m +++ b/matlab/unstable_examples/IMUKittiExampleAdvanced.m @@ -82,7 +82,7 @@ w_coriolis = [0;0;0]; %% 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 index f35d36512..4183e439a 100644 --- a/matlab/unstable_examples/IMUKittiExampleVO.m +++ b/matlab/unstable_examples/IMUKittiExampleVO.m @@ -58,7 +58,7 @@ w_coriolis = [0;0;0]; %% 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 index 8b6b4fdf0..d00f633ba 100644 --- 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 index c70dbfa6f..3a8de0317 100644 --- 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 index 59b9fb2ac..cb71813c5 100644 --- 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 index bacf510ec..4b480fab7 100644 --- 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 index a8fed4b23..4ebd8accd 100644 --- 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 From 40d3badf1f445977bedd5e175e2082f43127308c Mon Sep 17 00:00:00 2001 From: senselessDev Date: Sat, 29 Jan 2022 23:06:53 +0100 Subject: [PATCH 7/8] expose iSAM2 parameter factorization enum --- gtsam/nonlinear/nonlinear.i | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 293b666ae..68d30c56c 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -598,6 +598,9 @@ class ISAM2Params { bool enableDetailedResults; bool enablePartialRelinearizationCheck; bool findUnusedFactorSlots; + + enum Factorization { CHOLESKY, QR }; + Factorization factorization; }; class ISAM2Clique { From 75263296b37945d2473a753e969e5fe5e258e990 Mon Sep 17 00:00:00 2001 From: senselessDev Date: Sat, 29 Jan 2022 23:31:07 +0100 Subject: [PATCH 8/8] expose iSAM object for iFLS, fix __repr__ of iFLS, bFLS --- gtsam_unstable/gtsam_unstable.i | 5 +++++ gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h | 3 +++ 2 files changed, 8 insertions(+) diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 7d5241771..dd66e7a73 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -582,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 diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 79c05a01a..4079dbb23 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -113,6 +113,9 @@ public: /// 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 */