From dbc07997651b8bc7343d95ec661c1d10566d1ff9 Mon Sep 17 00:00:00 2001 From: Jeremy Date: Wed, 27 Feb 2019 01:57:39 -0500 Subject: [PATCH 01/17] Init uncomment of fixed lag smoother --- gtsam_unstable/gtsam_unstable.h | 250 ++++++++++++++++---------------- 1 file changed, 125 insertions(+), 125 deletions(-) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 39c910826..f1bb4005d 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -505,132 +505,132 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { //************************************************************************* // nonlinear //************************************************************************* -// #include -// gtsam::GaussianFactorGraph* summarizeGraphSequential( -// const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices); -// gtsam::GaussianFactorGraph* summarizeGraphSequential( -// const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys); +#include +gtsam::GaussianFactorGraph* summarizeGraphSequential( + const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices); +gtsam::GaussianFactorGraph* summarizeGraphSequential( + const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys); -// #include -// class FixedLagSmootherKeyTimestampMapValue { -// FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& 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 gtsam::Key& 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& timestamps); -// gtsam::Values calculateEstimate() const; -// }; -// -// #include -// virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { -// BatchFixedLagSmoother(); -// BatchFixedLagSmoother(double smootherLag); -// BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); -// -// gtsam::LevenbergMarquardtParams params() const; -// }; -// -// #include -// virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { -// IncrementalFixedLagSmoother(); -// IncrementalFixedLagSmoother(double smootherLag); -// IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); -// -// gtsam::ISAM2Params params() const; -// }; -// -// #include -// virtual class ConcurrentFilter { -// void print(string s) const; -// bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; -// }; -// -// virtual class ConcurrentSmoother { -// void print(string s) const; -// bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; -// }; -// -// // Synchronize function -// void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); -// -// #include -// class ConcurrentBatchFilterResult { -// size_t getIterations() const; -// size_t getLambdas() const; -// size_t getNonlinearVariables() const; -// size_t getLinearVariables() const; -// double getError() const; -// }; -// -// virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { -// ConcurrentBatchFilter(); -// ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); -// -// gtsam::NonlinearFactorGraph getFactors() const; -// gtsam::Values getLinearizationPoint() const; -// gtsam::Ordering getOrdering() const; -// gtsam::VectorValues getDelta() const; -// -// gtsam::ConcurrentBatchFilterResult update(); -// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); -// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); -// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); -// gtsam::Values calculateEstimate() const; -// }; -// -// #include -// class ConcurrentBatchSmootherResult { -// size_t getIterations() const; -// size_t getLambdas() const; -// size_t getNonlinearVariables() const; -// size_t getLinearVariables() const; -// double getError() const; -// }; -// -// virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { -// ConcurrentBatchSmoother(); -// ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); -// -// gtsam::NonlinearFactorGraph getFactors() const; -// gtsam::Values getLinearizationPoint() const; -// gtsam::Ordering getOrdering() const; -// gtsam::VectorValues getDelta() const; -// -// gtsam::ConcurrentBatchSmootherResult update(); -// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); -// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); -// gtsam::Values calculateEstimate() const; -// }; +#include +class FixedLagSmootherKeyTimestampMapValue { + FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& 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 gtsam::Key& 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& timestamps); + gtsam::Values calculateEstimate() const; +}; + +#include +virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { + BatchFixedLagSmoother(); + BatchFixedLagSmoother(double smootherLag); + BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); + + gtsam::LevenbergMarquardtParams params() const; +}; + +#include +virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { + IncrementalFixedLagSmoother(); + IncrementalFixedLagSmoother(double smootherLag); + IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); + + gtsam::ISAM2Params params() const; +}; + +#include +virtual class ConcurrentFilter { + void print(string s) const; + bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; +}; + +virtual class ConcurrentSmoother { + void print(string s) const; + bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; +}; + +// Synchronize function +void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); + +#include +class ConcurrentBatchFilterResult { + size_t getIterations() const; + size_t getLambdas() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { + ConcurrentBatchFilter(); + ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); + + gtsam::NonlinearFactorGraph getFactors() const; + gtsam::Values getLinearizationPoint() const; + gtsam::Ordering getOrdering() const; + gtsam::VectorValues getDelta() const; + + gtsam::ConcurrentBatchFilterResult update(); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); + gtsam::Values calculateEstimate() const; +}; + +#include +class ConcurrentBatchSmootherResult { + size_t getIterations() const; + size_t getLambdas() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { + ConcurrentBatchSmoother(); + ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); + + gtsam::NonlinearFactorGraph getFactors() const; + gtsam::Values getLinearizationPoint() const; + gtsam::Ordering getOrdering() const; + gtsam::VectorValues getDelta() const; + + gtsam::ConcurrentBatchSmootherResult update(); + gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); + gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::Values calculateEstimate() const; +}; //************************************************************************* // slam From 93fd884aa74c5db01b95e7d11bcf3a595c97e4ae Mon Sep 17 00:00:00 2001 From: Jeremy Date: Wed, 27 Feb 2019 04:37:02 -0500 Subject: [PATCH 02/17] Implement and add example --- .../examples/FixedLagSmootherExample.py | 86 +++++++++++++++++++ gtsam_unstable/gtsam_unstable.h | 14 ++- 2 files changed, 92 insertions(+), 8 deletions(-) create mode 100644 cython/gtsam_unstable/examples/FixedLagSmootherExample.py diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py new file mode 100644 index 000000000..5d44dab64 --- /dev/null +++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py @@ -0,0 +1,86 @@ +""" +Demonstration of the fixed-lag smoothers using a planar robot example +and multiple odometry-like sensors +Author: Frank Dellaert (C++), Jeremy Aguilon (Python) +""" + +import numpy as np + +import gtsam +import gtsam_unstable + +# Create noise models +PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) +MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2])) + +def _timestamp_key_value(key, value): + return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( + key, value + ) + + +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) + + + # 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() + + + # Create a prior on the first pose, placing it at the origin + prior_mean = gtsam.Pose2(0, 0, 0) + X1 = 0 + new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, PRIOR_NOISE)) + new_values.insert(X1, prior_mean) + new_timestamps.insert(_timestamp_key_value(X1, 0.0)) + + delta_time = 0.25 + time = 0.25 + + while time <= 3.0: + previous_key = 1000 * (time - delta_time) + current_key = 1000 * time + + # assign current key to the current timestamp + new_timestamps.insert(_timestamp_key_value(current_key, time)) + + # Add a guess for this pose to the new values + # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s] + current_pose = gtsam.Pose2(time * 2, 0, 0) + new_values.insert(current_key, current_pose) + + # Add odometry factors from two different sources with different error stats + odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02) + odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 0.05])) + new_factors.push_back(gtsam.BetweenFactorPose2( + previous_key, current_key, odometry_measurement_1, odometry_noise_1 + )) + + odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01) + odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.05, 0.05, 0.05])) + new_factors.push_back(gtsam.BetweenFactorPose2( + previous_key, current_key, odometry_measurement_2, odometry_noise_2 + )) + + # Update the smoothers with the new factors + smoother_batch.update(new_factors, new_values, new_timestamps) + + print("Timestamp = " + str(time) + ", Key = " + str(current_key)) + print(smoother_batch.calculateEstimatePose2(current_key)) + + new_timestamps.clear() + new_values.clear() + new_factors.resize(0) + + + time += delta_time + +if __name__ == '__main__': + BatchFixedLagSmootherExample() + print("Example complete") diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index f1bb4005d..d77895d86 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -505,15 +505,9 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { //************************************************************************* // nonlinear //************************************************************************* -#include -gtsam::GaussianFactorGraph* summarizeGraphSequential( - const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices); -gtsam::GaussianFactorGraph* summarizeGraphSequential( - const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys); - #include class FixedLagSmootherKeyTimestampMapValue { - FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp); + FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp); FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); }; @@ -528,7 +522,7 @@ class FixedLagSmootherKeyTimestampMap { bool empty() const; void clear(); - double at(const gtsam::Key& key) const; + double at(const size_t key) const; void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); }; @@ -558,6 +552,10 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); gtsam::LevenbergMarquardtParams params() const; + template + VALUE calculateEstimate(size_t key) const; }; #include From 42ac0e589e233f7f476e0240014df9a1d8ed38af Mon Sep 17 00:00:00 2001 From: Jeremy Date: Wed, 27 Feb 2019 04:55:15 -0500 Subject: [PATCH 03/17] Implement unit test --- .../examples/FixedLagSmootherExample.py | 6 +- cython/gtsam_unstable/examples/__init__.py | 0 cython/gtsam_unstable/tests/__init__.py | 0 .../tests/test_FixedLagSmootherExample.py | 94 +++++++++++++++++++ 4 files changed, 96 insertions(+), 4 deletions(-) create mode 100644 cython/gtsam_unstable/examples/__init__.py create mode 100644 cython/gtsam_unstable/tests/__init__.py create mode 100644 cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py index 5d44dab64..b9777a07c 100644 --- a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py @@ -9,9 +9,6 @@ import numpy as np import gtsam import gtsam_unstable -# Create noise models -PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) -MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2])) def _timestamp_key_value(key, value): return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( @@ -35,8 +32,9 @@ def BatchFixedLagSmootherExample(): # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) + prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) X1 = 0 - new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, PRIOR_NOISE)) + new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise)) new_values.insert(X1, prior_mean) new_timestamps.insert(_timestamp_key_value(X1, 0.0)) diff --git a/cython/gtsam_unstable/examples/__init__.py b/cython/gtsam_unstable/examples/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/cython/gtsam_unstable/tests/__init__.py b/cython/gtsam_unstable/tests/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py new file mode 100644 index 000000000..64895f4d3 --- /dev/null +++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py @@ -0,0 +1,94 @@ +import unittest +import gtsam +import gtsam_unstable +import numpy as np + +def _timestamp_key_value(key, value): + return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( + key, value + ) +class TestFixedLagSmootherExample(unittest.TestCase): + # Simple test that checks for equality between C++ example + # file and the Python implementation + def test_FixedLagSmootherExample(self): + # Define a batch fixed lag smoother, which uses + # Levenberg-Marquardt to perform the nonlinear optimization + lag = 2.0 + smoother_batch = gtsam_unstable.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() + + + # Create a prior on the first pose, placing it at the origin + prior_mean = gtsam.Pose2(0, 0, 0) + prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) + X1 = 0 + new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise)) + new_values.insert(X1, prior_mean) + new_timestamps.insert(_timestamp_key_value(X1, 0.0)) + + delta_time = 0.25 + time = 0.25 + + i = 0 + + ground_truth = [ + gtsam.Pose2(0.49792, 0.007802, 0.015), + gtsam.Pose2(0.99547, 0.023019, 0.03), + gtsam.Pose2(1.4928, 0.045725, 0.045), + gtsam.Pose2(1.9898, 0.075888, 0.06), + gtsam.Pose2(2.4863, 0.1135, 0.075), + gtsam.Pose2(2.9821, 0.15856, 0.09), + gtsam.Pose2(3.4772, 0.21105, 0.105), + gtsam.Pose2(3.9715, 0.27096, 0.12), + gtsam.Pose2(4.4648, 0.33827, 0.135), + gtsam.Pose2(4.957, 0.41298, 0.15), + gtsam.Pose2(5.4481, 0.49506, 0.165), + gtsam.Pose2(5.9379, 0.5845, 0.18), + ] + while time <= 3.0: + previous_key = 1000 * (time - delta_time) + current_key = 1000 * time + + # assign current key to the current timestamp + new_timestamps.insert(_timestamp_key_value(current_key, time)) + + # Add a guess for this pose to the new values + # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s] + current_pose = gtsam.Pose2(time * 2, 0, 0) + new_values.insert(current_key, current_pose) + + # Add odometry factors from two different sources with different error stats + odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02) + odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 0.05])) + new_factors.push_back(gtsam.BetweenFactorPose2( + previous_key, current_key, odometry_measurement_1, odometry_noise_1 + )) + + odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01) + odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.05, 0.05, 0.05])) + new_factors.push_back(gtsam.BetweenFactorPose2( + previous_key, current_key, odometry_measurement_2, odometry_noise_2 + )) + + # Update the smoothers with the new factors + smoother_batch.update(new_factors, new_values, new_timestamps) + + estimate = smoother_batch.calculateEstimatePose2(current_key) + self.assertTrue(estimate.equals(ground_truth[i], 1e-4)) + print("PASS") + + new_timestamps.clear() + new_values.clear() + new_factors.resize(0) + + time += delta_time + i += 1 + +if __name__ == "__main__": + unittest.main() From 5e97efa81518f3ecc83d83d43e425edbe951ceed Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Wed, 27 Feb 2019 13:04:38 -0500 Subject: [PATCH 04/17] Liner and update Cmakelists --- THANKS | 1 + cython/CMakeLists.txt | 3 +-- cython/gtsam_unstable/__init__.py.in | 1 + .../examples/FixedLagSmootherExample.py | 19 +++++++++++++++++++ .../tests/test_FixedLagSmootherExample.py | 12 +++++++++--- 5 files changed, 31 insertions(+), 5 deletions(-) create mode 100644 cython/gtsam_unstable/__init__.py.in diff --git a/THANKS b/THANKS index f84cfa185..689074b6f 100644 --- a/THANKS +++ b/THANKS @@ -1,5 +1,6 @@ GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech: +* Jeremy Aguilon * Sungtae An * Doru Balcan, Bank of America * Chris Beall diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index bc21b91d1..b832a76a5 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -26,8 +26,7 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) gtsam_unstable # library to link with "gtsam_unstable;gtsam_unstable_header;cythonize_gtsam" # dependencies to be built before wrapping ) - # for some reasons cython gtsam_unstable can't find gtsam/gtsam.pxd without doing this - file(WRITE ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py "") + configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py) endif() # Install the custom-generated __init__.py diff --git a/cython/gtsam_unstable/__init__.py.in b/cython/gtsam_unstable/__init__.py.in new file mode 100644 index 000000000..2b0a28321 --- /dev/null +++ b/cython/gtsam_unstable/__init__.py.in @@ -0,0 +1 @@ +from gtsam_unstable import * diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py index b9777a07c..5fc3bc98d 100644 --- a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py @@ -1,4 +1,11 @@ """ +GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +Authors: Frank Dellaert, et al. (see THANKS for the full author list) + +See LICENSE for the license information + Demonstration of the fixed-lag smoothers using a planar robot example and multiple odometry-like sensors Author: Frank Dellaert (C++), Jeremy Aguilon (Python) @@ -11,12 +18,21 @@ import gtsam_unstable def _timestamp_key_value(key, value): + """ + + """ return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( key, value ) def BatchFixedLagSmootherExample(): + """ + Runs a batch fixed smoother on an agent with two odometry + sensors that is simply moving along the x axis in constant + speed. + """ + # Define a batch fixed lag smoother, which uses # Levenberg-Marquardt to perform the nonlinear optimization lag = 2.0 @@ -41,6 +57,9 @@ def BatchFixedLagSmootherExample(): delta_time = 0.25 time = 0.25 + # Iterates from 0.25s to 3.0s, adding 0.25s each loop + # In each iteration, the agent moves at a constant speed + # and its two odometers measure the change. while time <= 3.0: previous_key = 1000 * (time - delta_time) current_key = 1000 * time diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py index 64895f4d3..8994913a2 100644 --- a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py @@ -8,9 +8,16 @@ def _timestamp_key_value(key, value): key, value ) class TestFixedLagSmootherExample(unittest.TestCase): - # Simple test that checks for equality between C++ example - # file and the Python implementation + ''' + Tests the fixed lag smoother wrapper + ''' + def test_FixedLagSmootherExample(self): + ''' + Simple test that checks for equality between C++ example + file and the Python implementation. See + gtsam_unstable/examples/FixedLagSmootherExample.cpp + ''' # Define a batch fixed lag smoother, which uses # Levenberg-Marquardt to perform the nonlinear optimization lag = 2.0 @@ -81,7 +88,6 @@ class TestFixedLagSmootherExample(unittest.TestCase): estimate = smoother_batch.calculateEstimatePose2(current_key) self.assertTrue(estimate.equals(ground_truth[i], 1e-4)) - print("PASS") new_timestamps.clear() new_values.clear() From 1ca14d516416be6efb460c9695efa2bf773460c8 Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Wed, 27 Feb 2019 13:08:49 -0500 Subject: [PATCH 05/17] Add comment --- THANKS | 2 +- cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/THANKS b/THANKS index 689074b6f..f2b51f61d 100644 --- a/THANKS +++ b/THANKS @@ -1,6 +1,6 @@ GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech: -* Jeremy Aguilon +* Jeremy Aguilon, Facebook * Sungtae An * Doru Balcan, Bank of America * Chris Beall diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py index 8994913a2..e26c450a0 100644 --- a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py @@ -58,6 +58,10 @@ class TestFixedLagSmootherExample(unittest.TestCase): gtsam.Pose2(5.4481, 0.49506, 0.165), gtsam.Pose2(5.9379, 0.5845, 0.18), ] + # Iterates from 0.25s to 3.0s, adding 0.25s each loop + # In each iteration, the agent moves at a constant speed + # and its two odometers measure the change. The smoothed + # result is then compared to the ground truth while time <= 3.0: previous_key = 1000 * (time - delta_time) current_key = 1000 * time From 2eee111960d46b5cf90bfd68dabb72210817a4a7 Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Wed, 27 Feb 2019 13:58:40 -0500 Subject: [PATCH 06/17] Forgotten docstring --- cython/gtsam_unstable/examples/FixedLagSmootherExample.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py index 5fc3bc98d..141162044 100644 --- a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py @@ -19,7 +19,7 @@ import gtsam_unstable def _timestamp_key_value(key, value): """ - + Creates a key value pair for a FixedLagSmootherKeyTimeStampMap """ return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( key, value From 27d47c32bb65c22ac6929a23a33afeed1405c16e Mon Sep 17 00:00:00 2001 From: Jeremy Date: Wed, 27 Feb 2019 23:38:59 -0500 Subject: [PATCH 07/17] Update cmaklists --- cython/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index b832a76a5..9acf8174c 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -27,7 +27,9 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) "gtsam_unstable;gtsam_unstable_header;cythonize_gtsam" # dependencies to be built before wrapping ) configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py) + install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable") endif() + install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") # Install the custom-generated __init__.py # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable From 6bd8e44fc759e7291c41211ee998e12c2be11cdc Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Tue, 5 Mar 2019 17:35:50 -0500 Subject: [PATCH 08/17] init --- THANKS | 1 + cmake/GtsamCythonWrap.cmake | 2 +- cython/CMakeLists.txt | 10 ++++++---- cython/gtsam_unstable/__init__.py.in | 1 + 4 files changed, 9 insertions(+), 5 deletions(-) create mode 100644 cython/gtsam_unstable/__init__.py.in diff --git a/THANKS b/THANKS index f84cfa185..f2b51f61d 100644 --- a/THANKS +++ b/THANKS @@ -1,5 +1,6 @@ GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech: +* Jeremy Aguilon, Facebook * Sungtae An * Doru Balcan, Bank of America * Chris Beall diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake index 73e2b63e0..1c9868d0d 100644 --- a/cmake/GtsamCythonWrap.cmake +++ b/cmake/GtsamCythonWrap.cmake @@ -212,7 +212,7 @@ function(install_cython_scripts source_directory dest_directory patterns) FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) endforeach() else() - install(DIRECTORY "${source_directory}" DESTINATION "${dest_directory}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) + install(DIRECTORY "${source_directory}/" DESTINATION "${dest_directory}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) endif() endfunction() diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index bc21b91d1..7a4f518ef 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -22,19 +22,21 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) set(GTSAM_UNSTABLE_IMPORT "from gtsam_unstable import *") wrap_and_install_library_cython("../gtsam_unstable/gtsam_unstable.h" # interface_header "from gtsam.gtsam cimport *" # extra imports - "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" # install path + "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" # install path gtsam_unstable # library to link with "gtsam_unstable;gtsam_unstable_header;cythonize_gtsam" # dependencies to be built before wrapping ) - # for some reasons cython gtsam_unstable can't find gtsam/gtsam.pxd without doing this - file(WRITE ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py "") + configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py) + install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable") endif() + install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" "*.py") # Install the custom-generated __init__.py # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py) install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam") # install scripts and tests - install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") + install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam/" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" "*.py") endif () + diff --git a/cython/gtsam_unstable/__init__.py.in b/cython/gtsam_unstable/__init__.py.in new file mode 100644 index 000000000..85f7c3df2 --- /dev/null +++ b/cython/gtsam_unstable/__init__.py.in @@ -0,0 +1 @@ +${GTSAM_UNSTABLE_IMPORT} From 73681b4d2db05127b691ad6199ca5b9852848c67 Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Tue, 5 Mar 2019 17:42:30 -0500 Subject: [PATCH 09/17] Revert unneeded change --- cmake/GtsamCythonWrap.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake index 1c9868d0d..73e2b63e0 100644 --- a/cmake/GtsamCythonWrap.cmake +++ b/cmake/GtsamCythonWrap.cmake @@ -212,7 +212,7 @@ function(install_cython_scripts source_directory dest_directory patterns) FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) endforeach() else() - install(DIRECTORY "${source_directory}/" DESTINATION "${dest_directory}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) + install(DIRECTORY "${source_directory}" DESTINATION "${dest_directory}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE) endif() endfunction() From 409806efc6d3ffeedb0a0a899b6f8c78f6cf52dc Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Tue, 5 Mar 2019 18:07:46 -0500 Subject: [PATCH 10/17] Clumsy mistake - line should be inside if statement --- cython/CMakeLists.txt | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index 7a4f518ef..b5f1365c7 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -28,15 +28,18 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) ) configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py) install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable") - endif() install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" "*.py") + endif() # Install the custom-generated __init__.py # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py) install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam") # install scripts and tests - install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam/" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" "*.py") + + message("FOO") + message ("${GTSAM_CYTHON_INSTALL_PATH}") + install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") endif () From f3baa4d5b008adf5c59eb0e92547e822014222bc Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Tue, 5 Mar 2019 18:08:22 -0500 Subject: [PATCH 11/17] Forgot to remove print statement --- cython/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index b5f1365c7..8c2a478db 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -37,8 +37,6 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam") # install scripts and tests - message("FOO") - message ("${GTSAM_CYTHON_INSTALL_PATH}") install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") endif () From ed2300dd397307f571a2b7a5494e0610129837ca Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Tue, 5 Mar 2019 18:08:48 -0500 Subject: [PATCH 12/17] Remove unwanted lines --- cython/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index 8c2a478db..8e5d38eec 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -36,8 +36,6 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py) install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam") # install scripts and tests - install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") endif () - From 7d2e4d2e64ce16715a110e800c6ab81509ba480b Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Wed, 6 Mar 2019 17:47:34 -0500 Subject: [PATCH 13/17] Fix warning message in the unit tests/examples --- .../examples/FixedLagSmootherExample.py | 39 ++++----- .../tests/test_FixedLagSmootherExample.py | 86 +++++++++++-------- .../examples/FixedLagSmootherExample.cpp | 36 ++++---- 3 files changed, 91 insertions(+), 70 deletions(-) diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py index 141162044..786701e0f 100644 --- a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py @@ -19,7 +19,7 @@ import gtsam_unstable def _timestamp_key_value(key, value): """ - Creates a key value pair for a FixedLagSmootherKeyTimeStampMap + """ return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( key, value @@ -29,8 +29,7 @@ def _timestamp_key_value(key, value): def BatchFixedLagSmootherExample(): """ Runs a batch fixed smoother on an agent with two odometry - sensors that is simply moving along the x axis in constant - speed. + sensors that is simply moving to the """ # Define a batch fixed lag smoother, which uses @@ -38,14 +37,12 @@ def BatchFixedLagSmootherExample(): lag = 2.0 smoother_batch = gtsam_unstable.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() - # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) @@ -57,9 +54,6 @@ def BatchFixedLagSmootherExample(): delta_time = 0.25 time = 0.25 - # Iterates from 0.25s to 3.0s, adding 0.25s each loop - # In each iteration, the agent moves at a constant speed - # and its two odometers measure the change. while time <= 3.0: previous_key = 1000 * (time - delta_time) current_key = 1000 * time @@ -72,32 +66,37 @@ def BatchFixedLagSmootherExample(): current_pose = gtsam.Pose2(time * 2, 0, 0) new_values.insert(current_key, current_pose) - # Add odometry factors from two different sources with different error stats + # Add odometry factors from two different sources with different error + # stats odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02) - odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 0.05])) + odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.1, 0.1, 0.05])) new_factors.push_back(gtsam.BetweenFactorPose2( previous_key, current_key, odometry_measurement_1, odometry_noise_1 )) odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01) - odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.05, 0.05, 0.05])) + odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.05, 0.05, 0.05])) new_factors.push_back(gtsam.BetweenFactorPose2( previous_key, current_key, odometry_measurement_2, odometry_noise_2 )) - # Update the smoothers with the new factors - smoother_batch.update(new_factors, new_values, new_timestamps) - - print("Timestamp = " + str(time) + ", Key = " + str(current_key)) - print(smoother_batch.calculateEstimatePose2(current_key)) - - new_timestamps.clear() - new_values.clear() - new_factors.resize(0) + # Update the smoothers with the new factors. In this case, + # one iteration must pass for Levenberg-Marquardt to accurately + # estimate + if time >= 0.50: + smoother_batch.update(new_factors, new_values, new_timestamps) + print("Timestamp = " + str(time) + ", Key = " + str(current_key)) + print(smoother_batch.calculateEstimatePose2(current_key)) + new_timestamps.clear() + new_values.clear() + new_factors.resize(0) time += delta_time + if __name__ == '__main__': BatchFixedLagSmootherExample() print("Example complete") diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py index e26c450a0..90c4e436b 100644 --- a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py @@ -3,10 +3,13 @@ import gtsam import gtsam_unstable import numpy as np + def _timestamp_key_value(key, value): return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( key, value ) + + class TestFixedLagSmootherExample(unittest.TestCase): ''' Tests the fixed lag smoother wrapper @@ -23,19 +26,20 @@ class TestFixedLagSmootherExample(unittest.TestCase): lag = 2.0 smoother_batch = gtsam_unstable.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() - # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) - prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) + prior_noise = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.3, 0.3, 0.1])) X1 = 0 - new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise)) + new_factors.push_back( + gtsam.PriorFactorPose2( + X1, prior_mean, prior_noise)) new_values.insert(X1, prior_mean) new_timestamps.insert(_timestamp_key_value(X1, 0.0)) @@ -45,19 +49,19 @@ class TestFixedLagSmootherExample(unittest.TestCase): i = 0 ground_truth = [ - gtsam.Pose2(0.49792, 0.007802, 0.015), - gtsam.Pose2(0.99547, 0.023019, 0.03), - gtsam.Pose2(1.4928, 0.045725, 0.045), - gtsam.Pose2(1.9898, 0.075888, 0.06), - gtsam.Pose2(2.4863, 0.1135, 0.075), - gtsam.Pose2(2.9821, 0.15856, 0.09), - gtsam.Pose2(3.4772, 0.21105, 0.105), - gtsam.Pose2(3.9715, 0.27096, 0.12), - gtsam.Pose2(4.4648, 0.33827, 0.135), - gtsam.Pose2(4.957, 0.41298, 0.15), - gtsam.Pose2(5.4481, 0.49506, 0.165), - gtsam.Pose2(5.9379, 0.5845, 0.18), + gtsam.Pose2(0.995821, 0.0231012, 0.0300001), + gtsam.Pose2(1.49284, 0.0457247, 0.045), + gtsam.Pose2(1.98981, 0.0758879, 0.06), + gtsam.Pose2(2.48627, 0.113502, 0.075), + gtsam.Pose2(2.98211, 0.158558, 0.09), + gtsam.Pose2(3.47722, 0.211047, 0.105), + gtsam.Pose2(3.97149, 0.270956, 0.12), + gtsam.Pose2(4.4648, 0.338272, 0.135), + gtsam.Pose2(4.95705, 0.41298, 0.15), + gtsam.Pose2(5.44812, 0.495063, 0.165), + gtsam.Pose2(5.9379, 0.584503, 0.18), ] + # Iterates from 0.25s to 3.0s, adding 0.25s each loop # In each iteration, the agent moves at a constant speed # and its two odometers measure the change. The smoothed @@ -70,35 +74,49 @@ class TestFixedLagSmootherExample(unittest.TestCase): new_timestamps.insert(_timestamp_key_value(current_key, time)) # Add a guess for this pose to the new values - # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s] + # Assume that the robot moves at 2 m/s. Position is time[s] * + # 2[m/s] current_pose = gtsam.Pose2(time * 2, 0, 0) new_values.insert(current_key, current_pose) - # Add odometry factors from two different sources with different error stats + # Add odometry factors from two different sources with different + # error stats odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02) - odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 0.05])) - new_factors.push_back(gtsam.BetweenFactorPose2( - previous_key, current_key, odometry_measurement_1, odometry_noise_1 - )) + odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.1, 0.1, 0.05])) + new_factors.push_back( + gtsam.BetweenFactorPose2( + previous_key, + current_key, + odometry_measurement_1, + odometry_noise_1)) odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01) - odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.05, 0.05, 0.05])) - new_factors.push_back(gtsam.BetweenFactorPose2( - previous_key, current_key, odometry_measurement_2, odometry_noise_2 - )) + odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.05, 0.05, 0.05])) + new_factors.push_back( + gtsam.BetweenFactorPose2( + previous_key, + current_key, + odometry_measurement_2, + odometry_noise_2)) - # Update the smoothers with the new factors - smoother_batch.update(new_factors, new_values, new_timestamps) + # Update the smoothers with the new factors. In this case, + # one iteration must pass for Levenberg-Marquardt to accurately + # estimate + if time >= 0.50: + smoother_batch.update(new_factors, new_values, new_timestamps) - estimate = smoother_batch.calculateEstimatePose2(current_key) - self.assertTrue(estimate.equals(ground_truth[i], 1e-4)) + estimate = smoother_batch.calculateEstimatePose2(current_key) + self.assertTrue(estimate.equals(ground_truth[i], 1e-4)) + i += 1 - new_timestamps.clear() - new_values.clear() - new_factors.resize(0) + new_timestamps.clear() + new_values.clear() + new_factors.resize(0) time += delta_time - i += 1 + if __name__ == "__main__": unittest.main() diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index 931e85c1a..8ccc9cc2c 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -111,23 +111,27 @@ int main(int argc, char** argv) { noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); - // Update the smoothers with the new factors - smootherBatch.update(newFactors, newValues, newTimestamps); - smootherISAM2.update(newFactors, newValues, newTimestamps); - for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations - smootherISAM2.update(); + // Update the smoothers with the new factors. + // In this example, Levenberg-Marquadt needs one iteration + // to pass to accurately estimate. + if (time >= 0.50) { + smootherBatch.update(newFactors, newValues, newTimestamps); + smootherISAM2.update(newFactors, newValues, newTimestamps); + for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations + smootherISAM2.update(); + } + + // Print the optimized current pose + cout << setprecision(5) << "Timestamp = " << time << endl; + smootherBatch.calculateEstimate(currentKey).print("Batch Estimate:"); + smootherISAM2.calculateEstimate(currentKey).print("iSAM2 Estimate:"); + cout << endl; + + // Clear contains for the next iteration + newTimestamps.clear(); + newValues.clear(); + newFactors.resize(0); } - - // Print the optimized current pose - cout << setprecision(5) << "Timestamp = " << time << endl; - smootherBatch.calculateEstimate(currentKey).print("Batch Estimate:"); - smootherISAM2.calculateEstimate(currentKey).print("iSAM2 Estimate:"); - cout << endl; - - // Clear contains for the next iteration - newTimestamps.clear(); - newValues.clear(); - newFactors.resize(0); } // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds From 9a3d51792527ff284d8f6bbc4a5a9cf812d78f2b Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Wed, 6 Mar 2019 17:49:40 -0500 Subject: [PATCH 14/17] Make documentation on .cpp file more specific --- gtsam_unstable/examples/FixedLagSmootherExample.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index 8ccc9cc2c..1376aca40 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -111,9 +111,9 @@ int main(int argc, char** argv) { noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); - // Update the smoothers with the new factors. - // In this example, Levenberg-Marquadt needs one iteration - // to pass to accurately estimate. + // Update the smoothers with the new factors. In this example, batch smoother needs one iteration + // to accurately converge. The ISAM smoother doesn't, but we only start getting estiates when + // both are ready for simplicity. if (time >= 0.50) { smootherBatch.update(newFactors, newValues, newTimestamps); smootherISAM2.update(newFactors, newValues, newTimestamps); From f4bf0d5b0b4319ed99edeec66a18a54b9fe2464a Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Thu, 7 Mar 2019 14:23:11 -0500 Subject: [PATCH 15/17] Update unstable.h file to match upstream --- gtsam_unstable/gtsam_unstable.h | 248 ++++++++++++++++---------------- 1 file changed, 125 insertions(+), 123 deletions(-) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index d77895d86..39c910826 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -505,130 +505,132 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { //************************************************************************* // nonlinear //************************************************************************* -#include -class FixedLagSmootherKeyTimestampMapValue { - FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp); - FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); -}; +// #include +// gtsam::GaussianFactorGraph* summarizeGraphSequential( +// const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices); +// gtsam::GaussianFactorGraph* summarizeGraphSequential( +// const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys); -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& timestamps); - gtsam::Values calculateEstimate() const; -}; - -#include -virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { - BatchFixedLagSmoother(); - BatchFixedLagSmoother(double smootherLag); - BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); - - gtsam::LevenbergMarquardtParams params() const; - template - VALUE calculateEstimate(size_t key) const; -}; - -#include -virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { - IncrementalFixedLagSmoother(); - IncrementalFixedLagSmoother(double smootherLag); - IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); - - gtsam::ISAM2Params params() const; -}; - -#include -virtual class ConcurrentFilter { - void print(string s) const; - bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; -}; - -virtual class ConcurrentSmoother { - void print(string s) const; - bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; -}; - -// Synchronize function -void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); - -#include -class ConcurrentBatchFilterResult { - size_t getIterations() const; - size_t getLambdas() const; - size_t getNonlinearVariables() const; - size_t getLinearVariables() const; - double getError() const; -}; - -virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { - ConcurrentBatchFilter(); - ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); - - gtsam::NonlinearFactorGraph getFactors() const; - gtsam::Values getLinearizationPoint() const; - gtsam::Ordering getOrdering() const; - gtsam::VectorValues getDelta() const; - - gtsam::ConcurrentBatchFilterResult update(); - gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); - gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); - gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); - gtsam::Values calculateEstimate() const; -}; - -#include -class ConcurrentBatchSmootherResult { - size_t getIterations() const; - size_t getLambdas() const; - size_t getNonlinearVariables() const; - size_t getLinearVariables() const; - double getError() const; -}; - -virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { - ConcurrentBatchSmoother(); - ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); - - gtsam::NonlinearFactorGraph getFactors() const; - gtsam::Values getLinearizationPoint() const; - gtsam::Ordering getOrdering() const; - gtsam::VectorValues getDelta() const; - - gtsam::ConcurrentBatchSmootherResult update(); - gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); - gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); - gtsam::Values calculateEstimate() const; -}; +// #include +// class FixedLagSmootherKeyTimestampMapValue { +// FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& 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 gtsam::Key& 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& timestamps); +// gtsam::Values calculateEstimate() const; +// }; +// +// #include +// virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { +// BatchFixedLagSmoother(); +// BatchFixedLagSmoother(double smootherLag); +// BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); +// +// gtsam::LevenbergMarquardtParams params() const; +// }; +// +// #include +// virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { +// IncrementalFixedLagSmoother(); +// IncrementalFixedLagSmoother(double smootherLag); +// IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); +// +// gtsam::ISAM2Params params() const; +// }; +// +// #include +// virtual class ConcurrentFilter { +// void print(string s) const; +// bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; +// }; +// +// virtual class ConcurrentSmoother { +// void print(string s) const; +// bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; +// }; +// +// // Synchronize function +// void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); +// +// #include +// class ConcurrentBatchFilterResult { +// size_t getIterations() const; +// size_t getLambdas() const; +// size_t getNonlinearVariables() const; +// size_t getLinearVariables() const; +// double getError() const; +// }; +// +// virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { +// ConcurrentBatchFilter(); +// ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); +// +// gtsam::NonlinearFactorGraph getFactors() const; +// gtsam::Values getLinearizationPoint() const; +// gtsam::Ordering getOrdering() const; +// gtsam::VectorValues getDelta() const; +// +// gtsam::ConcurrentBatchFilterResult update(); +// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); +// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); +// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); +// gtsam::Values calculateEstimate() const; +// }; +// +// #include +// class ConcurrentBatchSmootherResult { +// size_t getIterations() const; +// size_t getLambdas() const; +// size_t getNonlinearVariables() const; +// size_t getLinearVariables() const; +// double getError() const; +// }; +// +// virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { +// ConcurrentBatchSmoother(); +// ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); +// +// gtsam::NonlinearFactorGraph getFactors() const; +// gtsam::Values getLinearizationPoint() const; +// gtsam::Ordering getOrdering() const; +// gtsam::VectorValues getDelta() const; +// +// gtsam::ConcurrentBatchSmootherResult update(); +// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); +// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); +// gtsam::Values calculateEstimate() const; +// }; //************************************************************************* // slam From fe3741e4662c8400e9b41196311d670d881573fa Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Thu, 7 Mar 2019 14:25:05 -0500 Subject: [PATCH 16/17] Fix broken file --- gtsam_unstable/gtsam_unstable.h | 248 ++++++++++++++++---------------- 1 file changed, 123 insertions(+), 125 deletions(-) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 39c910826..d77895d86 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -505,132 +505,130 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { //************************************************************************* // nonlinear //************************************************************************* -// #include -// gtsam::GaussianFactorGraph* summarizeGraphSequential( -// const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices); -// gtsam::GaussianFactorGraph* summarizeGraphSequential( -// const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys); +#include +class FixedLagSmootherKeyTimestampMapValue { + FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp); + FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); +}; -// #include -// class FixedLagSmootherKeyTimestampMapValue { -// FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& 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 gtsam::Key& 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& timestamps); -// gtsam::Values calculateEstimate() const; -// }; -// -// #include -// virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { -// BatchFixedLagSmoother(); -// BatchFixedLagSmoother(double smootherLag); -// BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); -// -// gtsam::LevenbergMarquardtParams params() const; -// }; -// -// #include -// virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { -// IncrementalFixedLagSmoother(); -// IncrementalFixedLagSmoother(double smootherLag); -// IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); -// -// gtsam::ISAM2Params params() const; -// }; -// -// #include -// virtual class ConcurrentFilter { -// void print(string s) const; -// bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; -// }; -// -// virtual class ConcurrentSmoother { -// void print(string s) const; -// bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; -// }; -// -// // Synchronize function -// void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); -// -// #include -// class ConcurrentBatchFilterResult { -// size_t getIterations() const; -// size_t getLambdas() const; -// size_t getNonlinearVariables() const; -// size_t getLinearVariables() const; -// double getError() const; -// }; -// -// virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { -// ConcurrentBatchFilter(); -// ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); -// -// gtsam::NonlinearFactorGraph getFactors() const; -// gtsam::Values getLinearizationPoint() const; -// gtsam::Ordering getOrdering() const; -// gtsam::VectorValues getDelta() const; -// -// gtsam::ConcurrentBatchFilterResult update(); -// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); -// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); -// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); -// gtsam::Values calculateEstimate() const; -// }; -// -// #include -// class ConcurrentBatchSmootherResult { -// size_t getIterations() const; -// size_t getLambdas() const; -// size_t getNonlinearVariables() const; -// size_t getLinearVariables() const; -// double getError() const; -// }; -// -// virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { -// ConcurrentBatchSmoother(); -// ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); -// -// gtsam::NonlinearFactorGraph getFactors() const; -// gtsam::Values getLinearizationPoint() const; -// gtsam::Ordering getOrdering() const; -// gtsam::VectorValues getDelta() const; -// -// gtsam::ConcurrentBatchSmootherResult update(); -// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); -// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); -// gtsam::Values calculateEstimate() const; -// }; +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& timestamps); + gtsam::Values calculateEstimate() const; +}; + +#include +virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { + BatchFixedLagSmoother(); + BatchFixedLagSmoother(double smootherLag); + BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); + + gtsam::LevenbergMarquardtParams params() const; + template + VALUE calculateEstimate(size_t key) const; +}; + +#include +virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { + IncrementalFixedLagSmoother(); + IncrementalFixedLagSmoother(double smootherLag); + IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); + + gtsam::ISAM2Params params() const; +}; + +#include +virtual class ConcurrentFilter { + void print(string s) const; + bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; +}; + +virtual class ConcurrentSmoother { + void print(string s) const; + bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; +}; + +// Synchronize function +void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); + +#include +class ConcurrentBatchFilterResult { + size_t getIterations() const; + size_t getLambdas() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { + ConcurrentBatchFilter(); + ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); + + gtsam::NonlinearFactorGraph getFactors() const; + gtsam::Values getLinearizationPoint() const; + gtsam::Ordering getOrdering() const; + gtsam::VectorValues getDelta() const; + + gtsam::ConcurrentBatchFilterResult update(); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); + gtsam::Values calculateEstimate() const; +}; + +#include +class ConcurrentBatchSmootherResult { + size_t getIterations() const; + size_t getLambdas() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { + ConcurrentBatchSmoother(); + ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); + + gtsam::NonlinearFactorGraph getFactors() const; + gtsam::Values getLinearizationPoint() const; + gtsam::Ordering getOrdering() const; + gtsam::VectorValues getDelta() const; + + gtsam::ConcurrentBatchSmootherResult update(); + gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); + gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::Values calculateEstimate() const; +}; //************************************************************************* // slam From fe647a9e941bfad17a6a3d22983866fb91e78f39 Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Wed, 13 Mar 2019 01:32:49 -0400 Subject: [PATCH 17/17] Remove unneeeded file in build chain --- cython/gtsam_unstable/__init__.py.in | 1 - 1 file changed, 1 deletion(-) delete mode 100644 cython/gtsam_unstable/__init__.py.in diff --git a/cython/gtsam_unstable/__init__.py.in b/cython/gtsam_unstable/__init__.py.in deleted file mode 100644 index 85f7c3df2..000000000 --- a/cython/gtsam_unstable/__init__.py.in +++ /dev/null @@ -1 +0,0 @@ -${GTSAM_UNSTABLE_IMPORT}