From dbc07997651b8bc7343d95ec661c1d10566d1ff9 Mon Sep 17 00:00:00 2001 From: Jeremy Date: Wed, 27 Feb 2019 01:57:39 -0500 Subject: [PATCH 01/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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 549fcca2acd92556ac528ee9cdfad19426ee9192 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 9 Mar 2019 13:06:31 -0500 Subject: [PATCH 17/61] Fixed 2 unit tests --- cython/gtsam/tests/test_Scenario.py | 18 ++++++++++++++++- cython/gtsam/tests/test_Values.py | 31 +++++++++++++++++++++-------- 2 files changed, 40 insertions(+), 9 deletions(-) diff --git a/cython/gtsam/tests/test_Scenario.py b/cython/gtsam/tests/test_Scenario.py index 4cca1400b..d68566b25 100644 --- a/cython/gtsam/tests/test_Scenario.py +++ b/cython/gtsam/tests/test_Scenario.py @@ -1,5 +1,20 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for IMU testing scenarios. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + import math import unittest + import numpy as np import gtsam @@ -29,7 +44,8 @@ class TestScenario(unittest.TestCase): T30 = scenario.pose(T) np.testing.assert_almost_equal( np.array([math.pi, 0, math.pi]), T30.rotation().xyz()) - self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation(), 1e-9)) + self.assertTrue(gtsam.Point3( + 0, 0, 2.0 * R).equals(T30.translation(), 1e-9)) if __name__ == '__main__': diff --git a/cython/gtsam/tests/test_Values.py b/cython/gtsam/tests/test_Values.py index 08e133840..0bb1e0806 100644 --- a/cython/gtsam/tests/test_Values.py +++ b/cython/gtsam/tests/test_Values.py @@ -1,8 +1,21 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for IMU testing scenarios. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +# pylint: disable=invalid-name, E1101, E0611 import unittest + import numpy as np -from gtsam import Point2, Point3, Unit3, Rot2, Pose2, Rot3, Pose3 -from gtsam import Values, Cal3_S2, Cal3DS2, Cal3Bundler, EssentialMatrix, imuBias_ConstantBias +from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2, + Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values, + imuBias_ConstantBias) class TestValues(unittest.TestCase): @@ -12,8 +25,8 @@ class TestValues(unittest.TestCase): E = EssentialMatrix(Rot3(), Unit3()) tol = 1e-9 - values.insert(0, Point2(0,0)) - values.insert(1, Point3(0,0,0)) + values.insert(0, Point2(0, 0)) + values.insert(1, Point3(0, 0, 0)) values.insert(2, Rot2()) values.insert(3, Pose2()) values.insert(4, Rot3()) @@ -34,18 +47,19 @@ class TestValues(unittest.TestCase): # The wrapper will automatically fix the type and storage order for you, # but for performance reasons, it's recommended to specify the correct # type and storage order. - vec = np.array([1., 2., 3.]) # for vectors, the order is not important, but dtype still is + # for vectors, the order is not important, but dtype still is + vec = np.array([1., 2., 3.]) values.insert(11, vec) mat = np.array([[1., 2.], [3., 4.]], order='F') values.insert(12, mat) # Test with dtype int and the default order='C' # This still works as the wrapper converts to the correct type and order for you # but is nornally not recommended! - mat2 = np.array([[1,2,],[3,5]]) + mat2 = np.array([[1, 2, ], [3, 5]]) values.insert(13, mat2) - self.assertTrue(values.atPoint2(0).equals(Point2(), tol)) - self.assertTrue(values.atPoint3(1).equals(Point3(), tol)) + self.assertTrue(values.atPoint2(0).equals(Point2(0,0), tol)) + self.assertTrue(values.atPoint3(1).equals(Point3(0,0,0), tol)) self.assertTrue(values.atRot2(2).equals(Rot2(), tol)) self.assertTrue(values.atPose2(3).equals(Pose2(), tol)) self.assertTrue(values.atRot3(4).equals(Rot3(), tol)) @@ -65,5 +79,6 @@ class TestValues(unittest.TestCase): actualMatrix2 = values.atMatrix(13) self.assertTrue(np.allclose(mat2, actualMatrix2, tol)) + if __name__ == "__main__": unittest.main() From 54512731e31c90f10b136b4449ffc0abc0217555 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 11:57:05 +0000 Subject: [PATCH 18/61] added setup.py --- cython/CMakeLists.txt | 1 + cython/README.md | 4 +++- cython/setup.py.in | 28 ++++++++++++++++++++++++++++ 3 files changed, 32 insertions(+), 1 deletion(-) create mode 100644 cython/setup.py.in diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index e53c0b73e..317fb7e2d 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -33,6 +33,7 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) # 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) + configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${PROJECT_BINARY_DIR}/cython/setup.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") diff --git a/cython/README.md b/cython/README.md index 8ba824f8d..78fc7e594 100644 --- a/cython/README.md +++ b/cython/README.md @@ -17,10 +17,12 @@ named **gtsam_eigency**, to interface between C++'s Eigen and Python's numpy. The wrapped module will be installed to `GTSAM_CYTHON_INSTALL_PATH`, which is by default: `/cython` -- Modify your `PYTHONPATH` to include the `GTSAM_CYTHON_INSTALL_PATH`: +- To use the library without installing system-wide: modify your `PYTHONPATH` to include the `GTSAM_CYTHON_INSTALL_PATH`: ```bash export PYTHONPATH=$PYTHONPATH: ``` +- To install system-wide: navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install` + - (the same command can be used to install into a virtual environment if it is active) UNIT TESTS ========== diff --git a/cython/setup.py.in b/cython/setup.py.in new file mode 100644 index 000000000..6a2009913 --- /dev/null +++ b/cython/setup.py.in @@ -0,0 +1,28 @@ +try: + from setuptools import setup +except ImportError: + from distutils.core import setup + +setup( + name='gtsam', + + description='Georgia Tech Smoothing And Mapping library', + url='https://bitbucket.org/gtborg/gtsam', + version='${GTSAM_VERSION_STRING}', # https://www.python.org/dev/peps/pep-0440/ + license='Simplified BSD license', + keywords='slam sam', + long_description=open('${PROJECT_SOURCE_DIR}/README.md', 'r').read(), + # https://pypi.org/pypi?%3Aaction=list_classifiers + classifiers=[ + 'Development Status :: 5 - Production/Stable', + 'Intended Audience :: Education', + 'Intended Audience :: Developers', + 'Intended Audience :: Science/Research', + 'License :: OSI Approved :: BSD License', + 'Programming Language :: Python :: 2', + 'Programming Language :: Python :: 3', + ], + + packages=['gtsam', 'gtsam_eigency'], + install_requires=open('${PROJECT_SOURCE_DIR}/cython/requirements.txt', 'r').read().splitlines() +) From f6af989e6776aee45d968f9c7eada5f74e88f63f Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 15:15:20 +0000 Subject: [PATCH 19/61] setup.py install .so files --- cython/setup.py.in | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/cython/setup.py.in b/cython/setup.py.in index 6a2009913..e99bd5de1 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -1,8 +1,15 @@ +import os try: from setuptools import setup except ImportError: from distutils.core import setup + +packages = ['gtsam', 'gtsam_eigency'] +if ${GTSAM_BUILD_UNSTABLE}: + packages += 'gtsam_unstable' + + setup( name='gtsam', @@ -23,6 +30,10 @@ setup( 'Programming Language :: Python :: 3', ], - packages=['gtsam', 'gtsam_eigency'], + packages=packages, + package_data={package: + [f for f in os.listdir(package) if os.path.splitext(f)[1] == '.so'] + for package in packages + }, install_requires=open('${PROJECT_SOURCE_DIR}/cython/requirements.txt', 'r').read().splitlines() ) From a62197ec6ecc98ff092bbf43b4b7bfa4dc490999 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 15:34:09 +0000 Subject: [PATCH 20/61] gtsam_unstable as a separate package --- cython/CMakeLists.txt | 7 ++----- cython/gtsam/__init__.py | 1 + cython/gtsam/__init__.py.in | 2 -- cython/gtsam_unstable/__init__.py | 2 ++ cython/setup.py.in | 10 +++------- 5 files changed, 8 insertions(+), 14 deletions(-) create mode 100644 cython/gtsam/__init__.py delete mode 100644 cython/gtsam/__init__.py.in create mode 100644 cython/gtsam_unstable/__init__.py diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index 317fb7e2d..c2d38fd47 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -22,19 +22,16 @@ 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 "") 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) configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${PROJECT_BINARY_DIR}/cython/setup.py) - install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam") + install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}/") # install scripts and tests install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") diff --git a/cython/gtsam/__init__.py b/cython/gtsam/__init__.py new file mode 100644 index 000000000..9bda86efe --- /dev/null +++ b/cython/gtsam/__init__.py @@ -0,0 +1 @@ +from .gtsam import * diff --git a/cython/gtsam/__init__.py.in b/cython/gtsam/__init__.py.in deleted file mode 100644 index 85451c680..000000000 --- a/cython/gtsam/__init__.py.in +++ /dev/null @@ -1,2 +0,0 @@ -from .gtsam import * -${GTSAM_UNSTABLE_IMPORT} diff --git a/cython/gtsam_unstable/__init__.py b/cython/gtsam_unstable/__init__.py new file mode 100644 index 000000000..04ce7f1e0 --- /dev/null +++ b/cython/gtsam_unstable/__init__.py @@ -0,0 +1,2 @@ +from .gtsam_unstable import * + diff --git a/cython/setup.py.in b/cython/setup.py.in index e99bd5de1..e72dbd7c1 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -1,18 +1,14 @@ import os try: - from setuptools import setup + from setuptools import setup, find_packages except ImportError: - from distutils.core import setup + from distutils.core import setup, find_packages -packages = ['gtsam', 'gtsam_eigency'] -if ${GTSAM_BUILD_UNSTABLE}: - packages += 'gtsam_unstable' - +packages = find_packages() setup( name='gtsam', - description='Georgia Tech Smoothing And Mapping library', url='https://bitbucket.org/gtborg/gtsam', version='${GTSAM_VERSION_STRING}', # https://www.python.org/dev/peps/pep-0440/ From fe3003a688a85cb7cdb80511bffec50e138ec2aa Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 15:38:25 +0000 Subject: [PATCH 21/61] install gtsam_unstable __init__.py --- cython/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index c2d38fd47..3dedbeec2 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -34,5 +34,6 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}/") # 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_unstable" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") endif () From c1b048020ee99ee209abe1d35fb0a9aaba3d6ec7 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 15:55:49 +0000 Subject: [PATCH 22/61] fixed bug with detecting nested python packages in setup.py --- cython/setup.py.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cython/setup.py.in b/cython/setup.py.in index e72dbd7c1..0a994b18f 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -28,7 +28,7 @@ setup( packages=packages, package_data={package: - [f for f in os.listdir(package) if os.path.splitext(f)[1] == '.so'] + [f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] == '.so'] for package in packages }, install_requires=open('${PROJECT_SOURCE_DIR}/cython/requirements.txt', 'r').read().splitlines() From 88dac759d7e675662919ad8fc8524d741ca81ca4 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 15:57:52 +0000 Subject: [PATCH 23/61] setup.py gets installed into correct directory --- cython/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index 3dedbeec2..d5e8b8fac 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -31,7 +31,7 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) # 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/setup.py.in ${PROJECT_BINARY_DIR}/cython/setup.py) - install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}/") + install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}") # 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_unstable" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") From 0d7b52d99fb47a2aa9131e99567ad0e1b8213a01 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 16:03:51 +0000 Subject: [PATCH 24/61] copy __init__.py before compiling c++ --- cython/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index d5e8b8fac..ca5c0ecfb 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -30,6 +30,8 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) # 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 ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py COPYONLY) + configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py COPYONLY) configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${PROJECT_BINARY_DIR}/cython/setup.py) install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}") # install scripts and tests From d56efcceadb6d9258a6441a0484d6ae85c1ca32f Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 20:32:05 +0000 Subject: [PATCH 25/61] setup.py only installs for the python version it is built for --- cython/setup.py.in | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/cython/setup.py.in b/cython/setup.py.in index 0a994b18f..0f133f781 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -22,8 +22,7 @@ setup( 'Intended Audience :: Developers', 'Intended Audience :: Science/Research', 'License :: OSI Approved :: BSD License', - 'Programming Language :: Python :: 2', - 'Programming Language :: Python :: 3', + 'Programming Language :: Python :: ${PYTHON_VERSION_MAJOR}', ], packages=packages, From fe9ede47d1956390894175ce6f26b4df66d16e3a Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Fri, 8 Feb 2019 20:32:38 +0000 Subject: [PATCH 26/61] added dlls to package_data --- cython/setup.py.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cython/setup.py.in b/cython/setup.py.in index 0f133f781..3161c36e7 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -27,7 +27,7 @@ setup( packages=packages, package_data={package: - [f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] == '.so'] + [f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] in ('.so', '.dll')] for package in packages }, install_requires=open('${PROJECT_SOURCE_DIR}/cython/requirements.txt', 'r').read().splitlines() From 135ef5a0d0647e0d9ef14071c5c27a1b88d951bf Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 13 Feb 2019 14:38:46 +0000 Subject: [PATCH 27/61] baking in requirements and README to setup.py rather than reading at install time --- cython/CMakeLists.txt | 3 +++ cython/setup.py.in | 6 ++++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index ca5c0ecfb..400e96e13 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -28,6 +28,9 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) ) endif() + file(READ "${PROJECT_SOURCE_DIR}/cython/requirements.txt" CYTHON_INSTALL_REQUIREMENTS) + file(READ "${PROJECT_SOURCE_DIR}/README.md" README_CONTENTS) + # 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 ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py COPYONLY) diff --git a/cython/setup.py.in b/cython/setup.py.in index 3161c36e7..0e22dbf11 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -14,7 +14,7 @@ setup( version='${GTSAM_VERSION_STRING}', # https://www.python.org/dev/peps/pep-0440/ license='Simplified BSD license', keywords='slam sam', - long_description=open('${PROJECT_SOURCE_DIR}/README.md', 'r').read(), + long_description='''${README_CONTENTS}''', # https://pypi.org/pypi?%3Aaction=list_classifiers classifiers=[ 'Development Status :: 5 - Production/Stable', @@ -30,5 +30,7 @@ setup( [f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] in ('.so', '.dll')] for package in packages }, - install_requires=open('${PROJECT_SOURCE_DIR}/cython/requirements.txt', 'r').read().splitlines() + install_requires=[line.strip() for line in ''' +${CYTHON_INSTALL_REQUIREMENTS} +'''.splitlines() if len(line) > 0 and not line.strip().startswith('#')] ) From 290aad3372fb9372cef8f852d9c27cb57a4154b3 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 20 Feb 2019 09:03:34 +0000 Subject: [PATCH 28/61] small change to setup.py --- cython/gtsam_unstable/__init__.py | 1 - cython/setup.py.in | 8 ++++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/cython/gtsam_unstable/__init__.py b/cython/gtsam_unstable/__init__.py index 04ce7f1e0..3195e6da4 100644 --- a/cython/gtsam_unstable/__init__.py +++ b/cython/gtsam_unstable/__init__.py @@ -1,2 +1 @@ from .gtsam_unstable import * - diff --git a/cython/setup.py.in b/cython/setup.py.in index 0e22dbf11..e631d89c0 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -21,8 +21,12 @@ setup( 'Intended Audience :: Education', 'Intended Audience :: Developers', 'Intended Audience :: Science/Research', + 'Operating System :: MacOS', + 'Operating System :: Microsoft :: Windows', + 'Operating System :: POSIX', 'License :: OSI Approved :: BSD License', - 'Programming Language :: Python :: ${PYTHON_VERSION_MAJOR}', + 'Programming Language :: Python :: 2', + 'Programming Language :: Python :: 3', ], packages=packages, @@ -32,5 +36,5 @@ setup( }, install_requires=[line.strip() for line in ''' ${CYTHON_INSTALL_REQUIREMENTS} -'''.splitlines() if len(line) > 0 and not line.strip().startswith('#')] +'''.splitlines() if len(line.strip()) > 0 and not line.strip().startswith('#')] ) From 803c14deb3058e12aa6779c1f9cb4fe7e1764e7d Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 6 Mar 2019 10:15:07 +0000 Subject: [PATCH 29/61] removed unnecessary variable from cmake --- cython/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index 400e96e13..a351ec52b 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -19,7 +19,6 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) # wrap gtsam_unstable if(GTSAM_BUILD_UNSTABLE) add_custom_target(gtsam_unstable_header DEPENDS "../gtsam_unstable/gtsam_unstable.h") - 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_unstable" # install path From fcfcceef71bc833bfc9000b487333bf20ad6df26 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Thu, 7 Mar 2019 09:11:16 +0000 Subject: [PATCH 30/61] added gtsam_unstable import back to gtsam --- cython/gtsam/__init__.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/cython/gtsam/__init__.py b/cython/gtsam/__init__.py index 9bda86efe..724572240 100644 --- a/cython/gtsam/__init__.py +++ b/cython/gtsam/__init__.py @@ -1 +1,19 @@ from .gtsam import * + +import gtsam_unstable + + +def deprecated_wrapper(item, name): + def wrapper(*args, **kwargs): + from warnings import warn + warn('importing the unstable item "{}" from gtsam is deprecated. Please import it from gtsam_unstable.'.format(name)) + return item(*args, **kwargs) + return wrapper + +for name in dir(gtsam_unstable): + if not name.startswith('__'): + item = getattr(gtsam_unstable, name) + if callable(item): + item = deprecated_wrapper(item, name) + globals()[name] = item + From 7161c0461095b92d407a2a40ce76d295a51d9ec6 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Thu, 7 Mar 2019 10:05:02 +0000 Subject: [PATCH 31/61] fixed __init__.py to not crash if gtsam_unstable doesn't exist --- cython/gtsam/__init__.py | 33 ++++++++++++++++++++------------- 1 file changed, 20 insertions(+), 13 deletions(-) diff --git a/cython/gtsam/__init__.py b/cython/gtsam/__init__.py index 724572240..f27c6fa44 100644 --- a/cython/gtsam/__init__.py +++ b/cython/gtsam/__init__.py @@ -1,19 +1,26 @@ from .gtsam import * -import gtsam_unstable +try: + import gtsam_unstable -def deprecated_wrapper(item, name): - def wrapper(*args, **kwargs): - from warnings import warn - warn('importing the unstable item "{}" from gtsam is deprecated. Please import it from gtsam_unstable.'.format(name)) - return item(*args, **kwargs) - return wrapper + def _deprecated_wrapper(item, name): + def wrapper(*args, **kwargs): + from warnings import warn + message = ('importing the unstable item "{}" directly from gtsam is deprecated. '.format(name) + + 'Please import it from gtsam_unstable.') + warn(message, category=DeprecationWarning) + return item(*args, **kwargs) + return wrapper -for name in dir(gtsam_unstable): - if not name.startswith('__'): - item = getattr(gtsam_unstable, name) - if callable(item): - item = deprecated_wrapper(item, name) - globals()[name] = item + + for name in dir(gtsam_unstable): + if not name.startswith('__'): + item = getattr(gtsam_unstable, name) + if callable(item): + item = _deprecated_wrapper(item, name) + globals()[name] = item + +except ImportError: + pass From 9b80f4b15865346d57a82bef44ccd59e191aaca9 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Thu, 7 Mar 2019 10:11:01 +0000 Subject: [PATCH 32/61] not using DeprecationWarning because it is ignored by default --- cython/gtsam/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cython/gtsam/__init__.py b/cython/gtsam/__init__.py index f27c6fa44..d40ee4502 100644 --- a/cython/gtsam/__init__.py +++ b/cython/gtsam/__init__.py @@ -9,7 +9,7 @@ try: from warnings import warn message = ('importing the unstable item "{}" directly from gtsam is deprecated. '.format(name) + 'Please import it from gtsam_unstable.') - warn(message, category=DeprecationWarning) + warn(message) return item(*args, **kwargs) return wrapper From 91fa7adf0753de4f8ff92a13c39e83bcfc2978eb Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Mon, 11 Mar 2019 14:54:12 +0000 Subject: [PATCH 33/61] added more keywords --- cython/setup.py.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cython/setup.py.in b/cython/setup.py.in index e631d89c0..a4c96f1b8 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -13,7 +13,7 @@ setup( url='https://bitbucket.org/gtborg/gtsam', version='${GTSAM_VERSION_STRING}', # https://www.python.org/dev/peps/pep-0440/ license='Simplified BSD license', - keywords='slam sam', + keywords='slam sam robotics localization mapping optimization', long_description='''${README_CONTENTS}''', # https://pypi.org/pypi?%3Aaction=list_classifiers classifiers=[ From 03500b004bc5e16e48725a3d3500b0f25bab43dd Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Mon, 11 Mar 2019 15:02:11 +0000 Subject: [PATCH 34/61] enforcing the setup script from being run from the installation directory --- cython/setup.py.in | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/cython/setup.py.in b/cython/setup.py.in index a4c96f1b8..0c37cd660 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -1,10 +1,19 @@ import os +import sys try: from setuptools import setup, find_packages except ImportError: from distutils.core import setup, find_packages +script_path = os.path.abspath(os.path.realpath(__file__)) +install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}', 'setup.py'))) +if script_path != install_path: + print('setup.py is being run from an unexpected location: "{script_path}"') + print('please run `make install` and run the script from there') + sys.exit(1) + + packages = find_packages() setup( From 5a0e7bb92a38cc3d2e6fe91584ac590dab5fb98c Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Mon, 11 Mar 2019 15:05:53 +0000 Subject: [PATCH 35/61] fixed string formatting to work with python 2 and 3 --- cython/setup.py.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cython/setup.py.in b/cython/setup.py.in index 0c37cd660..b7b0b7bc5 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -9,7 +9,7 @@ except ImportError: script_path = os.path.abspath(os.path.realpath(__file__)) install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}', 'setup.py'))) if script_path != install_path: - print('setup.py is being run from an unexpected location: "{script_path}"') + print('setup.py is being run from an unexpected location: "{}"'.format(script_path)) print('please run `make install` and run the script from there') sys.exit(1) From 0d924e23dbd2d519dfb3d9b50589caabae20ebe2 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 12 Mar 2019 14:49:18 +0800 Subject: [PATCH 36/61] Fix compilation on MKL 2019 --- cmake/FindMKL.cmake | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/cmake/FindMKL.cmake b/cmake/FindMKL.cmake index cbe46a908..32e183baa 100644 --- a/cmake/FindMKL.cmake +++ b/cmake/FindMKL.cmake @@ -206,6 +206,15 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS ) ENDIF() + IF(NOT MKL_LAPACK_LIBRARY) + FIND_LIBRARY(MKL_LAPACK_LIBRARY + mkl_intel_lp64 + PATHS + ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} + ${MKL_ROOT_DIR}/lib/ + ) + ENDIF() + # iomp5 IF("${MKL_ARCH_DIR}" STREQUAL "32") IF(UNIX AND NOT APPLE) From ecb8471e860850f8c8f8a665519d3ea6bd6fa16a Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Tue, 12 Mar 2019 09:47:25 +0000 Subject: [PATCH 37/61] updated cython README --- cython/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cython/README.md b/cython/README.md index 78fc7e594..4e76225c7 100644 --- a/cython/README.md +++ b/cython/README.md @@ -21,8 +21,9 @@ by default: `/cython` ```bash export PYTHONPATH=$PYTHONPATH: ``` -- To install system-wide: navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install` +- To install system-wide: run `make install` then navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install` - (the same command can be used to install into a virtual environment if it is active) + - note: if you don't want gtsam to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to install gtsam to a subdirectory of the build directory. UNIT TESTS ========== From a4b713454a9f955cafd44bace050957734c4f484 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Tue, 12 Mar 2019 09:49:03 +0000 Subject: [PATCH 38/61] updated cython README --- cython/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cython/README.md b/cython/README.md index 4e76225c7..a524ac04c 100644 --- a/cython/README.md +++ b/cython/README.md @@ -23,7 +23,7 @@ export PYTHONPATH=$PYTHONPATH: ``` - To install system-wide: run `make install` then navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install` - (the same command can be used to install into a virtual environment if it is active) - - note: if you don't want gtsam to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to install gtsam to a subdirectory of the build directory. + - note: if you don't want gtsam to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to cmake to install gtsam to a subdirectory of the build directory. UNIT TESTS ========== From e24b402db44850db06730008b73b96471069c3b5 Mon Sep 17 00:00:00 2001 From: chrisbeall Date: Tue, 12 Mar 2019 09:22:09 -0700 Subject: [PATCH 39/61] Turn MKL off by default. Add section on performance to INSTALL.md --- CMakeLists.txt | 10 ++-------- INSTALL.md | 37 +++++++++++++++++++++++++++++++------ 2 files changed, 33 insertions(+), 14 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 834ce732e..d9633b3c3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,8 +61,8 @@ option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the defau option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF) option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) -option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" ON) -option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON) +option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) +option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" OFF) option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) @@ -563,12 +563,6 @@ message(STATUS "===============================================================" if(GTSAM_WITH_TBB AND NOT TBB_FOUND) message(WARNING "TBB was not found - this is ok, but note that GTSAM parallelization will be disabled. Set GTSAM_WITH_TBB to 'Off' to avoid this warning.") endif() -if(GTSAM_WITH_EIGEN_MKL AND NOT MKL_FOUND) - message(WARNING "MKL was not found - this is ok, but note that MKL yields better performance. Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning.") -endif() -if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND) - message(WARNING "Your compiler does not support OpenMP - this is ok, but performance may be improved with OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning.") -endif() if(GTSAM_BUILD_PYTHON AND GTSAM_PYTHON_WARNINGS) message(WARNING "${GTSAM_PYTHON_WARNINGS}") endif() diff --git a/INSTALL.md b/INSTALL.md index e19363c7e..23953decf 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -23,18 +23,22 @@ $ make install disable the CMake flag GTSAM_WITH_TBB (enabled by default). On Ubuntu, TBB may be installed from the Ubuntu repositories, and for other platforms it may be downloaded from https://www.threadingbuildingblocks.org/ + - GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and + `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually + achieved with MKL disabled. We therefore advise you to benchmark your problem + before using MKL. Tested compilers: - - GCC 4.2-4.7 - - OSX Clang 2.9-5.0 - - OSX GCC 4.2 - - MSVC 2010, 2012 + - GCC 4.2-7.3 + - OS X Clang 2.9-10.0 + - OS X GCC 4.2 + - MSVC 2010, 2012, 2017 Tested systems: - - Ubuntu 11.04 - 18.04 - - MacOS 10.6 - 10.9 + - Ubuntu 16.04 - 18.04 + - MacOS 10.6 - 10.14 - Windows 7, 8, 8.1, 10 Known issues: @@ -134,6 +138,27 @@ MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included i $MATLABROOT can be found by executing the command `matlabroot` in MATLAB +## Performance + +Here are some tips to get the best possible performance out of GTSAM. + +1. Build in `Release` mode. GTSAM will run up to 10x faster compared to `Debug` mode. +2. Enable TBB. On modern processors with multiple cores, this can easily speed up + optimization by 30-50%. Please note that this may not be true for very small + problems where the overhead of dispatching work to multiple threads outweighs + the benefit. We recommend that you benchmark your problem with/without TBB. +3. Add `-march=native` to `GTSAM_CMAKE_CXX_FLAGS`. A performance gain of + 25-30% can be expected on modern processors. Note that this affects the portability + of your executable. It may not run when copied to another system with older/different + processor architecture. + Also note that all dependent projects *must* be compiled with the same flag, or + seg-faults and other undefined behavior may result. +4. Enable MKL. Please note that our benchmarks have shown that this helps only + in very limited cases, and actually hurts performance in the usual case. We therefore + recommend that you do *not* enable MKL, unless you have benchmarked it on + your problem and have verified that it improves performance. + + ## Debugging tips Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks and safe containers in the standard C++ library and makes problems much easier to find. From 40134c3a9ecaf32a8312ef856d383ae92c04af04 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Tue, 12 Mar 2019 11:16:32 -0700 Subject: [PATCH 40/61] Restore warnings about MKL, change notice about performance. --- CMakeLists.txt | 6 ++++++ INSTALL.md | 2 +- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d9633b3c3..186dabaf4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -563,6 +563,12 @@ message(STATUS "===============================================================" if(GTSAM_WITH_TBB AND NOT TBB_FOUND) message(WARNING "TBB was not found - this is ok, but note that GTSAM parallelization will be disabled. Set GTSAM_WITH_TBB to 'Off' to avoid this warning.") endif() +if(GTSAM_WITH_EIGEN_MKL AND NOT MKL_FOUND) + message(WARNING "MKL was not found - this is ok, but note that MKL will be disabled. Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning. See INSTALL.md for notes on performance.") +endif() +if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND) + message(WARNING "Your compiler does not support OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning. See INSTALL.md for notes on performance.") +endif() if(GTSAM_BUILD_PYTHON AND GTSAM_PYTHON_WARNINGS) message(WARNING "${GTSAM_PYTHON_WARNINGS}") endif() diff --git a/INSTALL.md b/INSTALL.md index 23953decf..3437d074d 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -153,7 +153,7 @@ Here are some tips to get the best possible performance out of GTSAM. processor architecture. Also note that all dependent projects *must* be compiled with the same flag, or seg-faults and other undefined behavior may result. -4. Enable MKL. Please note that our benchmarks have shown that this helps only +4. Possibly enable MKL. Please note that our benchmarks have shown that this helps only in very limited cases, and actually hurts performance in the usual case. We therefore recommend that you do *not* enable MKL, unless you have benchmarked it on your problem and have verified that it improves performance. From fe647a9e941bfad17a6a3d22983866fb91e78f39 Mon Sep 17 00:00:00 2001 From: Jeremy Aguilon Date: Wed, 13 Mar 2019 01:32:49 -0400 Subject: [PATCH 41/61] 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} From 9982d79d74c20704cc987cd4ca63cb0934dfb8b1 Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 13 Mar 2019 09:02:04 +0000 Subject: [PATCH 42/61] added reasoning behind the setup.py unexpected location check --- cython/README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cython/README.md b/cython/README.md index a524ac04c..b9ce2f012 100644 --- a/cython/README.md +++ b/cython/README.md @@ -24,6 +24,8 @@ export PYTHONPATH=$PYTHONPATH: - To install system-wide: run `make install` then navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install` - (the same command can be used to install into a virtual environment if it is active) - note: if you don't want gtsam to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to cmake to install gtsam to a subdirectory of the build directory. + - if you run `setup.py` from the build directory rather than the installation directory, the script will warn you with the message: `setup.py is being run from an unexpected location`. + Before `make install` is run, not all the components of the package have been copied across, so running `setup.py` from the build directory would result in an incomplete package. UNIT TESTS ========== From 173191621eee0c3ae0fde428e547335f540fe3ac Mon Sep 17 00:00:00 2001 From: Matthew Broadway Date: Wed, 13 Mar 2019 09:45:56 +0000 Subject: [PATCH 43/61] made it possible to disable the setup.py check --- cython/setup.py.in | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/cython/setup.py.in b/cython/setup.py.in index b7b0b7bc5..d6af28762 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -5,13 +5,13 @@ try: except ImportError: from distutils.core import setup, find_packages - -script_path = os.path.abspath(os.path.realpath(__file__)) -install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}', 'setup.py'))) -if script_path != install_path: - print('setup.py is being run from an unexpected location: "{}"'.format(script_path)) - print('please run `make install` and run the script from there') - sys.exit(1) +if 'SETUP_PY_NO_CHECK' not in os.environ: + script_path = os.path.abspath(os.path.realpath(__file__)) + install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}', 'setup.py'))) + if script_path != install_path: + print('setup.py is being run from an unexpected location: "{}"'.format(script_path)) + print('please run `make install` and run the script from there') + sys.exit(1) packages = find_packages() From 724a906bee985b3c7b3141fce81dd72141163094 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 13 Mar 2019 22:47:23 -0400 Subject: [PATCH 44/61] Test existing readG2o --- cython/gtsam/tests/test_dataset.py | 33 ++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 cython/gtsam/tests/test_dataset.py diff --git a/cython/gtsam/tests/test_dataset.py b/cython/gtsam/tests/test_dataset.py new file mode 100644 index 000000000..c634fb3f7 --- /dev/null +++ b/cython/gtsam/tests/test_dataset.py @@ -0,0 +1,33 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for testing dataset access. +Author: Frank Dellaert +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import unittest + +import gtsam + + +class TestDataset(unittest.TestCase): + def setUp(self): + pass + + def test_3d_graph(self): + is3D = True + g2o_file = gtsam.findExampleDataFile("pose3example.txt") + graph, initial = gtsam.readG2o(g2o_file, is3D) + self.assertEqual(graph.size(), 6) + self.assertEqual(initial.size(), 5) + + +if __name__ == '__main__': + unittest.main() From 0ca3f9d199e8900f119248a64a71694af27bcf17 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 13 Mar 2019 23:22:37 -0400 Subject: [PATCH 45/61] Use c+11 initializer lists --- gtsam/slam/tests/testDataset.cpp | 91 +++++++++++++++----------------- 1 file changed, 44 insertions(+), 47 deletions(-) diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 39c2d3722..4a327b8e1 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -162,65 +162,62 @@ TEST( dataSet, readG2o) } /* ************************************************************************* */ -TEST( dataSet, readG2o3D) -{ +TEST(dataSet, readG2o3D) { const string g2oFile = findExampleDataFile("pose3example"); NonlinearFactorGraph::shared_ptr actualGraph; Values::shared_ptr actualValues; bool is3D = true; boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); - Values expectedValues; - Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); - Point3 p0 = Point3(0.000000, 0.000000, 0.000000); - expectedValues.insert(0, Pose3(R0, p0)); + // Initialize 5 poses with quaternion/point3 values: + const std::vector poses = { + {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}}, + {{0.854230, 0.190253, 0.283162, -0.392318}, + {1.001367, 0.015390, 0.004948}}, + {{0.421446, -0.351729, -0.597838, 0.584174}, + {1.993500, 0.023275, 0.003793}}, + {{0.067024, 0.331798, -0.200659, 0.919323}, + {2.004291, 1.024305, 0.018047}}, + {{0.765488, -0.035697, -0.462490, 0.445933}, + {0.999908, 1.055073, 0.020212}}, + }; - Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); - Point3 p1 = Point3(1.001367, 0.015390, 0.004948); - expectedValues.insert(1, Pose3(R1, p1)); + // Check values + for (size_t i : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(poses[i], actualValues->at(i), 1e-5)); + } - Rot3 R2 = Rot3::Quaternion(0.421446, -0.351729, -0.597838, 0.584174 ); - Point3 p2 = Point3(1.993500, 0.023275, 0.003793); - expectedValues.insert(2, Pose3(R2, p2)); + // Initialize 6 relative measurements with quaternion/point3 values: + const std::vector measurements = { + {{0.854230, 0.190253, 0.283162, -0.392318}, + {1.001367, 0.015390, 0.004948}}, + {{0.105373, 0.311512, 0.656877, -0.678505}, + {0.523923, 0.776654, 0.326659}}, + {{0.568551, 0.595795, -0.561677, 0.079353}, + {0.910927, 0.055169, -0.411761}}, + {{0.542221, -0.592077, 0.303380, -0.513226}, + {0.775288, 0.228798, -0.596923}}, + {{0.327419, -0.125250, -0.534379, 0.769122}, + {-0.577841, 0.628016, -0.543592}}, + {{0.083672, 0.104639, 0.627755, 0.766795}, + {-0.623267, 0.086928, 0.773222}}, + }; - Rot3 R3 = Rot3::Quaternion(0.067024, 0.331798, -0.200659, 0.919323); - Point3 p3 = Point3(2.004291, 1.024305, 0.018047); - expectedValues.insert(3, Pose3(R3, p3)); + // Specify connectivity: + std::vector> pairs = {{0, 1}, {1, 2}, {2, 3}, + {3, 4}, {1, 4}, {3, 0}}; - Rot3 R4 = Rot3::Quaternion(0.765488, -0.035697, -0.462490, 0.445933); - Point3 p4 = Point3(0.999908, 1.055073, 0.020212); - expectedValues.insert(4, Pose3(R4, p4)); - - EXPECT(assert_equal(expectedValues,*actualValues,1e-5)); - - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(6) << 10000.0,10000.0,10000.0,10000.0,10000.0,10000.0).finished()); + // Create expected factor graph + auto model = noiseModel::Isotropic::Precision(6, 10000); NonlinearFactorGraph expectedGraph; + size_t i = 0; + for (const auto& keys : pairs) { + expectedGraph.emplace_shared>( + keys.first, keys.second, measurements[i++], model); + } - Point3 p01 = Point3(1.001367, 0.015390, 0.004948); - Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); - expectedGraph.emplace_shared >(0, 1, Pose3(R01,p01), model); - - Point3 p12 = Point3(0.523923, 0.776654, 0.326659); - Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); - expectedGraph.emplace_shared >(1, 2, Pose3(R12,p12), model); - - Point3 p23 = Point3(0.910927, 0.055169, -0.411761); - Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); - expectedGraph.emplace_shared >(2, 3, Pose3(R23,p23), model); - - Point3 p34 = Point3(0.775288, 0.228798, -0.596923); - Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); - expectedGraph.emplace_shared >(3, 4, Pose3(R34,p34), model); - - Point3 p14 = Point3(-0.577841, 0.628016, -0.543592); - Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); - expectedGraph.emplace_shared >(1, 4, Pose3(R14,p14), model); - - Point3 p30 = Point3(-0.623267, 0.086928, 0.773222); - Rot3 R30 = Rot3::Quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); - expectedGraph.emplace_shared >(3, 0, Pose3(R30,p30), model); - - EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); + // Check if actual graph is correct + EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5)); } /* ************************************************************************* */ From d8ee79fb8fd747bf2d497bacb9c808cb8d1b5fb6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 14 Mar 2019 00:27:02 -0400 Subject: [PATCH 46/61] Working parseG2o3D --- gtsam/slam/tests/testDataset.cpp | 167 +++++++++++++++++++++++++------ 1 file changed, 135 insertions(+), 32 deletions(-) diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 4a327b8e1..3e2ed51c7 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -162,33 +162,96 @@ TEST( dataSet, readG2o) } /* ************************************************************************* */ +using BetweenFactors = std::vector::shared_ptr>; +using Poses = std::map; +std::pair parseG2o3D(const std::string&); + +#include +#define LINESIZE 81920 + +/* ************************************************************************* */ +std::pair parseG2o3D(const string& filename) { + ifstream is(filename.c_str()); + if (!is) throw invalid_argument("load3D: can not find file " + filename); + + BetweenFactors factors; + Poses poses; + + while (!is.eof()) { + char buf[LINESIZE]; + is.getline(buf, LINESIZE); + istringstream ls(buf); + string tag; + ls >> tag; + + if (tag == "VERTEX3") { + Key id; + double x, y, z, roll, pitch, yaw; + ls >> id >> x >> y >> z >> roll >> pitch >> yaw; + poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z})); + } + if (tag == "VERTEX_SE3:QUAT") { + Key id; + double x, y, z, qx, qy, qz, qw; + ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; + poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z})); + } + } + is.clear(); /* clears the end-of-file and error flags */ + is.seekg(0, ios::beg); + + while (!is.eof()) { + char buf[LINESIZE]; + is.getline(buf, LINESIZE); + istringstream ls(buf); + string tag; + ls >> tag; + + if (tag == "EDGE3") { + Key id1, id2; + double x, y, z, roll, pitch, yaw; + ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; + Matrix m(6, 6); + for (size_t i = 0; i < 6; i++) + for (size_t j = i; j < 6; j++) ls >> m(i, j); + SharedNoiseModel model = noiseModel::Gaussian::Information(m); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model)); + } + if (tag == "EDGE_SE3:QUAT") { + Key id1, id2; + double x, y, z, qx, qy, qz, qw; + ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; + Matrix m(6, 6); + for (size_t i = 0; i < 6; i++) { + for (size_t j = i; j < 6; j++) { + double mij; + ls >> mij; + m(i, j) = mij; + m(j, i) = mij; + } + } + Matrix mgtsam(6, 6); + + mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // cov rotation + mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // cov translation + mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3); // off diagonal + mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal + + SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model)); + } + } + return make_pair(factors, poses); +} + TEST(dataSet, readG2o3D) { const string g2oFile = findExampleDataFile("pose3example"); - NonlinearFactorGraph::shared_ptr actualGraph; - Values::shared_ptr actualValues; - bool is3D = true; - boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); - - // Initialize 5 poses with quaternion/point3 values: - const std::vector poses = { - {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}}, - {{0.854230, 0.190253, 0.283162, -0.392318}, - {1.001367, 0.015390, 0.004948}}, - {{0.421446, -0.351729, -0.597838, 0.584174}, - {1.993500, 0.023275, 0.003793}}, - {{0.067024, 0.331798, -0.200659, 0.919323}, - {2.004291, 1.024305, 0.018047}}, - {{0.765488, -0.035697, -0.462490, 0.445933}, - {0.999908, 1.055073, 0.020212}}, - }; - - // Check values - for (size_t i : {0, 1, 2, 3, 4}) { - EXPECT(assert_equal(poses[i], actualValues->at(i), 1e-5)); - } + auto model = noiseModel::Isotropic::Precision(6, 10000); // Initialize 6 relative measurements with quaternion/point3 values: - const std::vector measurements = { + const std::vector relative_poses = { {{0.854230, 0.190253, 0.283162, -0.392318}, {1.001367, 0.015390, 0.004948}}, {{0.105373, 0.311512, 0.656877, -0.678505}, @@ -203,21 +266,61 @@ TEST(dataSet, readG2o3D) { {-0.623267, 0.086928, 0.773222}}, }; - // Specify connectivity: - std::vector> pairs = {{0, 1}, {1, 2}, {2, 3}, - {3, 4}, {1, 4}, {3, 0}}; + // Initialize 5 poses with quaternion/point3 values: + const std::vector poses = { + {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}}, + {{0.854230, 0.190253, 0.283162, -0.392318}, + {1.001367, 0.015390, 0.004948}}, + {{0.421446, -0.351729, -0.597838, 0.584174}, + {1.993500, 0.023275, 0.003793}}, + {{0.067024, 0.331798, -0.200659, 0.919323}, + {2.004291, 1.024305, 0.018047}}, + {{0.765488, -0.035697, -0.462490, 0.445933}, + {0.999908, 1.055073, 0.020212}}, + }; - // Create expected factor graph - auto model = noiseModel::Isotropic::Precision(6, 10000); - NonlinearFactorGraph expectedGraph; + // Specify connectivity: + using KeyPair = pair; + std::vector edges = {{0, 1}, {1, 2}, {2, 3}, {3, 4}, {1, 4}, {3, 0}}; + + // Created expected factor graph size_t i = 0; - for (const auto& keys : pairs) { + NonlinearFactorGraph expectedGraph; + for (const auto& keys : edges) { expectedGraph.emplace_shared>( - keys.first, keys.second, measurements[i++], model); + keys.first, keys.second, relative_poses[i++], model); } - // Check if actual graph is correct + // Call parse version + BetweenFactors actualFactors; + Poses actualPoses; + std::tie(actualFactors, actualPoses) = parseG2o3D(g2oFile); + + // Check factors + for (size_t i : {0, 1, 2, 3, 4, 5}) { + EXPECT(assert_equal( + *boost::dynamic_pointer_cast>(expectedGraph[i]), + *actualFactors[i], 1e-5)); + } + + // Check poses + for (size_t j : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); + } + + // Call graph version + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + bool is3D = true; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); + + // Check factor graph EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5)); + + // Check values + for (size_t j : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(poses[j], actualValues->at(j), 1e-5)); + } } /* ************************************************************************* */ From a47c52cb5ee858fec44b4fde82734aff95e47f64 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 14 Mar 2019 00:58:22 -0400 Subject: [PATCH 47/61] Split parsing and moved to dataset.* --- gtsam/slam/dataset.cpp | 100 ++++++++++++++++-------------- gtsam/slam/dataset.h | 10 ++- gtsam/slam/tests/testDataset.cpp | 101 ++----------------------------- 3 files changed, 67 insertions(+), 144 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 34370d4e2..b7d1b04d6 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -409,17 +409,17 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, /* ************************************************************************* */ GraphAndValues readG2o(const string& g2oFile, const bool is3D, - KernelFunctionType kernelFunctionType) { - // just call load2D - int maxID = 0; - bool addNoise = false; - bool smart = true; - - if(is3D) + KernelFunctionType kernelFunctionType) { + if (is3D) { return load3D(g2oFile); - - return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, - NoiseFormatG2O, kernelFunctionType); + } else { + // just call load2D + int maxID = 0; + bool addNoise = false; + bool smart = true; + return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, + NoiseFormatG2O, kernelFunctionType); + } } /* ************************************************************************* */ @@ -510,15 +510,12 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, } /* ************************************************************************* */ -GraphAndValues load3D(const string& filename) { - +std::map parse3DPoses(const string& filename) { ifstream is(filename.c_str()); if (!is) - throw invalid_argument("load3D: can not find file " + filename); - - Values::shared_ptr initial(new Values); - NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); + throw invalid_argument("parse3DPoses: can not find file " + filename); + std::map poses; while (!is.eof()) { char buf[LINESIZE]; is.getline(buf, LINESIZE); @@ -530,22 +527,24 @@ GraphAndValues load3D(const string& filename) { Key id; double x, y, z, roll, pitch, yaw; ls >> id >> x >> y >> z >> roll >> pitch >> yaw; - Rot3 R = Rot3::Ypr(yaw,pitch,roll); - Point3 t = Point3(x, y, z); - initial->insert(id, Pose3(R,t)); + poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z})); } if (tag == "VERTEX_SE3:QUAT") { Key id; double x, y, z, qx, qy, qz, qw; ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; - Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); - Point3 t = Point3(x, y, z); - initial->insert(id, Pose3(R,t)); + poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z})); } } - is.clear(); /* clears the end-of-file and error flags */ - is.seekg(0, ios::beg); + return poses; +} +/* ************************************************************************* */ +std::vector::shared_ptr> parse3DFactors(const string& filename) { + ifstream is(filename.c_str()); + if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename); + + std::vector::shared_ptr> factors; while (!is.eof()) { char buf[LINESIZE]; is.getline(buf, LINESIZE); @@ -557,44 +556,55 @@ GraphAndValues load3D(const string& filename) { Key id1, id2; double x, y, z, roll, pitch, yaw; ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; - Rot3 R = Rot3::Ypr(yaw,pitch,roll); - Point3 t = Point3(x, y, z); - Matrix m = I_6x6; - for (int i = 0; i < 6; i++) - for (int j = i; j < 6; j++) - ls >> m(i, j); + Matrix m(6, 6); + for (size_t i = 0; i < 6; i++) + for (size_t j = i; j < 6; j++) ls >> m(i, j); SharedNoiseModel model = noiseModel::Gaussian::Information(m); - NonlinearFactor::shared_ptr factor( - new BetweenFactor(id1, id2, Pose3(R,t), model)); - graph->push_back(factor); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model)); } if (tag == "EDGE_SE3:QUAT") { - Matrix m = I_6x6; Key id1, id2; double x, y, z, qx, qy, qz, qw; ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; - Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); - Point3 t = Point3(x, y, z); - for (int i = 0; i < 6; i++){ - for (int j = i; j < 6; j++){ + Matrix m(6, 6); + for (size_t i = 0; i < 6; i++) { + for (size_t j = i; j < 6; j++) { double mij; ls >> mij; m(i, j) = mij; m(j, i) = mij; } } - Matrix mgtsam = I_6x6; + Matrix mgtsam(6, 6); - mgtsam.block<3,3>(0,0) = m.block<3,3>(3,3); // cov rotation - mgtsam.block<3,3>(3,3) = m.block<3,3>(0,0); // cov translation - mgtsam.block<3,3>(0,3) = m.block<3,3>(0,3); // off diagonal - mgtsam.block<3,3>(3,0) = m.block<3,3>(3,0); // off diagonal + mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // cov rotation + mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // cov translation + mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3); // off diagonal + mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); - NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, Pose3(R,t), model)); - graph->push_back(factor); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model)); } } + return factors; +} + +/* ************************************************************************* */ +GraphAndValues load3D(const string& filename) { + const auto factors = parse3DFactors(filename); + NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); + for (const auto& factor : factors) { + graph->push_back(factor); + } + + const auto poses = parse3DPoses(filename); + Values::shared_ptr initial(new Values); + for (const auto& key_pose : poses) { + initial->insert(key_pose.first, key_pose.second); + } + return make_pair(graph, initial); } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 7c9b54a6f..9706bace0 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -153,9 +153,13 @@ GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, const std::string& filename); -/** - * Load TORO 3D Graph - */ +/// Parse edges in 3D TORO graph file into a set of BetweenFactors. +GTSAM_EXPORT std::vector::shared_ptr> parse3DFactors(const std::string& filename); + +/// Parse vertices in 3D TORO graph file into a map of Pose3s. +GTSAM_EXPORT std::map parse3DPoses(const std::string& filename); + +/// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); /// A measurement with its camera index diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 3e2ed51c7..0e131af32 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -162,90 +162,6 @@ TEST( dataSet, readG2o) } /* ************************************************************************* */ -using BetweenFactors = std::vector::shared_ptr>; -using Poses = std::map; -std::pair parseG2o3D(const std::string&); - -#include -#define LINESIZE 81920 - -/* ************************************************************************* */ -std::pair parseG2o3D(const string& filename) { - ifstream is(filename.c_str()); - if (!is) throw invalid_argument("load3D: can not find file " + filename); - - BetweenFactors factors; - Poses poses; - - while (!is.eof()) { - char buf[LINESIZE]; - is.getline(buf, LINESIZE); - istringstream ls(buf); - string tag; - ls >> tag; - - if (tag == "VERTEX3") { - Key id; - double x, y, z, roll, pitch, yaw; - ls >> id >> x >> y >> z >> roll >> pitch >> yaw; - poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z})); - } - if (tag == "VERTEX_SE3:QUAT") { - Key id; - double x, y, z, qx, qy, qz, qw; - ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; - poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z})); - } - } - is.clear(); /* clears the end-of-file and error flags */ - is.seekg(0, ios::beg); - - while (!is.eof()) { - char buf[LINESIZE]; - is.getline(buf, LINESIZE); - istringstream ls(buf); - string tag; - ls >> tag; - - if (tag == "EDGE3") { - Key id1, id2; - double x, y, z, roll, pitch, yaw; - ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; - Matrix m(6, 6); - for (size_t i = 0; i < 6; i++) - for (size_t j = i; j < 6; j++) ls >> m(i, j); - SharedNoiseModel model = noiseModel::Gaussian::Information(m); - factors.emplace_back(new BetweenFactor( - id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model)); - } - if (tag == "EDGE_SE3:QUAT") { - Key id1, id2; - double x, y, z, qx, qy, qz, qw; - ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; - Matrix m(6, 6); - for (size_t i = 0; i < 6; i++) { - for (size_t j = i; j < 6; j++) { - double mij; - ls >> mij; - m(i, j) = mij; - m(j, i) = mij; - } - } - Matrix mgtsam(6, 6); - - mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // cov rotation - mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // cov translation - mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3); // off diagonal - mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal - - SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); - factors.emplace_back(new BetweenFactor( - id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model)); - } - } - return make_pair(factors, poses); -} - TEST(dataSet, readG2o3D) { const string g2oFile = findExampleDataFile("pose3example"); auto model = noiseModel::Isotropic::Precision(6, 10000); @@ -291,33 +207,26 @@ TEST(dataSet, readG2o3D) { keys.first, keys.second, relative_poses[i++], model); } - // Call parse version - BetweenFactors actualFactors; - Poses actualPoses; - std::tie(actualFactors, actualPoses) = parseG2o3D(g2oFile); - - // Check factors + // Check factor parsing + const auto actualFactors = parse3DFactors(g2oFile); for (size_t i : {0, 1, 2, 3, 4, 5}) { EXPECT(assert_equal( *boost::dynamic_pointer_cast>(expectedGraph[i]), *actualFactors[i], 1e-5)); } - // Check poses + // Check pose parsing + const auto actualPoses = parse3DPoses(g2oFile); for (size_t j : {0, 1, 2, 3, 4}) { EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); } - // Call graph version + // Check graph version NonlinearFactorGraph::shared_ptr actualGraph; Values::shared_ptr actualValues; bool is3D = true; boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); - - // Check factor graph EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5)); - - // Check values for (size_t j : {0, 1, 2, 3, 4}) { EXPECT(assert_equal(poses[j], actualValues->at(j), 1e-5)); } From 88ac6de4afb1cef6ac6837f1bd33280c9f0cfb5d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 14 Mar 2019 01:25:06 -0400 Subject: [PATCH 48/61] Wrapped parse3DFactors --- cython/gtsam/tests/test_dataset.py | 23 +++++++++++++++++------ gtsam.h | 10 ++++++++++ gtsam/slam/dataset.cpp | 2 +- gtsam/slam/dataset.h | 5 ++++- gtsam/slam/tests/testDataset.cpp | 2 +- 5 files changed, 33 insertions(+), 9 deletions(-) diff --git a/cython/gtsam/tests/test_dataset.py b/cython/gtsam/tests/test_dataset.py index c634fb3f7..e561be1a8 100644 --- a/cython/gtsam/tests/test_dataset.py +++ b/cython/gtsam/tests/test_dataset.py @@ -8,26 +8,37 @@ See LICENSE for the license information Unit tests for testing dataset access. Author: Frank Dellaert """ -# pylint: disable=invalid-name, E1101 +# pylint: disable=invalid-name, no-name-in-module, no-member from __future__ import print_function import unittest import gtsam +from gtsam import BetweenFactorPose3, BetweenFactorPose3s class TestDataset(unittest.TestCase): - def setUp(self): - pass + """Tests for datasets.h wrapper.""" - def test_3d_graph(self): + def setUp(self): + """Get some common paths.""" + self.pose3_example_g2o_file = gtsam.findExampleDataFile( + "pose3example.txt") + + def test_readG2o3D(self): + """Test reading directly into factor graph.""" is3D = True - g2o_file = gtsam.findExampleDataFile("pose3example.txt") - graph, initial = gtsam.readG2o(g2o_file, is3D) + graph, initial = gtsam.readG2o(self.pose3_example_g2o_file, is3D) self.assertEqual(graph.size(), 6) self.assertEqual(initial.size(), 5) + def test_parse3Dfactors(self): + """Test parsing into data structure.""" + factors = gtsam.parse3DFactors(self.pose3_example_g2o_file) + self.assertEqual(factors.size(), 6) + self.assertIsInstance(factors.at(0), BetweenFactorPose3) + if __name__ == '__main__': unittest.main() diff --git a/gtsam.h b/gtsam.h index 48768db40..255a7a449 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2480,6 +2480,16 @@ void save2D(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, string filename); +// std::vector::shared_ptr> +class BetweenFactorPose3s +{ + size_t size() const; + gtsam::BetweenFactorPose3* at(size_t i) const; +}; + +gtsam::BetweenFactorPose3s parse3DFactors(string filename); +pair load3D(string filename); + pair readG2o(string filename); pair readG2o(string filename, bool is3D); void writeG2o(const gtsam::NonlinearFactorGraph& graph, diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index b7d1b04d6..c32a049e2 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -540,7 +540,7 @@ std::map parse3DPoses(const string& filename) { } /* ************************************************************************* */ -std::vector::shared_ptr> parse3DFactors(const string& filename) { +BetweenFactorPose3s parse3DFactors(const string& filename) { ifstream is(filename.c_str()); if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 9706bace0..2106df48d 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include #include @@ -34,6 +35,7 @@ #include // for pair #include #include +#include namespace gtsam { @@ -154,7 +156,8 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, const std::string& filename); /// Parse edges in 3D TORO graph file into a set of BetweenFactors. -GTSAM_EXPORT std::vector::shared_ptr> parse3DFactors(const std::string& filename); +using BetweenFactorPose3s = std::vector::shared_ptr>; +GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename); /// Parse vertices in 3D TORO graph file into a map of Pose3s. GTSAM_EXPORT std::map parse3DPoses(const std::string& filename); diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 0e131af32..9434280d4 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -16,8 +16,8 @@ */ -#include #include +#include #include #include #include From cbb84a64360cce71ced04ba0ad5f8224ef746c53 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 14 Mar 2019 14:14:20 +0000 Subject: [PATCH 49/61] Added information about LieGroup helper class --- GTSAM-Concepts.md | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 366a58a09..a6cfee984 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -97,12 +97,24 @@ Note that in the Lie group case, the usual valid expressions for Retract and Loc For Lie groups, the `exponential map` above is the most obvious mapping: it associates straight lines in the tangent space with geodesics on the manifold -(and it's inverse, the log map). However, there are two cases in which we deviate from this: +(and it's inverse, the log map). However, there are several cases in which we deviate from this: However, the exponential map is unnecessarily expensive for use in optimization. Hence, in GTSAM there is the option to provide a cheaper chart by means of the `ChartAtOrigin` struct in a class. This is done for *SE(2)*, *SO(3)* and *SE(3)* (see `Pose2`, `Rot3`, `Pose3`) Most Lie groups we care about are *Matrix groups*, continuous sub-groups of *GL(n)*, the group of *n x n* invertible matrices. In this case, a lot of the derivatives calculations needed can be standardized, and this is done by the `LieGroup` superclass. You only need to provide an `AdjointMap` method. +A CRTP helper class `LieGroup` is available that can take a class and create some of the Lie group methods automatically. The class needs: + +* operator* : implements group operator +* inverse: implements group inverse +* AdjointMap: maps tangent vectors according to group element +* Expmap/Logmap: exponential map and its inverse +* ChartAtOrigin: struct where you define Retract/Local at origin + +To use, simply derive, but also say `using LieGroup::inverse` so you get an inverse with a derivative. + +Finally, to create the traits automatically you can use `internal::LieGroupTraits` + Vector Space ------------ From dd7fa966e4093d166c4301b64c1bb9cf6a0cb9c7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 15 Mar 2019 00:25:52 -0400 Subject: [PATCH 50/61] Added print in base class so all derived have it. Added comment how wrap currently does not handle Base class correctly in case of name clash, apparently. --- gtsam.h | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/gtsam.h b/gtsam.h index 48768db40..ed4bf8425 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1218,6 +1218,13 @@ class VariableIndex { namespace noiseModel { #include virtual class Base { + void print(string s) const; + // Methods below are available for all noise models. However, can't add them + // because wrap (incorrectly) thinks robust classes derive from this Base as well. + // bool isConstrained() const; + // bool isUnit() const; + // size_t dim() const; + // Vector sigmas() const; }; virtual class Gaussian : gtsam::noiseModel::Base { @@ -1225,7 +1232,6 @@ virtual class Gaussian : gtsam::noiseModel::Base { static gtsam::noiseModel::Gaussian* Covariance(Matrix R); Matrix R() const; bool equals(gtsam::noiseModel::Base& expected, double tol); - void print(string s) const; // enabling serialization functionality void serializable() const; @@ -1236,7 +1242,6 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian { static gtsam::noiseModel::Diagonal* Variances(Vector variances); static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); Matrix R() const; - void print(string s) const; // enabling serialization functionality void serializable() const; @@ -1263,7 +1268,6 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal { static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); - void print(string s) const; // enabling serialization functionality void serializable() const; @@ -1271,7 +1275,6 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal { virtual class Unit : gtsam::noiseModel::Isotropic { static gtsam::noiseModel::Unit* Create(size_t dim); - void print(string s) const; // enabling serialization functionality void serializable() const; @@ -1279,11 +1282,11 @@ virtual class Unit : gtsam::noiseModel::Isotropic { namespace mEstimator { virtual class Base { + void print(string s) const; }; virtual class Null: gtsam::noiseModel::mEstimator::Base { Null(); - void print(string s) const; static gtsam::noiseModel::mEstimator::Null* Create(); // enabling serialization functionality @@ -1292,7 +1295,6 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base { virtual class Fair: gtsam::noiseModel::mEstimator::Base { Fair(double c); - void print(string s) const; static gtsam::noiseModel::mEstimator::Fair* Create(double c); // enabling serialization functionality @@ -1301,7 +1303,6 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base { virtual class Huber: gtsam::noiseModel::mEstimator::Base { Huber(double k); - void print(string s) const; static gtsam::noiseModel::mEstimator::Huber* Create(double k); // enabling serialization functionality @@ -1310,7 +1311,6 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { virtual class Tukey: gtsam::noiseModel::mEstimator::Base { Tukey(double k); - void print(string s) const; static gtsam::noiseModel::mEstimator::Tukey* Create(double k); // enabling serialization functionality @@ -1322,7 +1322,6 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { virtual class Robust : gtsam::noiseModel::Base { Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); - void print(string s) const; // enabling serialization functionality void serializable() const; From a7e60a08fe5a5c6345eb8db9206fe0a24b40b231 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 18 Mar 2019 23:15:53 -0400 Subject: [PATCH 51/61] Wrapped more useful noiseModel methods --- gtsam.h | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index ed4bf8425..fb72660d5 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1230,9 +1230,19 @@ virtual class Base { virtual class Gaussian : gtsam::noiseModel::Base { static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); static gtsam::noiseModel::Gaussian* Covariance(Matrix R); - Matrix R() const; + bool equals(gtsam::noiseModel::Base& expected, double tol); + // access to noise model + Matrix R() const; + Matrix information() const; + Matrix covariance() const; + + // Whitening operations + Vector whiten(Vector v) const; + Vector unwhiten(Vector v) const; + Matrix Whiten(Matrix H) const; + // enabling serialization functionality void serializable() const; }; @@ -1243,6 +1253,11 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian { static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); Matrix R() const; + // access to noise model + Vector sigmas() const; + Vector invsigmas() const; + Vector precisions() const; + // enabling serialization functionality void serializable() const; }; @@ -1269,6 +1284,9 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal { static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); + // access to noise model + double sigma() const; + // enabling serialization functionality void serializable() const; }; From 205803a0eabd2c0b1fd4098bb37ee0e6b1b71a30 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 19 Mar 2019 00:11:45 -0400 Subject: [PATCH 52/61] Better optimization parameter wrapping, plus python test --- cython/gtsam/tests/test_NonlinearOptimizer.py | 71 +++++++++++++++++++ gtsam.h | 29 ++++++-- gtsam/nonlinear/LevenbergMarquardtParams.h | 6 +- gtsam/nonlinear/NonlinearOptimizerParams.h | 47 +++--------- 4 files changed, 109 insertions(+), 44 deletions(-) create mode 100644 cython/gtsam/tests/test_NonlinearOptimizer.py diff --git a/cython/gtsam/tests/test_NonlinearOptimizer.py b/cython/gtsam/tests/test_NonlinearOptimizer.py new file mode 100644 index 000000000..0d0318f40 --- /dev/null +++ b/cython/gtsam/tests/test_NonlinearOptimizer.py @@ -0,0 +1,71 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for IMU testing scenarios. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +# pylint: disable=invalid-name, no-name-in-module + +from __future__ import print_function + +import unittest + +import gtsam +from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer, + GaussNewtonParams, LevenbergMarquardtOptimizer, + LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, + Point2, PriorFactorPoint2, Values) + +KEY1 = 1 +KEY2 = 2 + + +class TestScenario(unittest.TestCase): + def test_optimize(self): + """Do trivial test with three optimizer variants.""" + fg = NonlinearFactorGraph() + model = gtsam.noiseModel_Unit.Create(2) + fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model)) + + # test error at minimum + xstar = Point2(0, 0) + optimal_values = Values() + optimal_values.insert(KEY1, xstar) + self.assertEqual(0.0, fg.error(optimal_values), 0.0) + + # test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = + x0 = Point2(3, 3) + initial_values = Values() + initial_values.insert(KEY1, x0) + self.assertEqual(9.0, fg.error(initial_values), 1e-3) + + # optimize parameters + ordering = Ordering() + ordering.push_back(KEY1) + + # Gauss-Newton + gnParams = GaussNewtonParams() + gnParams.setOrdering(ordering) + actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize() + self.assertAlmostEqual(0, fg.error(actual1)) + + # Levenberg-Marquardt + lmParams = LevenbergMarquardtParams.CeresDefaults() + lmParams.setOrdering(ordering) + actual2 = LevenbergMarquardtOptimizer( + fg, initial_values, lmParams).optimize() + self.assertAlmostEqual(0, fg.error(actual2)) + + # Dogleg + dlParams = DoglegParams() + dlParams.setOrdering(ordering) + actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize() + self.assertAlmostEqual(0, fg.error(actual3)) + + +if __name__ == "__main__": + unittest.main() diff --git a/gtsam.h b/gtsam.h index ed4bf8425..50d59f973 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2001,10 +2001,12 @@ virtual class NonlinearOptimizerParams { void setVerbosity(string s); string getLinearSolverType() const; - void setLinearSolverType(string solver); - void setOrdering(const gtsam::Ordering& ordering); + void setIterativeParams(gtsam::IterativeOptimizationParameters* params); + void setOrdering(const gtsam::Ordering& ordering); + string getOrderingType() const; + void setOrderingType(string ordering); bool isMultifrontal() const; bool isSequential() const; @@ -2025,15 +2027,32 @@ virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { LevenbergMarquardtParams(); - double getlambdaInitial() const; + bool getDiagonalDamping() const; double getlambdaFactor() const; + double getlambdaInitial() const; + double getlambdaLowerBound() const; double getlambdaUpperBound() const; + bool getUseFixedLambdaFactor(); + string getLogFile() const; string getVerbosityLM() const; - - void setlambdaInitial(double value); + + void setDiagonalDamping(bool flag); void setlambdaFactor(double value); + void setlambdaInitial(double value); + void setlambdaLowerBound(double value); void setlambdaUpperBound(double value); + void setUseFixedLambdaFactor(bool flag); + void setLogFile(string s); void setVerbosityLM(string s); + + static gtsam::LevenbergMarquardtParams LegacyDefaults(); + static gtsam::LevenbergMarquardtParams CeresDefaults(); + + static gtsam::LevenbergMarquardtParams EnsureHasOrdering( + gtsam::LevenbergMarquardtParams params, + const gtsam::NonlinearFactorGraph& graph); + static gtsam::LevenbergMarquardtParams ReplaceOrdering( + gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering); }; #include diff --git a/gtsam/nonlinear/LevenbergMarquardtParams.h b/gtsam/nonlinear/LevenbergMarquardtParams.h index abb8c3c22..491fbd233 100644 --- a/gtsam/nonlinear/LevenbergMarquardtParams.h +++ b/gtsam/nonlinear/LevenbergMarquardtParams.h @@ -122,22 +122,24 @@ public: virtual ~LevenbergMarquardtParams() {} void print(const std::string& str = "") const override; - /// @name Getters/Setters, mainly for MATLAB. Use fields above in C++. + /// @name Getters/Setters, mainly for wrappers. Use fields above in C++. /// @{ bool getDiagonalDamping() const { return diagonalDamping; } double getlambdaFactor() const { return lambdaFactor; } double getlambdaInitial() const { return lambdaInitial; } double getlambdaLowerBound() const { return lambdaLowerBound; } double getlambdaUpperBound() const { return lambdaUpperBound; } + bool getUseFixedLambdaFactor() { return useFixedLambdaFactor; } std::string getLogFile() const { return logFile; } std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM);} + void setDiagonalDamping(bool flag) { diagonalDamping = flag; } void setlambdaFactor(double value) { lambdaFactor = value; } void setlambdaInitial(double value) { lambdaInitial = value; } void setlambdaLowerBound(double value) { lambdaLowerBound = value; } void setlambdaUpperBound(double value) { lambdaUpperBound = value; } - void setLogFile(const std::string& s) { logFile = s; } void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;} + void setLogFile(const std::string& s) { logFile = s; } void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);} // @} }; diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 88fc0f850..8e4678590 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -54,47 +54,24 @@ public: } virtual void print(const std::string& str = "") const; - size_t getMaxIterations() const { - return maxIterations; - } - double getRelativeErrorTol() const { - return relativeErrorTol; - } - double getAbsoluteErrorTol() const { - return absoluteErrorTol; - } - double getErrorTol() const { - return errorTol; - } - std::string getVerbosity() const { - return verbosityTranslator(verbosity); - } + size_t getMaxIterations() const { return maxIterations; } + double getRelativeErrorTol() const { return relativeErrorTol; } + double getAbsoluteErrorTol() const { return absoluteErrorTol; } + double getErrorTol() const { return errorTol; } + std::string getVerbosity() const { return verbosityTranslator(verbosity); } - void setMaxIterations(int value) { - maxIterations = value; - } - void setRelativeErrorTol(double value) { - relativeErrorTol = value; - } - void setAbsoluteErrorTol(double value) { - absoluteErrorTol = value; - } - void setErrorTol(double value) { - errorTol = value; - } - void setVerbosity(const std::string &src) { + void setMaxIterations(int value) { maxIterations = value; } + void setRelativeErrorTol(double value) { relativeErrorTol = value; } + void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; } + void setErrorTol(double value) { errorTol = value; } + void setVerbosity(const std::string& src) { verbosity = verbosityTranslator(src); } static Verbosity verbosityTranslator(const std::string &s) ; static std::string verbosityTranslator(Verbosity value) ; - // Successive Linearization Parameters - -public: - /** See NonlinearOptimizerParams::linearSolverType */ - enum LinearSolverType { MULTIFRONTAL_CHOLESKY, MULTIFRONTAL_QR, @@ -168,13 +145,9 @@ public: private: std::string linearSolverTranslator(LinearSolverType linearSolverType) const; - LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; - std::string orderingTypeTranslator(Ordering::OrderingType type) const; - Ordering::OrderingType orderingTypeTranslator(const std::string& type) const; - }; // For backward compatibility: From 5af2256277372cfd5c17016155604e3c45f998e8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 19 Mar 2019 10:30:30 -0400 Subject: [PATCH 53/61] Added missing clone for MATLAB wrapper --- gtsam/nonlinear/LevenbergMarquardtParams.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam/nonlinear/LevenbergMarquardtParams.h b/gtsam/nonlinear/LevenbergMarquardtParams.h index 491fbd233..4962ad366 100644 --- a/gtsam/nonlinear/LevenbergMarquardtParams.h +++ b/gtsam/nonlinear/LevenbergMarquardtParams.h @@ -142,6 +142,15 @@ public: void setLogFile(const std::string& s) { logFile = s; } void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);} // @} + /// @name Clone + /// @{ + + /// @return a deep copy of this object + boost::shared_ptr clone() const { + return boost::shared_ptr(new LevenbergMarquardtParams(*this)); + } + + /// @} }; } From 37eba50932df03dae90cc3c5c94f19275c16dbb8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 19 Mar 2019 15:47:01 -0400 Subject: [PATCH 54/61] Modernized, documented --- gtsam/slam/InitializePose3.cpp | 186 +++++++++-------------- gtsam/slam/InitializePose3.h | 61 +++++--- gtsam/slam/tests/testInitializePose3.cpp | 24 +-- 3 files changed, 129 insertions(+), 142 deletions(-) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 3979996da..b781f79f0 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -16,7 +16,7 @@ * @date August, 2014 */ -#include +#include #include #include #include @@ -29,13 +29,9 @@ using namespace std; namespace gtsam { -namespace InitializePose3 { +namespace initialize_pose3 { -static const Matrix I9 = I_9x9; -static const Vector zero9 = Vector::Zero(9); -static const Matrix zero33 = Z_3x3; - -static const Key keyAnchor = symbol('Z', 9999999); +static const Key kAnchorKey = symbol('Z', 9999999); /* ************************************************************************* */ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { @@ -43,26 +39,29 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { GaussianFactorGraph linearGraph; noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); - for(const boost::shared_ptr& factor: g) { + for(const auto& factor: g) { Matrix3 Rij; - boost::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); + auto pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between) Rij = pose3Between->measured().rotation().matrix(); else - std::cout << "Error in buildLinearOrientationGraph" << std::endl; + cout << "Error in buildLinearOrientationGraph" << endl; - const KeyVector& keys = factor->keys(); + const auto& keys = factor->keys(); Key key1 = keys[0], key2 = keys[1]; Matrix M9 = Z_9x9; M9.block(0,0,3,3) = Rij; M9.block(3,3,3,3) = Rij; M9.block(6,6,3,3) = Rij; - linearGraph.add(key1, -I9, key2, M9, zero9, model); + linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, model); } // prior on the anchor orientation - linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0).finished(), model); + linearGraph.add( + kAnchorKey, I_9x9, + (Vector(9) << 1.0, 0.0, 0.0, /* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0) + .finished(), + model); return linearGraph; } @@ -75,23 +74,15 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1; Values validRot3; - for(const VectorValues::value_type& it: relaxedRot3) { + for(const auto& it: relaxedRot3) { Key key = it.first; - if (key != keyAnchor) { - const Vector& rotVector = it.second; - Matrix3 rotMat; - rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2); - rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5); - rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8); + if (key != kAnchorKey) { + Matrix3 R; R << it.second; Matrix U, V; Vector s; - svd(rotMat, U, s, V); + svd(R.transpose(), U, s, V); Matrix3 normalizedRotMat = U * V.transpose(); - // std::cout << "rotMat \n" << rotMat << std::endl; - // std::cout << "U V' \n" << U * V.transpose() << std::endl; - // std::cout << "V \n" << V << std::endl; - if(normalizedRotMat.determinant() < 0) normalizedRotMat = U * ppm * V.transpose(); @@ -103,31 +94,27 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { } /* ************************************************************************* */ -// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { gttic(InitializePose3_buildPose3graph); NonlinearFactorGraph pose3Graph; - for(const boost::shared_ptr& factor: graph) { + for(const auto& factor: graph) { // recast to a between on Pose3 - boost::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); + const auto pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between) pose3Graph.add(pose3Between); // recast PriorFactor to BetweenFactor - boost::shared_ptr > pose3Prior = - boost::dynamic_pointer_cast >(factor); + const auto pose3Prior = boost::dynamic_pointer_cast >(factor); if (pose3Prior) - pose3Graph.emplace_shared >(keyAnchor, pose3Prior->keys()[0], + pose3Graph.emplace_shared >(kAnchorKey, pose3Prior->keys()[0], pose3Prior->prior(), pose3Prior->noiseModel()); } return pose3Graph; } /* ************************************************************************* */ -// Return the orientations of a graph including only BetweenFactors Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { gttic(InitializePose3_computeOrientationsChordal); @@ -142,14 +129,16 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { } /* ************************************************************************* */ -// Return the orientations of a graph including only BetweenFactors -Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter, const bool setRefFrame) { +Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, + const Values& givenGuess, + const size_t maxIter, + const bool setRefFrame) { gttic(InitializePose3_computeOrientationsGradient); // this works on the inverse rotations, according to Tron&Vidal,2011 Values inverseRot; - inverseRot.insert(keyAnchor, Rot3()); - for(const Values::ConstKeyValuePair& key_value: givenGuess) { + inverseRot.insert(kAnchorKey, Rot3()); + for(const auto& key_value: givenGuess) { Key key = key_value.key; const Pose3& pose = givenGuess.at(key); inverseRot.insert(key, pose.rotation().inverse()); @@ -164,9 +153,9 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const // calculate max node degree & allocate gradient size_t maxNodeDeg = 0; VectorValues grad; - for(const Values::ConstKeyValuePair& key_value: inverseRot) { + for(const auto& key_value: inverseRot) { Key key = key_value.key; - grad.insert(key,Vector3::Zero()); + grad.insert(key,Z_3x1); size_t currNodeDeg = (adjEdgesMap.at(key)).size(); if(currNodeDeg > maxNodeDeg) maxNodeDeg = currNodeDeg; @@ -180,42 +169,37 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const double mu_max = maxNodeDeg * rho; double stepsize = 2/mu_max; // = 1/(a b dG) - std::cout <<" b " << b <<" f0 " << f0 <<" a " << a <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; double maxGrad; // gradient iterations size_t it; - for(it=0; it < maxIter; it++){ + for (it = 0; it < maxIter; it++) { ////////////////////////////////////////////////////////////////////////// // compute the gradient at each node - //std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a - // <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; maxGrad = 0; - for(const Values::ConstKeyValuePair& key_value: inverseRot) { + for (const auto& key_value : inverseRot) { Key key = key_value.key; - //std::cout << "---------------------------key " << DefaultKeyFormatter(key) << std::endl; - Vector gradKey = Vector3::Zero(); + Vector gradKey = Z_3x1; // collect the gradient for each edge incident on key - for(const size_t& factorId: adjEdgesMap.at(key)){ + for (const size_t& factorId : adjEdgesMap.at(key)) { Rot3 Rij = factorId2RotMap.at(factorId); Rot3 Ri = inverseRot.at(key); - if( key == (pose3Graph.at(factorId))->keys()[0] ){ - Key key1 = (pose3Graph.at(factorId))->keys()[1]; + auto factor = pose3Graph.at(factorId); + const auto& keys = factor->keys(); + if (key == keys[0]) { + Key key1 = keys[1]; Rot3 Rj = inverseRot.at(key1); - gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b); - //std::cout << "key1 " << DefaultKeyFormatter(key1) << " gradientTron(Ri, Rij * Rj, a, b) \n " << gradientTron(Ri, Rij * Rj, a, b) << std::endl; - }else if( key == (pose3Graph.at(factorId))->keys()[1] ){ - Key key0 = (pose3Graph.at(factorId))->keys()[0]; + gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b); + } else if (key == keys[1]) { + Key key0 = keys[0]; Rot3 Rj = inverseRot.at(key0); gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b); - //std::cout << "key0 " << DefaultKeyFormatter(key0) << " gradientTron(Ri, Rij.inverse() * Rj, a, b) \n " << gradientTron(Ri, Rij.between(Rj), a, b) << std::endl; - }else{ - std::cout << "Error in gradient computation" << std::endl; + } else { + cout << "Error in gradient computation" << endl; } - } // end of i-th gradient computation - grad.at(key) = stepsize * gradKey; + } // end of i-th gradient computation + grad.at(key) = stepsize * gradKey; double normGradKey = (gradKey).norm(); - //std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl; if(normGradKey>maxGrad) maxGrad = normGradKey; } // end of loop over nodes @@ -230,14 +214,12 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const break; } // enf of gradient iterations - std::cout << "nr of gradient iterations " << it << "maxGrad " << maxGrad << std::endl; - // Return correct rotations - const Rot3& Rref = inverseRot.at(keyAnchor); // This will be set to the identity as so far we included no prior + const Rot3& Rref = inverseRot.at(kAnchorKey); // This will be set to the identity as so far we included no prior Values estimateRot; - for(const Values::ConstKeyValuePair& key_value: inverseRot) { + for(const auto& key_value: inverseRot) { Key key = key_value.key; - if (key != keyAnchor) { + if (key != kAnchorKey) { const Rot3& R = inverseRot.at(key); if(setRefFrame) estimateRot.insert(key, Rref.compose(R.inverse())); @@ -249,11 +231,11 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const } /* ************************************************************************* */ -void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){ +void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, + const NonlinearFactorGraph& pose3Graph) { size_t factorId = 0; - for(const boost::shared_ptr& factor: pose3Graph) { - boost::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); + for(const auto& factor: pose3Graph) { + auto pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between){ Rot3 Rij = pose3Between->measured().rotation(); factorId2RotMap.insert(pair(factorId,Rij)); @@ -275,7 +257,7 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, adjEdgesMap.insert(pair >(key2, edge_id)); } }else{ - std::cout << "Error in computeOrientationsGradient" << std::endl; + cout << "Error in createSymbolicGraph" << endl; } factorId++; } @@ -292,10 +274,10 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl th = logRot.norm(); } // exclude small or invalid rotations - if (th > 1e-5 && th == th){ // nonzero valid rotations + if (th > 1e-5 && th == th) { // nonzero valid rotations logRot = logRot / th; - }else{ - logRot = Vector3::Zero(); + } else { + logRot = Z_3x1; th = 0.0; } @@ -320,24 +302,25 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { // put into Values structure Values initialPose; - for(const Values::ConstKeyValuePair& key_value: initialRot){ + for (const auto& key_value : initialRot) { Key key = key_value.key; const Rot3& rot = initialRot.at(key); Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0)); initialPose.insert(key, initializedPose); } + // add prior noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); - initialPose.insert(keyAnchor, Pose3()); - pose3graph.emplace_shared >(keyAnchor, Pose3(), priorModel); + initialPose.insert(kAnchorKey, Pose3()); + pose3graph.emplace_shared >(kAnchorKey, Pose3(), priorModel); // Create optimizer GaussNewtonParams params; bool singleIter = true; - if(singleIter){ + if (singleIter) { params.maxIterations = 1; - }else{ - std::cout << " \n\n\n\n performing more than 1 GN iterations \n\n\n" <(key); estimate.insert(key, pose); } @@ -356,22 +339,9 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { } /* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph) { +Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, + bool useGradient) { gttic(InitializePose3_initialize); - - // We "extract" the Pose3 subgraph of the original graph: this - // is done to properly model priors and avoiding operating on a larger graph - NonlinearFactorGraph pose3Graph = buildPose3graph(graph); - - // Get orientations from relative orientation measurements - Values valueRot3 = computeOrientationsChordal(pose3Graph); - - // Compute the full poses (1 GN iteration on full poses) - return computePoses(pose3Graph, valueRot3); -} - -/* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) { Values initialValues; // We "extract" the Pose3 subgraph of the original graph: this @@ -380,27 +350,19 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b // Get orientations from relative orientation measurements Values orientations; - if(useGradient) + if (useGradient) orientations = computeOrientationsGradient(pose3Graph, givenGuess); else orientations = computeOrientationsChordal(pose3Graph); -// orientations.print("orientations\n"); - // Compute the full poses (1 GN iteration on full poses) return computePoses(pose3Graph, orientations); - - // for(const Values::ConstKeyValuePair& key_value: orientations) { - // Key key = key_value.key; - // if (key != keyAnchor) { - // const Point3& pos = givenGuess.at(key).translation(); - // const Rot3& rot = orientations.at(key); - // Pose3 initializedPoses = Pose3(rot, pos); - // initialValues.insert(key, initializedPoses); - // } - // } - // return initialValues; } -} // end of namespace lago -} // end of namespace gtsam +/* ************************************************************************* */ +Values initialize(const NonlinearFactorGraph& graph) { + return initialize(graph, Values(), false); +} + +} // namespace initialize_pose3 +} // namespace gtsam diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index dba5ceac3..d9bea7932 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -20,40 +20,65 @@ #pragma once -#include +#include +#include #include #include -#include -#include +#include namespace gtsam { typedef std::map > KeyVectorMap; -typedef std::map KeyRotMap; +typedef std::map KeyRotMap; -namespace InitializePose3 { +namespace initialize_pose3 { -GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g); +GTSAM_EXPORT GaussianFactorGraph +buildLinearOrientationGraph(const NonlinearFactorGraph& g); GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); -GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); +/** + * Return the orientations of a graph including only BetweenFactors + */ +GTSAM_EXPORT Values +computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); -GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, - const Values& givenGuess, size_t maxIter = 10000, const bool setRefFrame = true); +/** + * Return the orientations of a graph including only BetweenFactors + */ +GTSAM_EXPORT Values computeOrientationsGradient( + const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, + size_t maxIter = 10000, const bool setRefFrame = true); -GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, - const NonlinearFactorGraph& pose3Graph); +GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, + KeyRotMap& factorId2RotMap, + const NonlinearFactorGraph& pose3Graph); -GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b); +GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2, + const double a, const double b); -GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph); +/** + * Select the subgraph of betweenFactors and transforms priors into between wrt + * a fictitious node + */ +GTSAM_EXPORT NonlinearFactorGraph +buildPose3graph(const NonlinearFactorGraph& graph); -GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot); +GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, + Values& initialRot); +/** + * "extract" the Pose3 subgraph of the original graph, get orientations from + * relative orientation measurements (using either gradient or chordal method), + * and finish up with 1 GN iteration on full poses. + */ +GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, + const Values& givenGuess, + bool useGradient = false); + +/// Calls initialize above using Chordal method. GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph); -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient = false); - -} // end of namespace lago -} // end of namespace gtsam +} // namespace initialize_pose3 +} // end of namespace gtsam diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index e6f08286a..ea474c480 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -74,15 +74,15 @@ NonlinearFactorGraph graph() { /* *************************************************************************** */ TEST( InitializePose3, buildPose3graph ) { - NonlinearFactorGraph pose3graph = InitializePose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3graph = initialize_pose3::buildPose3graph(simple::graph()); // pose3graph.print(""); } /* *************************************************************************** */ TEST( InitializePose3, orientations ) { - NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); - Values initial = InitializePose3::computeOrientationsChordal(pose3Graph); + Values initial = initialize_pose3::computeOrientationsChordal(pose3Graph); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal(simple::R0, initial.at(x0), 1e-6)); @@ -93,12 +93,12 @@ TEST( InitializePose3, orientations ) { /* *************************************************************************** */ TEST( InitializePose3, orientationsGradientSymbolicGraph ) { - NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); KeyVectorMap adjEdgesMap; KeyRotMap factorId2RotMap; - InitializePose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph); + initialize_pose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph); EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[0], 0, 1e-9); EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9); @@ -132,7 +132,7 @@ TEST( InitializePose3, singleGradient ) { double a = 6.010534238540223; double b = 1.0; - Vector actual = InitializePose3::gradientTron(R1, R2, a, b); + Vector actual = initialize_pose3::gradientTron(R1, R2, a, b); Vector expected = Vector3::Zero(); expected(2) = 1.962658662803917; @@ -142,7 +142,7 @@ TEST( InitializePose3, singleGradient ) { /* *************************************************************************** */ TEST( InitializePose3, iterationGradient ) { - NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); // Wrong initial guess - initialization should fix the rotations Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); @@ -154,7 +154,7 @@ TEST( InitializePose3, iterationGradient ) { size_t maxIter = 1; // test gradient at the first iteration bool setRefFrame = false; - Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame); + Values orientations = initialize_pose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame); Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281, 0.033572116359134, 0.999436104312325, -0.000621610948719, @@ -183,7 +183,7 @@ TEST( InitializePose3, iterationGradient ) { /* *************************************************************************** */ TEST( InitializePose3, orientationsGradient ) { - NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); // Wrong initial guess - initialization should fix the rotations Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); @@ -194,7 +194,7 @@ TEST( InitializePose3, orientationsGradient ) { givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) )); // do 10 gradient iterations bool setRefFrame = false; - Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame); + Values orientations = initialize_pose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame); // const Key keyAnchor = symbol('Z', 9999999); // givenPoses.insert(keyAnchor,simple::pose0); @@ -228,7 +228,7 @@ TEST( InitializePose3, posesWithGivenGuess ) { givenPoses.insert(x2,simple::pose2); givenPoses.insert(x3,simple::pose3); - Values initial = InitializePose3::initialize(simple::graph(), givenPoses); + Values initial = initialize_pose3::initialize(simple::graph(), givenPoses); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal(givenPoses, initial, 1e-6)); @@ -245,7 +245,7 @@ TEST( InitializePose3, initializePoses ) noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); inputGraph->add(PriorFactor(0, Pose3(), priorModel)); - Values initial = InitializePose3::initialize(*inputGraph); + Values initial = initialize_pose3::initialize(*inputGraph); EXPECT(assert_equal(*expectedValues,initial,0.1)); // TODO(frank): very loose !! } From 3a371a1cf284139717204b5c0fad71a87247486d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 19 Mar 2019 15:47:51 -0400 Subject: [PATCH 55/61] Wrapped --- gtsam.h | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/gtsam.h b/gtsam.h index 9a06b7bd5..0aca81d02 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2523,6 +2523,23 @@ class BetweenFactorPose3s gtsam::BetweenFactorPose3* at(size_t i) const; }; +#include +namespace initialize_pose3 { +gtsam::Values computeOrientationsChordal( + const gtsam::NonlinearFactorGraph& pose3Graph); +gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); +gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess); +gtsam::NonlinearFactorGraph buildPose3graph( + const gtsam::NonlinearFactorGraph& graph); +gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& givenGuess, bool useGradient); +gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); +} // namespace initialize_pose3 + gtsam::BetweenFactorPose3s parse3DFactors(string filename); pair load3D(string filename); From e2cf42773a67abdb386c12de5ef710325a35ff66 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 19 Mar 2019 17:04:31 -0400 Subject: [PATCH 56/61] Switched to struct with static methods as apparently global methods in namespaces are not wrapped. --- gtsam.h | 33 +++++----- gtsam/slam/InitializePose3.cpp | 32 +++++---- gtsam/slam/InitializePose3.h | 83 ++++++++++++------------ gtsam/slam/tests/testInitializePose3.cpp | 24 +++---- 4 files changed, 88 insertions(+), 84 deletions(-) diff --git a/gtsam.h b/gtsam.h index 0aca81d02..014907037 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2524,21 +2524,24 @@ class BetweenFactorPose3s }; #include -namespace initialize_pose3 { -gtsam::Values computeOrientationsChordal( - const gtsam::NonlinearFactorGraph& pose3Graph); -gtsam::Values computeOrientationsGradient( - const gtsam::NonlinearFactorGraph& pose3Graph, - const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); -gtsam::Values computeOrientationsGradient( - const gtsam::NonlinearFactorGraph& pose3Graph, - const gtsam::Values& givenGuess); -gtsam::NonlinearFactorGraph buildPose3graph( - const gtsam::NonlinearFactorGraph& graph); -gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& givenGuess, bool useGradient); -gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); -} // namespace initialize_pose3 +class InitializePose3 { + static gtsam::Values computeOrientationsChordal( + const gtsam::NonlinearFactorGraph& pose3Graph); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess); + static gtsam::NonlinearFactorGraph buildPose3graph( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initializeOrientations( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& givenGuess, + bool useGradient); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); +}; gtsam::BetweenFactorPose3s parse3DFactors(string filename); pair load3D(string filename); diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index b781f79f0..667ccef0d 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -29,12 +29,11 @@ using namespace std; namespace gtsam { -namespace initialize_pose3 { static const Key kAnchorKey = symbol('Z', 9999999); /* ************************************************************************* */ -GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { +GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) { GaussianFactorGraph linearGraph; noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); @@ -67,7 +66,8 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { /* ************************************************************************* */ // Transform VectorValues into valid Rot3 -Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { +Values InitializePose3::normalizeRelaxedRotations( + const VectorValues& relaxedRot3) { gttic(InitializePose3_computeOrientationsChordal); Matrix ppm = Z_3x3; // plus plus minus @@ -94,7 +94,7 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { } /* ************************************************************************* */ -NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { +NonlinearFactorGraph InitializePose3::buildPose3graph(const NonlinearFactorGraph& graph) { gttic(InitializePose3_buildPose3graph); NonlinearFactorGraph pose3Graph; @@ -115,7 +115,8 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { } /* ************************************************************************* */ -Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { +Values InitializePose3::computeOrientationsChordal( + const NonlinearFactorGraph& pose3Graph) { gttic(InitializePose3_computeOrientationsChordal); // regularize measurements and plug everything in a factor graph @@ -129,10 +130,9 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { } /* ************************************************************************* */ -Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, - const Values& givenGuess, - const size_t maxIter, - const bool setRefFrame) { +Values InitializePose3::computeOrientationsGradient( + const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, + const size_t maxIter, const bool setRefFrame) { gttic(InitializePose3_computeOrientationsGradient); // this works on the inverse rotations, according to Tron&Vidal,2011 @@ -231,7 +231,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, } /* ************************************************************************* */ -void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, +void InitializePose3::createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph) { size_t factorId = 0; for(const auto& factor: pose3Graph) { @@ -264,7 +264,7 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, } /* ************************************************************************* */ -Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) { +Vector3 InitializePose3::gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) { Vector3 logRot = Rot3::Logmap(R1.between(R2)); double th = logRot.norm(); @@ -286,8 +286,7 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl } /* ************************************************************************* */ -Values initializeOrientations(const NonlinearFactorGraph& graph) { - +Values InitializePose3::initializeOrientations(const NonlinearFactorGraph& graph) { // We "extract" the Pose3 subgraph of the original graph: this // is done to properly model priors and avoiding operating on a larger graph NonlinearFactorGraph pose3Graph = buildPose3graph(graph); @@ -297,7 +296,7 @@ Values initializeOrientations(const NonlinearFactorGraph& graph) { } ///* ************************************************************************* */ -Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { +Values InitializePose3::computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { gttic(InitializePose3_computePoses); // put into Values structure @@ -339,7 +338,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { } /* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, +Values InitializePose3::initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) { gttic(InitializePose3_initialize); Values initialValues; @@ -360,9 +359,8 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, } /* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph) { +Values InitializePose3::initialize(const NonlinearFactorGraph& graph) { return initialize(graph, Values(), false); } -} // namespace initialize_pose3 } // namespace gtsam diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index d9bea7932..ce1b28854 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -31,54 +31,57 @@ namespace gtsam { typedef std::map > KeyVectorMap; typedef std::map KeyRotMap; -namespace initialize_pose3 { +struct GTSAM_EXPORT InitializePose3 { + static GaussianFactorGraph buildLinearOrientationGraph( + const NonlinearFactorGraph& g); -GTSAM_EXPORT GaussianFactorGraph -buildLinearOrientationGraph(const NonlinearFactorGraph& g); + static Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); -GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); + /** + * Return the orientations of a graph including only BetweenFactors + */ + static Values computeOrientationsChordal( + const NonlinearFactorGraph& pose3Graph); -/** - * Return the orientations of a graph including only BetweenFactors - */ -GTSAM_EXPORT Values -computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); + /** + * Return the orientations of a graph including only BetweenFactors + */ + static Values computeOrientationsGradient( + const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, + size_t maxIter = 10000, const bool setRefFrame = true); -/** - * Return the orientations of a graph including only BetweenFactors - */ -GTSAM_EXPORT Values computeOrientationsGradient( - const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, - size_t maxIter = 10000, const bool setRefFrame = true); + static void createSymbolicGraph(KeyVectorMap& adjEdgesMap, + KeyRotMap& factorId2RotMap, + const NonlinearFactorGraph& pose3Graph); -GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, - KeyRotMap& factorId2RotMap, - const NonlinearFactorGraph& pose3Graph); + static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, + const double b); -GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2, - const double a, const double b); + /** + * Select the subgraph of betweenFactors and transforms priors into between + * wrt a fictitious node + */ + static NonlinearFactorGraph buildPose3graph( + const NonlinearFactorGraph& graph); -/** - * Select the subgraph of betweenFactors and transforms priors into between wrt - * a fictitious node - */ -GTSAM_EXPORT NonlinearFactorGraph -buildPose3graph(const NonlinearFactorGraph& graph); + static Values computePoses(NonlinearFactorGraph& pose3graph, + Values& initialRot); -GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, - Values& initialRot); + /** + * "extract" the Pose3 subgraph of the original graph, get orientations from + * relative orientation measurements using chordal method. + */ + static Values initializeOrientations(const NonlinearFactorGraph& graph); -/** - * "extract" the Pose3 subgraph of the original graph, get orientations from - * relative orientation measurements (using either gradient or chordal method), - * and finish up with 1 GN iteration on full poses. - */ -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, - const Values& givenGuess, - bool useGradient = false); + /** + * "extract" the Pose3 subgraph of the original graph, get orientations from + * relative orientation measurements (using either gradient or chordal + * method), and finish up with 1 GN iteration on full poses. + */ + static Values initialize(const NonlinearFactorGraph& graph, + const Values& givenGuess, bool useGradient = false); -/// Calls initialize above using Chordal method. -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph); - -} // namespace initialize_pose3 + /// Calls initialize above using Chordal method. + static Values initialize(const NonlinearFactorGraph& graph); +}; } // end of namespace gtsam diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index ea474c480..e6f08286a 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -74,15 +74,15 @@ NonlinearFactorGraph graph() { /* *************************************************************************** */ TEST( InitializePose3, buildPose3graph ) { - NonlinearFactorGraph pose3graph = initialize_pose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3graph = InitializePose3::buildPose3graph(simple::graph()); // pose3graph.print(""); } /* *************************************************************************** */ TEST( InitializePose3, orientations ) { - NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); - Values initial = initialize_pose3::computeOrientationsChordal(pose3Graph); + Values initial = InitializePose3::computeOrientationsChordal(pose3Graph); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal(simple::R0, initial.at(x0), 1e-6)); @@ -93,12 +93,12 @@ TEST( InitializePose3, orientations ) { /* *************************************************************************** */ TEST( InitializePose3, orientationsGradientSymbolicGraph ) { - NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); KeyVectorMap adjEdgesMap; KeyRotMap factorId2RotMap; - initialize_pose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph); + InitializePose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph); EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[0], 0, 1e-9); EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9); @@ -132,7 +132,7 @@ TEST( InitializePose3, singleGradient ) { double a = 6.010534238540223; double b = 1.0; - Vector actual = initialize_pose3::gradientTron(R1, R2, a, b); + Vector actual = InitializePose3::gradientTron(R1, R2, a, b); Vector expected = Vector3::Zero(); expected(2) = 1.962658662803917; @@ -142,7 +142,7 @@ TEST( InitializePose3, singleGradient ) { /* *************************************************************************** */ TEST( InitializePose3, iterationGradient ) { - NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); // Wrong initial guess - initialization should fix the rotations Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); @@ -154,7 +154,7 @@ TEST( InitializePose3, iterationGradient ) { size_t maxIter = 1; // test gradient at the first iteration bool setRefFrame = false; - Values orientations = initialize_pose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame); + Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame); Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281, 0.033572116359134, 0.999436104312325, -0.000621610948719, @@ -183,7 +183,7 @@ TEST( InitializePose3, iterationGradient ) { /* *************************************************************************** */ TEST( InitializePose3, orientationsGradient ) { - NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); // Wrong initial guess - initialization should fix the rotations Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); @@ -194,7 +194,7 @@ TEST( InitializePose3, orientationsGradient ) { givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) )); // do 10 gradient iterations bool setRefFrame = false; - Values orientations = initialize_pose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame); + Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame); // const Key keyAnchor = symbol('Z', 9999999); // givenPoses.insert(keyAnchor,simple::pose0); @@ -228,7 +228,7 @@ TEST( InitializePose3, posesWithGivenGuess ) { givenPoses.insert(x2,simple::pose2); givenPoses.insert(x3,simple::pose3); - Values initial = initialize_pose3::initialize(simple::graph(), givenPoses); + Values initial = InitializePose3::initialize(simple::graph(), givenPoses); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal(givenPoses, initial, 1e-6)); @@ -245,7 +245,7 @@ TEST( InitializePose3, initializePoses ) noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); inputGraph->add(PriorFactor(0, Pose3(), priorModel)); - Values initial = initialize_pose3::initialize(*inputGraph); + Values initial = InitializePose3::initialize(*inputGraph); EXPECT(assert_equal(*expectedValues,initial,0.1)); // TODO(frank): very loose !! } From a89e422a8ac4c49a5b476f10c65da7e7b8ab7b29 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 19 Mar 2019 17:04:48 -0400 Subject: [PATCH 57/61] Added python example and test --- ...Pose3SLAMExample_initializePose3Chordal.py | 35 ++++++++ cython/gtsam/tests/test_initialize_pose3.py | 88 +++++++++++++++++++ 2 files changed, 123 insertions(+) create mode 100644 cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py create mode 100644 cython/gtsam/tests/test_initialize_pose3.py diff --git a/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py b/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py new file mode 100644 index 000000000..02c696905 --- /dev/null +++ b/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py @@ -0,0 +1,35 @@ +""" +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 + +Initialize PoseSLAM with Chordal init +Author: Luca Carlone, Frank Dellaert (python port) +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import numpy as np + +import gtsam + +# Read graph from file +g2oFile = gtsam.findExampleDataFile("pose3example.txt") + +is3D = True +graph, initial = gtsam.readG2o(g2oFile, is3D) + +# Add prior on the first key. TODO: assumes first key ios z +priorModel = gtsam.noiseModel_Diagonal.Variances( + np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) +firstKey = initial.keys().at(0) +graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) + +# Initializing Pose3 - chordal relaxation" +initialization = gtsam.InitializePose3.initialize(graph) + +print(initialization) diff --git a/cython/gtsam/tests/test_initialize_pose3.py b/cython/gtsam/tests/test_initialize_pose3.py new file mode 100644 index 000000000..ee9195c48 --- /dev/null +++ b/cython/gtsam/tests/test_initialize_pose3.py @@ -0,0 +1,88 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for 3D SLAM initialization, using rotation relaxation. +Author: Luca Carlone and Frank Dellaert (Python) +""" +# pylint: disable=invalid-name, E1101, E0611 +import unittest + +import numpy as np + +import gtsam +from gtsam import NonlinearFactorGraph, Point3, Pose3, Rot3, Values + +x0, x1, x2, x3 = 0, 1, 2, 3 + + +class TestValues(unittest.TestCase): + + def setUp(self): + + model = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) + + # We consider a small graph: + # symbolic FG + # x2 0 1 + # / | \ 1 2 + # / | \ 2 3 + # x3 | x1 2 0 + # \ | / 0 3 + # \ | / + # x0 + # + p0 = Point3(0, 0, 0) + self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0])) + p1 = Point3(1, 2, 0) + self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796])) + p2 = Point3(0, 2, 0) + self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593])) + p3 = Point3(-1, 1, 0) + self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389])) + + pose0 = Pose3(self.R0, p0) + pose1 = Pose3(self.R1, p1) + pose2 = Pose3(self.R2, p2) + pose3 = Pose3(self.R3, p3) + + g = NonlinearFactorGraph() + g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model)) + g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model)) + g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model)) + g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model)) + g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model)) + g.add(gtsam.PriorFactorPose3(x0, pose0, model)) + self.graph = g + + def test_buildPose3graph(self): + pose3graph = gtsam.InitializePose3.buildPose3graph(self.graph) + + def test_orientations(self): + pose3Graph = gtsam.InitializePose3.buildPose3graph(self.graph) + + initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph) + + # comparison is up to M_PI, that's why we add some multiples of 2*M_PI + self.assertTrue(initial.atRot3(x0).equals(self.R0, 1e-6)) + self.assertTrue(initial.atRot3(x1).equals(self.R1, 1e-6)) + self.assertTrue(initial.atRot3(x2).equals(self.R2, 1e-6)) + self.assertTrue(initial.atRot3(x3).equals(self.R3, 1e-6)) + + def test_initializePoses(self): + g2oFile = gtsam.findExampleDataFile("pose3example-grid") + is3D = True + inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D) + priorModel = gtsam.noiseModel_Unit.Create(6) + inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel)) + + initial = gtsam.InitializePose3.initialize(inputGraph) + # TODO(frank): very loose !! + self.assertTrue(initial.equals(expectedValues, 0.1)) + + +if __name__ == "__main__": + unittest.main() From 1e944fb86a5420d7e9d60185a0d28a7725654998 Mon Sep 17 00:00:00 2001 From: lucacarlone Date: Tue, 19 Mar 2019 18:44:09 -0400 Subject: [PATCH 58/61] now initialization is aware of rotation noise model --- gtsam/slam/InitializePose3.cpp | 15 ++++++++++----- gtsam/slam/tests/testInitializePose3.cpp | 24 ++++++++++++++++++++++++ 2 files changed, 34 insertions(+), 5 deletions(-) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 667ccef0d..2f247811d 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -36,16 +36,21 @@ static const Key kAnchorKey = symbol('Z', 9999999); GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) { GaussianFactorGraph linearGraph; - noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); for(const auto& factor: g) { Matrix3 Rij; + double rotationPrecision = 1.0; auto pose3Between = boost::dynamic_pointer_cast >(factor); - if (pose3Between) + if (pose3Between){ Rij = pose3Between->measured().rotation().matrix(); - else + Vector precisions = Vector::Zero(6); + precisions[0] = 1.0; // vector of all zeros except first entry equal to 1 + pose3Between->noiseModel()->whitenInPlace(precisions); // gets marginal precision of first variable + rotationPrecision = precisions[0]; // rotations first + }else{ cout << "Error in buildLinearOrientationGraph" << endl; + } const auto& keys = factor->keys(); Key key1 = keys[0], key2 = keys[1]; @@ -53,14 +58,14 @@ GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const Nonlinear M9.block(0,0,3,3) = Rij; M9.block(3,3,3,3) = Rij; M9.block(6,6,3,3) = Rij; - linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, model); + linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, noiseModel::Isotropic::Precision(9, rotationPrecision)); } // prior on the anchor orientation linearGraph.add( kAnchorKey, I_9x9, (Vector(9) << 1.0, 0.0, 0.0, /* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0) .finished(), - model); + noiseModel::Isotropic::Precision(9, 1)); return linearGraph; } diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index e6f08286a..6fe8b3d7c 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -70,6 +70,17 @@ NonlinearFactorGraph graph() { g.add(PriorFactor(x0, pose0, model)); return g; } + +NonlinearFactorGraph graph2() { + NonlinearFactorGraph g; + g.add(BetweenFactor(x0, x1, pose0.between(pose1), noiseModel::Isotropic::Precision(6, 1.0))); + g.add(BetweenFactor(x1, x2, pose1.between(pose2), noiseModel::Isotropic::Precision(6, 1.0))); + g.add(BetweenFactor(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0))); + g.add(BetweenFactor(x2, x0, Pose3(Rot3::Ypr(0.1,0,0.1), Point3()), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero information + g.add(BetweenFactor(x0, x3, Pose3(Rot3::Ypr(0.5,-0.2,0.2), Point3(10,20,30)), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero informatoin + g.add(PriorFactor(x0, pose0, model)); + return g; +} } /* *************************************************************************** */ @@ -91,6 +102,19 @@ TEST( InitializePose3, orientations ) { EXPECT(assert_equal(simple::R3, initial.at(x3), 1e-6)); } +/* *************************************************************************** */ +TEST( InitializePose3, orientationsPrecisions ) { + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph2()); + + Values initial = InitializePose3::computeOrientationsChordal(pose3Graph); + + // comparison is up to M_PI, that's why we add some multiples of 2*M_PI + EXPECT(assert_equal(simple::R0, initial.at(x0), 1e-6)); + EXPECT(assert_equal(simple::R1, initial.at(x1), 1e-6)); + EXPECT(assert_equal(simple::R2, initial.at(x2), 1e-6)); + EXPECT(assert_equal(simple::R3, initial.at(x3), 1e-6)); +} + /* *************************************************************************** */ TEST( InitializePose3, orientationsGradientSymbolicGraph ) { NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); From 90e6eb95cf0372d95d166d0384bd77650cc36e43 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 20 Mar 2019 17:35:31 -0400 Subject: [PATCH 59/61] Added GtsamTestCase --- cython/gtsam/utils/test_case.py | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 cython/gtsam/utils/test_case.py diff --git a/cython/gtsam/utils/test_case.py b/cython/gtsam/utils/test_case.py new file mode 100644 index 000000000..7df1e6ee9 --- /dev/null +++ b/cython/gtsam/utils/test_case.py @@ -0,0 +1,27 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +TestCase class with GTSAM assert utils. +Author: Frank Dellaert +""" +import unittest + + +class GtsamTestCase(unittest.TestCase): + """TestCase class with GTSAM assert utils.""" + + def gtsamAssertEquals(self, actual, expected, tol=1e-9): + """ AssertEqual function that prints out actual and expected if not equal. + Usage: + self.gtsamAssertEqual(actual,expected) + Keyword Arguments: + tol {float} -- tolerance passed to 'equals', default 1e-9 + """ + equal = actual.equals(expected, tol) + if not equal: + raise self.failureException( + "Values are not equal:\n{}!={}".format(actual, expected)) From c442df38664a719ed7c5a482a82e8bdb3ee1acf8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 20 Mar 2019 17:35:53 -0400 Subject: [PATCH 60/61] Modernized all tests --- cython/gtsam/tests/test_Cal3Unified.py | 26 +++++++++++--- cython/gtsam/tests/test_JacobianFactor.py | 22 +++++++++--- cython/gtsam/tests/test_KalmanFilter.py | 18 ++++++++-- .../gtsam/tests/test_LocalizationExample.py | 20 +++++++++-- cython/gtsam/tests/test_NonlinearOptimizer.py | 3 +- cython/gtsam/tests/test_OdometryExample.py | 20 +++++++++-- cython/gtsam/tests/test_PlanarSLAMExample.py | 22 +++++++++--- cython/gtsam/tests/test_Pose2.py | 15 ++++++-- cython/gtsam/tests/test_Pose2SLAMExample.py | 20 +++++++++-- cython/gtsam/tests/test_Pose3.py | 19 +++++++--- cython/gtsam/tests/test_Pose3SLAMExample.py | 28 +++++++++++---- cython/gtsam/tests/test_PriorFactor.py | 18 ++++++++-- cython/gtsam/tests/test_SFMExample.py | 23 +++++++++--- cython/gtsam/tests/test_Scenario.py | 13 +++---- cython/gtsam/tests/test_SimpleCamera.py | 25 +++++++++---- cython/gtsam/tests/test_StereoVOExample.py | 23 +++++++++--- cython/gtsam/tests/test_Values.py | 36 ++++++++++--------- cython/gtsam/tests/test_VisualISAMExample.py | 23 +++++++++--- cython/gtsam/tests/test_dataset.py | 5 +-- cython/gtsam/tests/test_initialize_pose3.py | 13 +++---- .../tests/test_FixedLagSmootherExample.py | 17 +++++++-- 21 files changed, 316 insertions(+), 93 deletions(-) diff --git a/cython/gtsam/tests/test_Cal3Unified.py b/cython/gtsam/tests/test_Cal3Unified.py index 3225d2ff9..fbf5f3565 100644 --- a/cython/gtsam/tests/test_Cal3Unified.py +++ b/cython/gtsam/tests/test_Cal3Unified.py @@ -1,9 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Cal3Unified unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np +import gtsam +from gtsam.utils.test_case import GtsamTestCase -class TestCal3Unified(unittest.TestCase): + +class TestCal3Unified(GtsamTestCase): def test_Cal3Unified(self): K = gtsam.Cal3Unified() @@ -11,12 +24,15 @@ class TestCal3Unified(unittest.TestCase): self.assertEqual(K.fx(), 1.) def test_retract(self): - expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) - K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1) + expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) + K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240, + 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1) d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10, 1], order='F') actual = K.retract(d) - self.assertTrue(actual.equals(expected, 1e-9)) + self.gtsamAssertEquals(actual, expected) np.testing.assert_allclose(d, K.localCoordinates(actual)) + if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_JacobianFactor.py b/cython/gtsam/tests/test_JacobianFactor.py index bf63c839b..04433492b 100644 --- a/cython/gtsam/tests/test_JacobianFactor.py +++ b/cython/gtsam/tests/test_JacobianFactor.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +JacobianFactor unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestJacobianFactor(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestJacobianFactor(GtsamTestCase): def test_eliminate(self): # Recommended way to specify a matrix (see cython/README) @@ -54,7 +68,7 @@ class TestJacobianFactor(unittest.TestCase): expectedCG = gtsam.GaussianConditional( x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel_Unit.Create(2)) # check if the result matches - self.assertTrue(actualCG.equals(expectedCG, 1e-4)) + self.gtsamAssertEquals(actualCG, expectedCG, 1e-4) # the expected linear factor Bl1 = np.array([[4.47214, 0.00], @@ -72,7 +86,7 @@ class TestJacobianFactor(unittest.TestCase): expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2) # check if the result matches the combined (reduced) factor - self.assertTrue(lf.equals(expectedLF, 1e-4)) + self.gtsamAssertEquals(lf, expectedLF, 1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_KalmanFilter.py b/cython/gtsam/tests/test_KalmanFilter.py index 56f9e2573..94c41df72 100644 --- a/cython/gtsam/tests/test_KalmanFilter.py +++ b/cython/gtsam/tests/test_KalmanFilter.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +KalmanFilter unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestKalmanFilter(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestKalmanFilter(GtsamTestCase): def test_KalmanFilter(self): F = np.eye(2) diff --git a/cython/gtsam/tests/test_LocalizationExample.py b/cython/gtsam/tests/test_LocalizationExample.py index c373f162c..6ce65f087 100644 --- a/cython/gtsam/tests/test_LocalizationExample.py +++ b/cython/gtsam/tests/test_LocalizationExample.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Localization unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestLocalizationExample(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestLocalizationExample(GtsamTestCase): def test_LocalizationExample(self): # Create the graph (defined in pose2SLAM.h, derived from @@ -43,7 +57,7 @@ class TestLocalizationExample(unittest.TestCase): P = [None] * result.size() for i in range(0, result.size()): pose_i = result.atPose2(i) - self.assertTrue(pose_i.equals(groundTruth.atPose2(i), 1e-4)) + self.gtsamAssertEquals(pose_i, groundTruth.atPose2(i), 1e-4) P[i] = marginals.marginalCovariance(i) if __name__ == "__main__": diff --git a/cython/gtsam/tests/test_NonlinearOptimizer.py b/cython/gtsam/tests/test_NonlinearOptimizer.py index 0d0318f40..efefb218a 100644 --- a/cython/gtsam/tests/test_NonlinearOptimizer.py +++ b/cython/gtsam/tests/test_NonlinearOptimizer.py @@ -19,12 +19,13 @@ from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer, GaussNewtonParams, LevenbergMarquardtOptimizer, LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, Point2, PriorFactorPoint2, Values) +from gtsam.utils.test_case import GtsamTestCase KEY1 = 1 KEY2 = 2 -class TestScenario(unittest.TestCase): +class TestScenario(GtsamTestCase): def test_optimize(self): """Do trivial test with three optimizer variants.""" fg = NonlinearFactorGraph() diff --git a/cython/gtsam/tests/test_OdometryExample.py b/cython/gtsam/tests/test_OdometryExample.py index 1100e8334..c8ea95588 100644 --- a/cython/gtsam/tests/test_OdometryExample.py +++ b/cython/gtsam/tests/test_OdometryExample.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Odometry unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestOdometryExample(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestOdometryExample(GtsamTestCase): def test_OdometryExample(self): # Create the graph (defined in pose2SLAM.h, derived from @@ -39,7 +53,7 @@ class TestOdometryExample(unittest.TestCase): # Check first pose equality pose_1 = result.atPose2(1) - self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4)) + self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_PlanarSLAMExample.py b/cython/gtsam/tests/test_PlanarSLAMExample.py index 046a93f35..ae813d35c 100644 --- a/cython/gtsam/tests/test_PlanarSLAMExample.py +++ b/cython/gtsam/tests/test_PlanarSLAMExample.py @@ -1,11 +1,25 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +PlanarSLAM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam from math import pi + import numpy as np -class TestPose2SLAMExample(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase - def test_Pose2SLAMExample(self): + +class TestPlanarSLAM(GtsamTestCase): + + def test_PlanarSLAM(self): # Assumptions # - All values are axis aligned # - Robot poses are facing along the X axis (horizontal, to the right in images) @@ -56,7 +70,7 @@ class TestPose2SLAMExample(unittest.TestCase): P = marginals.marginalCovariance(1) pose_1 = result.atPose2(1) - self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4)) + self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4) diff --git a/cython/gtsam/tests/test_Pose2.py b/cython/gtsam/tests/test_Pose2.py index afd0c5773..9652b594a 100644 --- a/cython/gtsam/tests/test_Pose2.py +++ b/cython/gtsam/tests/test_Pose2.py @@ -1,12 +1,23 @@ -"""Pose2 unit tests.""" +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Pose2 unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest import numpy as np +import gtsam from gtsam import Pose2 +from gtsam.utils.test_case import GtsamTestCase -class TestPose2(unittest.TestCase): +class TestPose2(GtsamTestCase): """Test selected Pose2 methods.""" def test_adjoint(self): diff --git a/cython/gtsam/tests/test_Pose2SLAMExample.py b/cython/gtsam/tests/test_Pose2SLAMExample.py index bcaa7be4f..a79b6b18c 100644 --- a/cython/gtsam/tests/test_Pose2SLAMExample.py +++ b/cython/gtsam/tests/test_Pose2SLAMExample.py @@ -1,9 +1,23 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Pose2SLAM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam from math import pi + import numpy as np -class TestPose2SLAMExample(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestPose2SLAMExample(GtsamTestCase): def test_Pose2SLAMExample(self): # Assumptions @@ -56,7 +70,7 @@ class TestPose2SLAMExample(unittest.TestCase): P = marginals.marginalCovariance(1) pose_1 = result.atPose2(1) - self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4)) + self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py index b878a9551..3eb4067c9 100644 --- a/cython/gtsam/tests/test_Pose3.py +++ b/cython/gtsam/tests/test_Pose3.py @@ -1,13 +1,24 @@ -"""Pose3 unit tests.""" +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Pose3 unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import math import unittest import numpy as np +import gtsam from gtsam import Point3, Pose3, Rot3 +from gtsam.utils.test_case import GtsamTestCase -class TestPose3(unittest.TestCase): +class TestPose3(GtsamTestCase): """Test selected Pose3 methods.""" def test_between(self): @@ -16,14 +27,14 @@ class TestPose3(unittest.TestCase): T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3)) expected = T2.inverse().compose(T3) actual = T2.between(T3) - self.assertTrue(actual.equals(expected, 1e-6)) + self.gtsamAssertEquals(actual, expected, 1e-6) def test_transform_to(self): """Test transform_to method.""" transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0)) actual = transform.transform_to(Point3(3, 2, 10)) expected = Point3(2, 1, 10) - self.assertTrue(actual.equals(expected, 1e-6)) + self.gtsamAssertEquals(actual, expected, 1e-6) def test_range(self): """Test range method.""" diff --git a/cython/gtsam/tests/test_Pose3SLAMExample.py b/cython/gtsam/tests/test_Pose3SLAMExample.py index e33db2145..1e9eaac67 100644 --- a/cython/gtsam/tests/test_Pose3SLAMExample.py +++ b/cython/gtsam/tests/test_Pose3SLAMExample.py @@ -1,10 +1,24 @@ -import unittest -import numpy as np -import gtsam -from math import pi -from gtsam.utils.circlePose3 import * +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved -class TestPose3SLAMExample(unittest.TestCase): +See LICENSE for the license information + +PoseSLAM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest +from math import pi + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase +from gtsam.utils.circlePose3 import * + + +class TestPose3SLAMExample(GtsamTestCase): def test_Pose3SLAMExample(self): # Create a hexagon of poses @@ -40,7 +54,7 @@ class TestPose3SLAMExample(unittest.TestCase): result = optimizer.optimizeSafely() pose_1 = result.atPose3(1) - self.assertTrue(pose_1.equals(p1, 1e-4)) + self.gtsamAssertEquals(pose_1, p1, 1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_PriorFactor.py b/cython/gtsam/tests/test_PriorFactor.py index 95ec2ae94..0acf50c2c 100644 --- a/cython/gtsam/tests/test_PriorFactor.py +++ b/cython/gtsam/tests/test_PriorFactor.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +PriorFactor unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestPriorFactor(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestPriorFactor(GtsamTestCase): def test_PriorFactor(self): values = gtsam.Values() diff --git a/cython/gtsam/tests/test_SFMExample.py b/cython/gtsam/tests/test_SFMExample.py index 606b26a43..e8fa46186 100644 --- a/cython/gtsam/tests/test_SFMExample.py +++ b/cython/gtsam/tests/test_SFMExample.py @@ -1,11 +1,24 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +SFM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam -from gtsam import symbol + import numpy as np + +import gtsam import gtsam.utils.visual_data_generator as generator +from gtsam import symbol +from gtsam.utils.test_case import GtsamTestCase -class TestSFMExample(unittest.TestCase): +class TestSFMExample(GtsamTestCase): def test_SFMExample(self): options = generator.Options() @@ -59,11 +72,11 @@ class TestSFMExample(unittest.TestCase): # Check optimized results, should be equal to ground truth for i in range(len(truth.cameras)): pose_i = result.atPose3(symbol(ord('x'), i)) - self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5)) + self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) for j in range(len(truth.points)): point_j = result.atPoint3(symbol(ord('p'), j)) - self.assertTrue(point_j.equals(truth.points[j], 1e-5)) + self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_Scenario.py b/cython/gtsam/tests/test_Scenario.py index d68566b25..09601fba5 100644 --- a/cython/gtsam/tests/test_Scenario.py +++ b/cython/gtsam/tests/test_Scenario.py @@ -5,11 +5,9 @@ All Rights Reserved See LICENSE for the license information -Unit tests for IMU testing scenarios. +Scenario unit tests. Author: Frank Dellaert & Duy Nguyen Ta (Python) """ -# pylint: disable=invalid-name, E1101 - from __future__ import print_function import math @@ -18,9 +16,12 @@ import unittest import numpy as np import gtsam +from gtsam.utils.test_case import GtsamTestCase + +# pylint: disable=invalid-name, E1101 -class TestScenario(unittest.TestCase): +class TestScenario(GtsamTestCase): def setUp(self): pass @@ -44,8 +45,8 @@ class TestScenario(unittest.TestCase): T30 = scenario.pose(T) np.testing.assert_almost_equal( np.array([math.pi, 0, math.pi]), T30.rotation().xyz()) - self.assertTrue(gtsam.Point3( - 0, 0, 2.0 * R).equals(T30.translation(), 1e-9)) + self.gtsamAssertEquals(gtsam.Point3( + 0, 0, 2.0 * R), T30.translation(), 1e-9) if __name__ == '__main__': diff --git a/cython/gtsam/tests/test_SimpleCamera.py b/cython/gtsam/tests/test_SimpleCamera.py index 7924a9b1c..efdfec561 100644 --- a/cython/gtsam/tests/test_SimpleCamera.py +++ b/cython/gtsam/tests/test_SimpleCamera.py @@ -1,18 +1,31 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +SimpleCamera unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import math -import numpy as np import unittest -from gtsam import Pose2, Point3, Rot3, Pose3, Cal3_S2, SimpleCamera +import numpy as np + +import gtsam +from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera +from gtsam.utils.test_case import GtsamTestCase K = Cal3_S2(625, 625, 0, 0, 0) -class TestSimpleCamera(unittest.TestCase): +class TestSimpleCamera(GtsamTestCase): def test_constructor(self): pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5)) camera = SimpleCamera(pose1, K) - self.assertTrue(camera.calibration().equals(K, 1e-9)) - self.assertTrue(camera.pose().equals(pose1, 1e-9)) + self.gtsamAssertEquals(camera.calibration(), K, 1e-9) + self.gtsamAssertEquals(camera.pose(), pose1, 1e-9) def test_level2(self): # Create a level camera, looking in Y-direction @@ -25,7 +38,7 @@ class TestSimpleCamera(unittest.TestCase): z = Point3(0,1,0) wRc = Rot3(x,y,z) expected = Pose3(wRc,Point3(0.4,0.3,0.1)) - self.assertTrue(camera.pose().equals(expected, 1e-9)) + self.gtsamAssertEquals(camera.pose(), expected, 1e-9) if __name__ == "__main__": diff --git a/cython/gtsam/tests/test_StereoVOExample.py b/cython/gtsam/tests/test_StereoVOExample.py index dacd4a116..3f5f57522 100644 --- a/cython/gtsam/tests/test_StereoVOExample.py +++ b/cython/gtsam/tests/test_StereoVOExample.py @@ -1,10 +1,23 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Stereo VO unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam -from gtsam import symbol + import numpy as np +import gtsam +from gtsam import symbol +from gtsam.utils.test_case import GtsamTestCase -class TestStereoVOExample(unittest.TestCase): + +class TestStereoVOExample(GtsamTestCase): def test_StereoVOExample(self): ## Assumptions @@ -60,10 +73,10 @@ class TestStereoVOExample(unittest.TestCase): ## check equality for the first pose and point pose_x1 = result.atPose3(x1) - self.assertTrue(pose_x1.equals(first_pose,1e-4)) + self.gtsamAssertEquals(pose_x1, first_pose,1e-4) point_l1 = result.atPoint3(l1) - self.assertTrue(point_l1.equals(expected_l1,1e-4)) + self.gtsamAssertEquals(point_l1, expected_l1,1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_Values.py b/cython/gtsam/tests/test_Values.py index 0bb1e0806..20634a21c 100644 --- a/cython/gtsam/tests/test_Values.py +++ b/cython/gtsam/tests/test_Values.py @@ -5,7 +5,7 @@ All Rights Reserved See LICENSE for the license information -Unit tests for IMU testing scenarios. +Values unit tests. Author: Frank Dellaert & Duy Nguyen Ta (Python) """ # pylint: disable=invalid-name, E1101, E0611 @@ -13,12 +13,14 @@ import unittest import numpy as np +import gtsam from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2, Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values, imuBias_ConstantBias) +from gtsam.utils.test_case import GtsamTestCase -class TestValues(unittest.TestCase): +class TestValues(GtsamTestCase): def test_values(self): values = Values() @@ -58,26 +60,26 @@ class TestValues(unittest.TestCase): mat2 = np.array([[1, 2, ], [3, 5]]) values.insert(13, mat2) - self.assertTrue(values.atPoint2(0).equals(Point2(0,0), tol)) - self.assertTrue(values.atPoint3(1).equals(Point3(0,0,0), tol)) - self.assertTrue(values.atRot2(2).equals(Rot2(), tol)) - self.assertTrue(values.atPose2(3).equals(Pose2(), tol)) - self.assertTrue(values.atRot3(4).equals(Rot3(), tol)) - self.assertTrue(values.atPose3(5).equals(Pose3(), tol)) - self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol)) - self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol)) - self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol)) - self.assertTrue(values.atEssentialMatrix(9).equals(E, tol)) - self.assertTrue(values.atimuBias_ConstantBias( - 10).equals(imuBias_ConstantBias(), tol)) + self.gtsamAssertEquals(values.atPoint2(0), Point2(0,0), tol) + self.gtsamAssertEquals(values.atPoint3(1), Point3(0,0,0), tol) + self.gtsamAssertEquals(values.atRot2(2), Rot2(), tol) + self.gtsamAssertEquals(values.atPose2(3), Pose2(), tol) + self.gtsamAssertEquals(values.atRot3(4), Rot3(), tol) + self.gtsamAssertEquals(values.atPose3(5), Pose3(), tol) + self.gtsamAssertEquals(values.atCal3_S2(6), Cal3_S2(), tol) + self.gtsamAssertEquals(values.atCal3DS2(7), Cal3DS2(), tol) + self.gtsamAssertEquals(values.atCal3Bundler(8), Cal3Bundler(), tol) + self.gtsamAssertEquals(values.atEssentialMatrix(9), E, tol) + self.gtsamAssertEquals(values.atimuBias_ConstantBias( + 10), imuBias_ConstantBias(), tol) # special cases for Vector and Matrix: actualVector = values.atVector(11) - self.assertTrue(np.allclose(vec, actualVector, tol)) + np.testing.assert_allclose(vec, actualVector, tol) actualMatrix = values.atMatrix(12) - self.assertTrue(np.allclose(mat, actualMatrix, tol)) + np.testing.assert_allclose(mat, actualMatrix, tol) actualMatrix2 = values.atMatrix(13) - self.assertTrue(np.allclose(mat2, actualMatrix2, tol)) + np.testing.assert_allclose(mat2, actualMatrix2, tol) if __name__ == "__main__": diff --git a/cython/gtsam/tests/test_VisualISAMExample.py b/cython/gtsam/tests/test_VisualISAMExample.py index 39bfa6eb4..99d7e6160 100644 --- a/cython/gtsam/tests/test_VisualISAMExample.py +++ b/cython/gtsam/tests/test_VisualISAMExample.py @@ -1,10 +1,25 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +visual_isam unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest + import numpy as np -from gtsam import symbol + +import gtsam import gtsam.utils.visual_data_generator as generator import gtsam.utils.visual_isam as visual_isam +from gtsam import symbol +from gtsam.utils.test_case import GtsamTestCase -class TestVisualISAMExample(unittest.TestCase): + +class TestVisualISAMExample(GtsamTestCase): def test_VisualISAMExample(self): # Data Options @@ -32,11 +47,11 @@ class TestVisualISAMExample(unittest.TestCase): for i in range(len(truth.cameras)): pose_i = result.atPose3(symbol(ord('x'), i)) - self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5)) + self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) for j in range(len(truth.points)): point_j = result.atPoint3(symbol(ord('l'), j)) - self.assertTrue(point_j.equals(truth.points[j], 1e-5)) + self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_dataset.py b/cython/gtsam/tests/test_dataset.py index e561be1a8..60fb9450d 100644 --- a/cython/gtsam/tests/test_dataset.py +++ b/cython/gtsam/tests/test_dataset.py @@ -6,7 +6,7 @@ All Rights Reserved See LICENSE for the license information Unit tests for testing dataset access. -Author: Frank Dellaert +Author: Frank Dellaert & Duy Nguyen Ta (Python) """ # pylint: disable=invalid-name, no-name-in-module, no-member @@ -16,9 +16,10 @@ import unittest import gtsam from gtsam import BetweenFactorPose3, BetweenFactorPose3s +from gtsam.utils.test_case import GtsamTestCase -class TestDataset(unittest.TestCase): +class TestDataset(GtsamTestCase): """Tests for datasets.h wrapper.""" def setUp(self): diff --git a/cython/gtsam/tests/test_initialize_pose3.py b/cython/gtsam/tests/test_initialize_pose3.py index ee9195c48..3aa7e3470 100644 --- a/cython/gtsam/tests/test_initialize_pose3.py +++ b/cython/gtsam/tests/test_initialize_pose3.py @@ -15,11 +15,12 @@ import numpy as np import gtsam from gtsam import NonlinearFactorGraph, Point3, Pose3, Rot3, Values +from gtsam.utils.test_case import GtsamTestCase x0, x1, x2, x3 = 0, 1, 2, 3 -class TestValues(unittest.TestCase): +class TestValues(GtsamTestCase): def setUp(self): @@ -67,10 +68,10 @@ class TestValues(unittest.TestCase): initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph) # comparison is up to M_PI, that's why we add some multiples of 2*M_PI - self.assertTrue(initial.atRot3(x0).equals(self.R0, 1e-6)) - self.assertTrue(initial.atRot3(x1).equals(self.R1, 1e-6)) - self.assertTrue(initial.atRot3(x2).equals(self.R2, 1e-6)) - self.assertTrue(initial.atRot3(x3).equals(self.R3, 1e-6)) + self.gtsamAssertEquals(initial.atRot3(x0), self.R0, 1e-6) + self.gtsamAssertEquals(initial.atRot3(x1), self.R1, 1e-6) + self.gtsamAssertEquals(initial.atRot3(x2), self.R2, 1e-6) + self.gtsamAssertEquals(initial.atRot3(x3), self.R3, 1e-6) def test_initializePoses(self): g2oFile = gtsam.findExampleDataFile("pose3example-grid") @@ -81,7 +82,7 @@ class TestValues(unittest.TestCase): initial = gtsam.InitializePose3.initialize(inputGraph) # TODO(frank): very loose !! - self.assertTrue(initial.equals(expectedValues, 0.1)) + self.gtsamAssertEquals(initial, expectedValues, 0.1) if __name__ == "__main__": diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py index 90c4e436b..8d3af311f 100644 --- a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py @@ -1,7 +1,20 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Cal3Unified unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest + +import numpy as np + import gtsam import gtsam_unstable -import numpy as np +from gtsam.utils.test_case import GtsamTestCase def _timestamp_key_value(key, value): @@ -10,7 +23,7 @@ def _timestamp_key_value(key, value): ) -class TestFixedLagSmootherExample(unittest.TestCase): +class TestFixedLagSmootherExample(GtsamTestCase): ''' Tests the fixed lag smoother wrapper ''' From 79880d6a7ced9a838d6a9387a2f027c51c66af61 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 20 Mar 2019 17:36:11 -0400 Subject: [PATCH 61/61] Use GtsamTestCase in example --- cython/gtsam/examples/PlanarManipulatorExample.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cython/gtsam/examples/PlanarManipulatorExample.py b/cython/gtsam/examples/PlanarManipulatorExample.py index af21e6ca5..73959b6c5 100644 --- a/cython/gtsam/examples/PlanarManipulatorExample.py +++ b/cython/gtsam/examples/PlanarManipulatorExample.py @@ -24,6 +24,7 @@ from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 import gtsam import gtsam.utils.plot as gtsam_plot from gtsam import Pose2 +from gtsam.utils.test_case import GtsamTestCase def vector3(x, y, z): @@ -202,7 +203,7 @@ Q1 = np.radians(vector3(-30, -45, -90)) Q2 = np.radians(vector3(-90, 90, 0)) -class TestPose2SLAMExample(unittest.TestCase): +class TestPose2SLAMExample(GtsamTestCase): """Unit tests for functions used below.""" def setUp(self):