From 41384d12e1aff4a4ca1a8b128783f81561c27f7b Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 26 Jan 2022 10:35:47 -0700 Subject: [PATCH 01/15] Update slam.i --- gtsam/slam/slam.i | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 602b2afe3..ef02686d2 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -323,6 +323,9 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor { KarcherMeanFactor(const gtsam::KeyVector& keys); }; +template +const T FindKarcherMean(const std::vector& rotations; + #include gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, size_t d); From 8ba9ae51344ef07be3ebbc97a4c1ca08e3b6bc9c Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 26 Jan 2022 10:36:22 -0700 Subject: [PATCH 02/15] fix typo --- gtsam/slam/slam.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index ef02686d2..58ba37d67 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -324,7 +324,7 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor { }; template -const T FindKarcherMean(const std::vector& rotations; +const T FindKarcherMean(const std::vector& rotations); #include gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, From b7a502ea04c037ba75d2f0725cda36e3f36f2c60 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 26 Jan 2022 10:41:04 -0700 Subject: [PATCH 03/15] fix typo --- gtsam/slam/slam.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 58ba37d67..926489153 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -324,7 +324,7 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor { }; template -const T FindKarcherMean(const std::vector& rotations); +const T FindKarcherMean(const std::vector& rotations); #include gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, From 055e0276320552fef490441150eb92daee2e9863 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 26 Jan 2022 17:35:00 -0700 Subject: [PATCH 04/15] fix template bug --- gtsam/slam/slam.i | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 926489153..52c1e13b2 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -322,9 +322,7 @@ template -const T FindKarcherMean(const std::vector& rotations); +const gtsam::Rot3 FindKarcherMean(const std::vector& rotations); #include gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, From 20622c257918be4ec7980ba79c9ebb09b42bacee Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 26 Jan 2022 17:56:23 -0700 Subject: [PATCH 05/15] add new type for vector of Rot3's --- gtsam/slam/slam.i | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 52c1e13b2..8b64896f8 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -322,7 +322,15 @@ template (const std::vector& rotations); + +class Rot3Vector { + Rot3Vector(); + + // structure specific methods + gtsam::Rot3 at(size_t i) const; + void push_back(const gtsam::Rot3& R); +}; +const gtsam::Rot3 FindKarcherMean(const Rot3Vector& rotations); #include gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, From 9e63911835b1bacfa7843eeba5fe42cf8227c3cf Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 26 Jan 2022 17:58:36 -0700 Subject: [PATCH 06/15] add Rot3Vector type to specializations --- python/gtsam/specializations/slam.h | 1 + 1 file changed, 1 insertion(+) diff --git a/python/gtsam/specializations/slam.h b/python/gtsam/specializations/slam.h index 198485a72..04357ce41 100644 --- a/python/gtsam/specializations/slam.h +++ b/python/gtsam/specializations/slam.h @@ -17,3 +17,4 @@ py::bind_vector< py::bind_vector< std::vector > > >( m_, "BetweenFactorPose2s"); +py::bind_vector>(m_, "Rot3Vector"); From 6678fe01ade16fffa2de0ad1ca48e8c5bc8b1b94 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 26 Jan 2022 17:59:20 -0700 Subject: [PATCH 07/15] add Rot3Vector to python/CMakeLists.txt --- python/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index f42e330b2..bc631ee5a 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -45,6 +45,7 @@ set(ignore gtsam::Point3Pairs gtsam::Pose3Pairs gtsam::Pose3Vector + gtsam::Rot3Vector gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 gtsam::DiscreteKey From 15c3dd4b0e32e73259ee951669ffcd9e88b5cf26 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 26 Jan 2022 18:00:30 -0700 Subject: [PATCH 08/15] make vector of Rot3's opaque --- python/gtsam/preamble/slam.h | 1 + 1 file changed, 1 insertion(+) diff --git a/python/gtsam/preamble/slam.h b/python/gtsam/preamble/slam.h index 34dbb4b7a..bb014ddc8 100644 --- a/python/gtsam/preamble/slam.h +++ b/python/gtsam/preamble/slam.h @@ -15,3 +15,4 @@ PYBIND11_MAKE_OPAQUE( std::vector > >); PYBIND11_MAKE_OPAQUE( std::vector > >); +PYBIND11_MAKE_OPAQUE(std::vector); From 72e4a9a802d1e6b4603652998377c950458cb42e Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 27 Jan 2022 00:48:38 -0500 Subject: [PATCH 09/15] Address Akshay comments --- gtsam/slam/slam.i | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 8b64896f8..864de5300 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -325,12 +325,13 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor { class Rot3Vector { Rot3Vector(); + size_t size() const; // structure specific methods gtsam::Rot3 at(size_t i) const; void push_back(const gtsam::Rot3& R); }; -const gtsam::Rot3 FindKarcherMean(const Rot3Vector& rotations); +gtsam::Rot3 FindKarcherMean(const Rot3Vector& rotations); #include gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, From 2f3fe7372af58c07ace83c7f67d2268f08b3c21f Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 27 Jan 2022 07:00:29 -0700 Subject: [PATCH 10/15] fix templated function --- gtsam/slam/slam.i | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 864de5300..a3d5be246 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -331,7 +331,8 @@ class Rot3Vector { gtsam::Rot3 at(size_t i) const; void push_back(const gtsam::Rot3& R); }; -gtsam::Rot3 FindKarcherMean(const Rot3Vector& rotations); +typedef FindKarcherMean FindKarcherMeanRot3; +gtsam::Rot3 FindKarcherMeanRot3(const Rot3Vector& rotations); #include gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, From 8a22ffa5f24334baa125d6872bf95bf44fa68573 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 27 Jan 2022 11:02:04 -0700 Subject: [PATCH 11/15] define FindKarcherMean in .i file --- gtsam/slam/slam.i | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index a3d5be246..1e159ec5f 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -316,13 +316,18 @@ class InitializePose3 { static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); }; -#include +#include template virtual class KarcherMeanFactor : gtsam::NonlinearFactor { KarcherMeanFactor(const gtsam::KeyVector& keys); }; +template +T FindKarcherMean(const std::vector> &rotations); + +template T FindKarcherMean(std::initializer_list &&rotations); + class Rot3Vector { Rot3Vector(); size_t size() const; From e0b37ed7ad6d7dd4ff35c00dfaf6a53bb28ff301 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 27 Jan 2022 15:35:41 -0500 Subject: [PATCH 12/15] use Eigen::aligned_allocator --- python/gtsam/preamble/slam.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/preamble/slam.h b/python/gtsam/preamble/slam.h index bb014ddc8..d4c2a8919 100644 --- a/python/gtsam/preamble/slam.h +++ b/python/gtsam/preamble/slam.h @@ -15,4 +15,4 @@ PYBIND11_MAKE_OPAQUE( std::vector > >); PYBIND11_MAKE_OPAQUE( std::vector > >); -PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(std::vector >); From 56312d91be04118e4a6517e68f1162dd4de04884 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 27 Jan 2022 15:35:51 -0500 Subject: [PATCH 13/15] use Eigen::aligned_allocator --- python/gtsam/specializations/slam.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/specializations/slam.h b/python/gtsam/specializations/slam.h index 04357ce41..aea3a0c6c 100644 --- a/python/gtsam/specializations/slam.h +++ b/python/gtsam/specializations/slam.h @@ -17,4 +17,4 @@ py::bind_vector< py::bind_vector< std::vector > > >( m_, "BetweenFactorPose2s"); -py::bind_vector>(m_, "Rot3Vector"); +py::bind_vector>(m_, "Rot3Vector"); From d37881e6560aa17e246789551b5f6579e916db12 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 27 Jan 2022 15:36:20 -0500 Subject: [PATCH 14/15] use aligned allocator for Rot3Vector --- gtsam/slam/slam.i | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 1e159ec5f..ef946c655 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -322,12 +322,7 @@ template -T FindKarcherMean(const std::vector> &rotations); -template T FindKarcherMean(std::initializer_list &&rotations); - class Rot3Vector { Rot3Vector(); size_t size() const; @@ -336,8 +331,9 @@ class Rot3Vector { gtsam::Rot3 at(size_t i) const; void push_back(const gtsam::Rot3& R); }; +gtsam::Rot3 FindKarcherMean(const Rot3Vector &rotations); typedef FindKarcherMean FindKarcherMeanRot3; -gtsam::Rot3 FindKarcherMeanRot3(const Rot3Vector& rotations); +gtsam::Rot3 FindKarcherMeanRot3(const gtsam::Rot3Vector& rotations); #include gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, From 55d839989d20e566c9829e69428c3949e60ad34c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 28 Jan 2022 11:52:18 -0500 Subject: [PATCH 15/15] Fix wrapping --- gtsam/geometry/Rot3.h | 29 +++++++------- gtsam/slam/slam.i | 14 +------ python/gtsam/preamble/slam.h | 2 +- python/gtsam/specializations/slam.h | 6 +-- python/gtsam/tests/test_KarcherMeanFactor.py | 40 ++++++++++---------- 5 files changed, 41 insertions(+), 50 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index abd74e063..18bd88b52 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -49,16 +49,14 @@ namespace gtsam { - /** - * @brief A 3D rotation represented as a rotation matrix if the preprocessor - * symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it - * is defined. - * @addtogroup geometry - * \nosubgrouping - */ - class GTSAM_EXPORT Rot3 : public LieGroup { - - private: +/** + * @brief Rot3 is a 3D rotation represented as a rotation matrix if the + * preprocessor symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion + * if it is defined. + * @addtogroup geometry + */ +class GTSAM_EXPORT Rot3 : public LieGroup { + private: #ifdef GTSAM_USE_QUATERNIONS /** Internal Eigen Quaternion */ @@ -67,8 +65,7 @@ namespace gtsam { SO3 rot_; #endif - public: - + public: /// @name Constructors and named constructors /// @{ @@ -83,7 +80,7 @@ namespace gtsam { */ Rot3(const Point3& col1, const Point3& col2, const Point3& col3); - /** constructor from a rotation matrix, as doubles in *row-major* order !!! */ + /// Construct from a rotation matrix, as doubles in *row-major* order !!! Rot3(double R11, double R12, double R13, double R21, double R22, double R23, double R31, double R32, double R33); @@ -567,6 +564,9 @@ namespace gtsam { #endif }; + /// std::vector of Rot3s, mainly for wrapper + using Rot3Vector = std::vector >; + /** * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' @@ -585,5 +585,6 @@ namespace gtsam { template<> struct traits : public internal::LieGroup {}; -} + +} // namespace gtsam diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index ef946c655..e044dd2c1 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -316,24 +316,14 @@ class InitializePose3 { static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); }; -#include +#include template virtual class KarcherMeanFactor : gtsam::NonlinearFactor { KarcherMeanFactor(const gtsam::KeyVector& keys); }; -class Rot3Vector { - Rot3Vector(); - size_t size() const; - - // structure specific methods - gtsam::Rot3 at(size_t i) const; - void push_back(const gtsam::Rot3& R); -}; -gtsam::Rot3 FindKarcherMean(const Rot3Vector &rotations); -typedef FindKarcherMean FindKarcherMeanRot3; -gtsam::Rot3 FindKarcherMeanRot3(const gtsam::Rot3Vector& rotations); +gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations); #include gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, diff --git a/python/gtsam/preamble/slam.h b/python/gtsam/preamble/slam.h index d4c2a8919..f7bf5863c 100644 --- a/python/gtsam/preamble/slam.h +++ b/python/gtsam/preamble/slam.h @@ -15,4 +15,4 @@ PYBIND11_MAKE_OPAQUE( std::vector > >); PYBIND11_MAKE_OPAQUE( std::vector > >); -PYBIND11_MAKE_OPAQUE(std::vector >); +PYBIND11_MAKE_OPAQUE(gtsam::Rot3Vector); diff --git a/python/gtsam/specializations/slam.h b/python/gtsam/specializations/slam.h index aea3a0c6c..6a439c370 100644 --- a/python/gtsam/specializations/slam.h +++ b/python/gtsam/specializations/slam.h @@ -12,9 +12,9 @@ */ py::bind_vector< - std::vector > > >( + std::vector>>>( m_, "BetweenFactorPose3s"); py::bind_vector< - std::vector > > >( + std::vector>>>( m_, "BetweenFactorPose2s"); -py::bind_vector>(m_, "Rot3Vector"); +py::bind_vector(m_, "Rot3Vector"); diff --git a/python/gtsam/tests/test_KarcherMeanFactor.py b/python/gtsam/tests/test_KarcherMeanFactor.py index a315a506c..f4ec64283 100644 --- a/python/gtsam/tests/test_KarcherMeanFactor.py +++ b/python/gtsam/tests/test_KarcherMeanFactor.py @@ -15,27 +15,15 @@ import unittest import gtsam import numpy as np +from gtsam import Rot3 from gtsam.utils.test_case import GtsamTestCase KEY = 0 MODEL = gtsam.noiseModel.Unit.Create(3) -def find_Karcher_mean_Rot3(rotations): - """Find the Karcher mean of given values.""" - # Cost function C(R) = \sum PriorFactor(R_i)::error(R) - # No closed form solution. - graph = gtsam.NonlinearFactorGraph() - for R in rotations: - graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL)) - initial = gtsam.Values() - initial.insert(KEY, gtsam.Rot3()) - result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() - return result.atRot3(KEY) - - # Rot3 version -R = gtsam.Rot3.Expmap(np.array([0.1, 0, 0])) +R = Rot3.Expmap(np.array([0.1, 0, 0])) class TestKarcherMean(GtsamTestCase): @@ -43,11 +31,23 @@ class TestKarcherMean(GtsamTestCase): def test_find(self): # Check that optimizing for Karcher mean (which minimizes Between distance) # gets correct result. - rotations = {R, R.inverse()} - expected = gtsam.Rot3() - actual = find_Karcher_mean_Rot3(rotations) + rotations = gtsam.Rot3Vector([R, R.inverse()]) + expected = Rot3() + actual = gtsam.FindKarcherMean(rotations) self.gtsamAssertEquals(expected, actual) + def test_find_karcher_mean_identity(self): + """Averaging 3 identity rotations should yield the identity.""" + a1Rb1 = Rot3() + a2Rb2 = Rot3() + a3Rb3 = Rot3() + + aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3]) + aRb_expected = Rot3() + + aRb = gtsam.FindKarcherMean(aRb_list) + self.gtsamAssertEquals(aRb, aRb_expected) + def test_factor(self): """Check that the InnerConstraint factor leaves the mean unchanged.""" # Make a graph with two variables, one between, and one InnerConstraint @@ -66,11 +66,11 @@ class TestKarcherMean(GtsamTestCase): initial = gtsam.Values() initial.insert(1, R.inverse()) initial.insert(2, R) - expected = find_Karcher_mean_Rot3([R, R.inverse()]) + expected = Rot3() result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() - actual = find_Karcher_mean_Rot3( - [result.atRot3(1), result.atRot3(2)]) + actual = gtsam.FindKarcherMean( + gtsam.Rot3Vector([result.atRot3(1), result.atRot3(2)])) self.gtsamAssertEquals(expected, actual) self.gtsamAssertEquals( R12, result.atRot3(1).between(result.atRot3(2)))