From f828f3cb2522e0e0c039a93259ac8a39905dacf1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 27 Nov 2021 18:14:49 -0500 Subject: [PATCH 1/9] wrap additional ISAM2 methods --- gtsam/nonlinear/nonlinear.i | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index ecf63094d6..79b63ff6ae 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -738,7 +738,14 @@ class ISAM2 { const gtsam::KeyList& extraReelimKeys, bool force_relinearize); + void marginalizeLeaves( + const gtsam::KeyList& leafKeys, + boost::optional marginalFactorIndices = + boost::none, + boost::optional deletedFactorsIndices = + boost::none); gtsam::Values getLinearizationPoint() const; + bool valueExists(gtsam::Key key) const; gtsam::Values calculateEstimate() const; template Date: Sun, 28 Nov 2021 09:48:50 -0500 Subject: [PATCH 2/9] comment out method --- gtsam/nonlinear/nonlinear.i | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 79b63ff6ae..928b3029f7 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -611,6 +611,16 @@ class ISAM2GaussNewtonParams { void setWildfireThreshold(double wildfireThreshold); }; +class ISAM2LevenbergMarquardtParams { + ISAM2LevenbergMarquardtParams(); + + void print(string s = "") const; + + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); +}; + class ISAM2DoglegParams { ISAM2DoglegParams(); @@ -738,12 +748,13 @@ class ISAM2 { const gtsam::KeyList& extraReelimKeys, bool force_relinearize); - void marginalizeLeaves( - const gtsam::KeyList& leafKeys, - boost::optional marginalFactorIndices = - boost::none, - boost::optional deletedFactorsIndices = - boost::none); + // void marginalizeLeaves( + // const gtsam::KeyList& leafKeys, + // boost::optional marginalFactorIndices = + // boost::none, + // boost::optional deletedFactorsIndices = + // boost::none); + gtsam::Values getLinearizationPoint() const; bool valueExists(gtsam::Key key) const; gtsam::Values calculateEstimate() const; From 007022df6f1601d2e6152fc910dc3c047b31811d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 28 Nov 2021 22:07:13 -0500 Subject: [PATCH 3/9] make noise model default constructors available for serialization --- gtsam/linear/NoiseModel.h | 42 +++++++++++++++++++-------------------- 1 file changed, 20 insertions(+), 22 deletions(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 2fb54d329a..6ad4444914 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -177,17 +177,16 @@ namespace gtsam { return *sqrt_information_; } - protected: - - /** protected constructor takes square root information matrix */ - Gaussian(size_t dim = 1, const boost::optional& sqrt_information = boost::none) : - Base(dim), sqrt_information_(sqrt_information) { - } public: typedef boost::shared_ptr shared_ptr; + /** constructor takes square root information matrix */ + Gaussian(size_t dim = 1, + const boost::optional& sqrt_information = boost::none) + : Base(dim), sqrt_information_(sqrt_information) {} + ~Gaussian() override {} /** @@ -290,13 +289,13 @@ namespace gtsam { Vector sigmas_, invsigmas_, precisions_; protected: - /** protected constructor - no initializations */ - Diagonal(); /** constructor to allow for disabling initialization of invsigmas */ Diagonal(const Vector& sigmas); public: + /** constructor - no initializations, for serialization */ + Diagonal(); typedef boost::shared_ptr shared_ptr; @@ -387,14 +386,6 @@ namespace gtsam { // Sigmas are contained in the base class Vector mu_; ///< Penalty function weight - needs to be large enough to dominate soft constraints - /** - * protected constructor takes sigmas. - * prevents any inf values - * from appearing in invsigmas or precisions. - * mu set to large default value (1000.0) - */ - Constrained(const Vector& sigmas = Z_1x1); - /** * Constructor that prevents any inf values * from appearing in invsigmas or precisions. @@ -406,6 +397,14 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; + /** + * protected constructor takes sigmas. + * prevents any inf values + * from appearing in invsigmas or precisions. + * mu set to large default value (1000.0) + */ + Constrained(const Vector& sigmas = Z_1x1); + ~Constrained() override {} /// true if a constrained noise mode, saves slow/clumsy dynamic casting @@ -531,11 +530,11 @@ namespace gtsam { Isotropic(size_t dim, double sigma) : Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {} + public: + /* dummy constructor to allow for serialization */ Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {} - public: - ~Isotropic() override {} typedef boost::shared_ptr shared_ptr; @@ -592,14 +591,13 @@ namespace gtsam { * Unit: i.i.d. unit-variance noise on all m dimensions. */ class GTSAM_EXPORT Unit : public Isotropic { - protected: - - Unit(size_t dim=1): Isotropic(dim,1.0) {} - public: typedef boost::shared_ptr shared_ptr; + /** constructor for serialization */ + Unit(size_t dim=1): Isotropic(dim,1.0) {} + ~Unit() override {} /** From 247d996a79026251c4678a1ecac90f76ba111666 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 28 Nov 2021 22:08:19 -0500 Subject: [PATCH 4/9] enable pickling for a bunch of objects --- gtsam/linear/linear.i | 15 +++++++++++++++ gtsam/navigation/navigation.i | 24 ++++++++++++++++++++++++ 2 files changed, 39 insertions(+) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index cf65b1a387..d882cb38b5 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -34,6 +34,9 @@ virtual class Gaussian : gtsam::noiseModel::Base { // enabling serialization functionality void serializable() const; + + // enable pickling in python + void pickle() const; }; virtual class Diagonal : gtsam::noiseModel::Gaussian { @@ -49,6 +52,9 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian { // enabling serialization functionality void serializable() const; + + // enable pickling in python + void pickle() const; }; virtual class Constrained : gtsam::noiseModel::Diagonal { @@ -66,6 +72,9 @@ virtual class Constrained : gtsam::noiseModel::Diagonal { // enabling serialization functionality void serializable() const; + + // enable pickling in python + void pickle() const; }; virtual class Isotropic : gtsam::noiseModel::Diagonal { @@ -78,6 +87,9 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal { // enabling serialization functionality void serializable() const; + + // enable pickling in python + void pickle() const; }; virtual class Unit : gtsam::noiseModel::Isotropic { @@ -85,6 +97,9 @@ virtual class Unit : gtsam::noiseModel::Isotropic { // enabling serialization functionality void serializable() const; + + // enable pickling in python + void pickle() const; }; namespace mEstimator { diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 7a879c3efa..1f9ffcf2e5 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -41,6 +41,12 @@ class ConstantBias { Vector gyroscope() const; Vector correctAccelerometer(Vector measurement) const; Vector correctGyroscope(Vector measurement) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; }; }///\namespace imuBias @@ -64,6 +70,12 @@ class NavState { gtsam::NavState retract(const Vector& x) const; Vector localCoordinates(const gtsam::NavState& g) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -106,6 +118,12 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { Matrix getAccelerometerCovariance() const; Matrix getIntegrationCovariance() const; bool getUse2ndOrderCoriolis() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -135,6 +153,12 @@ class PreintegratedImuMeasurements { Vector biasHatVector() const; gtsam::NavState predict(const gtsam::NavState& state_i, const gtsam::imuBias::ConstantBias& bias) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; }; virtual class ImuFactor: gtsam::NonlinearFactor { From 5f071585af49ad65eb2376cc80f39553070fdd6b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 28 Nov 2021 22:13:01 -0500 Subject: [PATCH 5/9] comment out incomplete code --- gtsam/nonlinear/nonlinear.i | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 928b3029f7..1f8bbc17d5 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -611,15 +611,15 @@ class ISAM2GaussNewtonParams { void setWildfireThreshold(double wildfireThreshold); }; -class ISAM2LevenbergMarquardtParams { - ISAM2LevenbergMarquardtParams(); +// class ISAM2LevenbergMarquardtParams { +// ISAM2LevenbergMarquardtParams(); - void print(string s = "") const; +// void print(string s = "") const; - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); -}; +// /** Getters and Setters for all properties */ +// double getWildfireThreshold() const; +// void setWildfireThreshold(double wildfireThreshold); +// }; class ISAM2DoglegParams { ISAM2DoglegParams(); From afadea6c1ddf1ff904e229d8dba259e3181115d1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Nov 2021 13:49:56 -0500 Subject: [PATCH 6/9] remove commented out code --- gtsam/nonlinear/nonlinear.i | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 1f8bbc17d5..4d81049ea7 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -611,16 +611,6 @@ class ISAM2GaussNewtonParams { void setWildfireThreshold(double wildfireThreshold); }; -// class ISAM2LevenbergMarquardtParams { -// ISAM2LevenbergMarquardtParams(); - -// void print(string s = "") const; - -// /** Getters and Setters for all properties */ -// double getWildfireThreshold() const; -// void setWildfireThreshold(double wildfireThreshold); -// }; - class ISAM2DoglegParams { ISAM2DoglegParams(); @@ -748,13 +738,6 @@ class ISAM2 { const gtsam::KeyList& extraReelimKeys, bool force_relinearize); - // void marginalizeLeaves( - // const gtsam::KeyList& leafKeys, - // boost::optional marginalFactorIndices = - // boost::none, - // boost::optional deletedFactorsIndices = - // boost::none); - gtsam::Values getLinearizationPoint() const; bool valueExists(gtsam::Key key) const; gtsam::Values calculateEstimate() const; From 1fe7822981c77f759f7d42a5ace271653e46c555 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Nov 2021 18:46:16 -0500 Subject: [PATCH 7/9] make LinearContainerFactor public for serialization --- gtsam/nonlinear/LinearContainerFactor.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 8c5b34f014..efc095775a 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -29,9 +29,6 @@ class LinearContainerFactor : public NonlinearFactor { GaussianFactor::shared_ptr factor_; boost::optional linearizationPoint_; - /** Default constructor - necessary for serialization */ - LinearContainerFactor() {} - /** direct copy constructor */ GTSAM_EXPORT LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional& linearizationPoint); @@ -43,6 +40,9 @@ class LinearContainerFactor : public NonlinearFactor { typedef boost::shared_ptr shared_ptr; + /** Default constructor - necessary for serialization */ + LinearContainerFactor() {} + /** Primary constructor: store a linear factor with optional linearization point */ GTSAM_EXPORT LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values()); From bee289880ab684017d0f3104620197e59dc20e81 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Nov 2021 19:39:41 -0500 Subject: [PATCH 8/9] wrap other ISAM2 methods --- gtsam/nonlinear/nonlinear.i | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 4d81049ea7..aa595d5445 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -749,12 +749,17 @@ class ISAM2 { gtsam::PinholeCamera, gtsam::PinholeCamera, Vector, Matrix}> VALUE calculateEstimate(size_t key) const; - gtsam::Values calculateBestEstimate() const; Matrix marginalCovariance(size_t key) const; + gtsam::Values calculateBestEstimate() const; gtsam::VectorValues getDelta() const; + double error(const gtsam::VectorValues& x) const; gtsam::NonlinearFactorGraph getFactorsUnsafe() const; gtsam::VariableIndex getVariableIndex() const; + const gtsam::KeySet& getFixedVariables() const; gtsam::ISAM2Params params() const; + + void printStats() const; + gtsam::VectorValues gradientAtZero() const; }; #include From 0b89d2d7abf328214f1bf5478b67a9c260add087 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Nov 2021 19:52:49 -0500 Subject: [PATCH 9/9] wrap alternate ISAM2::update method --- gtsam/nonlinear/nonlinear.i | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index aa595d5445..152c4b8e74 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -738,6 +738,10 @@ class ISAM2 { const gtsam::KeyList& extraReelimKeys, bool force_relinearize); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::ISAM2UpdateParams& updateParams); + gtsam::Values getLinearizationPoint() const; bool valueExists(gtsam::Key key) const; gtsam::Values calculateEstimate() const;