From 2ce0b57f0525c5413cf2483a3316c9a9911d29c4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 20 Feb 2023 08:20:16 -0800 Subject: [PATCH] Fixed python wrapper --- gtsam/nonlinear/nonlinear.i | 61 ++++++++++++++++++ gtsam_unstable/gtsam_unstable.i | 63 +------------------ python/CMakeLists.txt | 2 +- .../examples/FixedLagSmootherExample.py | 4 +- .../tests/test_FixedLagSmootherExample.py | 4 +- 5 files changed, 68 insertions(+), 66 deletions(-) rename python/{gtsam_unstable => gtsam}/examples/FixedLagSmootherExample.py (95%) rename python/{gtsam_unstable => gtsam}/tests/test_FixedLagSmootherExample.py (96%) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index e7a05cf62..19f4ae588 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -650,4 +650,65 @@ virtual class NonlinearEquality2 : gtsam::NoiseModelFactor { gtsam::Vector evaluateError(const T& x1, const T& x2); }; +#include +class FixedLagSmootherKeyTimestampMapValue { + FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp); + FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); +}; + +class FixedLagSmootherKeyTimestampMap { + FixedLagSmootherKeyTimestampMap(); + FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + double at(const size_t key) const; + void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); +}; + +class FixedLagSmootherResult { + size_t getIterations() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +virtual class FixedLagSmoother { + void print(string s) const; + bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const; + + 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::Values calculateEstimate() const; +}; + +#include +virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { + BatchFixedLagSmoother(); + BatchFixedLagSmoother(double smootherLag); + BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); + + void print(string s = "BatchFixedLagSmoother:\n") const; + + gtsam::LevenbergMarquardtParams params() const; + template + VALUE calculateEstimate(size_t key) const; +}; + + } // namespace gtsam diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 5cd7e0da2..f36118873 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -40,6 +40,7 @@ class gtsam::LevenbergMarquardtParams; class gtsam::ISAM2Params; class gtsam::GaussianDensity; class gtsam::LevenbergMarquardtOptimizer; +class gtsam::FixedLagSmoother; namespace gtsam { @@ -530,67 +531,7 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { //************************************************************************* // nonlinear //************************************************************************* -#include -class FixedLagSmootherKeyTimestampMapValue { - FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp); - FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); -}; - -class FixedLagSmootherKeyTimestampMap { - FixedLagSmootherKeyTimestampMap(); - FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - double at(const size_t key) const; - void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); -}; - -class FixedLagSmootherResult { - size_t getIterations() const; - size_t getNonlinearVariables() const; - size_t getLinearVariables() const; - double getError() const; -}; - -#include -virtual class FixedLagSmoother { - void print(string s) const; - bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const; - - 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::Values calculateEstimate() const; -}; - -#include -virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { - BatchFixedLagSmoother(); - BatchFixedLagSmoother(double smootherLag); - BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); - - void print(string s = "BatchFixedLagSmoother:\n") const; - - gtsam::LevenbergMarquardtParams params() const; - template - VALUE calculateEstimate(size_t key) const; -}; - +#include #include virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { IncrementalFixedLagSmoother(); diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 28bad4ad2..48e0329fb 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -40,6 +40,7 @@ set(ignore gtsam::IndexPairVector gtsam::BetweenFactorPose2s gtsam::BetweenFactorPose3s + gtsam::FixedLagSmootherKeyTimestampMapValue gtsam::Point2Vector gtsam::Point2Pairs gtsam::Point3Pairs @@ -144,7 +145,6 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::Point2Vector gtsam::Pose3Vector gtsam::KeyVector - gtsam::FixedLagSmootherKeyTimestampMapValue gtsam::BinaryMeasurementsPoint3 gtsam::BinaryMeasurementsUnit3 gtsam::BinaryMeasurementsRot3 diff --git a/python/gtsam_unstable/examples/FixedLagSmootherExample.py b/python/gtsam/examples/FixedLagSmootherExample.py similarity index 95% rename from python/gtsam_unstable/examples/FixedLagSmootherExample.py rename to python/gtsam/examples/FixedLagSmootherExample.py index 7d2cea8ae..99af0edcf 100644 --- a/python/gtsam_unstable/examples/FixedLagSmootherExample.py +++ b/python/gtsam/examples/FixedLagSmootherExample.py @@ -25,13 +25,13 @@ def BatchFixedLagSmootherExample(): # Define a batch fixed lag smoother, which uses # Levenberg-Marquardt to perform the nonlinear optimization lag = 2.0 - smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag) + smoother_batch = gtsam.BatchFixedLagSmoother(lag) # Create containers to store the factors and linearization points # that will be sent to the smoothers new_factors = gtsam.NonlinearFactorGraph() new_values = gtsam.Values() - new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap() + new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap() # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) diff --git a/python/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/python/gtsam/tests/test_FixedLagSmootherExample.py similarity index 96% rename from python/gtsam_unstable/tests/test_FixedLagSmootherExample.py rename to python/gtsam/tests/test_FixedLagSmootherExample.py index c1ccd1ea1..ddd2d49e4 100644 --- a/python/gtsam_unstable/tests/test_FixedLagSmootherExample.py +++ b/python/gtsam/tests/test_FixedLagSmootherExample.py @@ -30,13 +30,13 @@ class TestFixedLagSmootherExample(GtsamTestCase): # Define a batch fixed lag smoother, which uses # Levenberg-Marquardt to perform the nonlinear optimization lag = 2.0 - smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag) + smoother_batch = gtsam.BatchFixedLagSmoother(lag) # Create containers to store the factors and linearization points # that will be sent to the smoothers new_factors = gtsam.NonlinearFactorGraph() new_values = gtsam.Values() - new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap() + new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap() # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0)