From 9b481cb790d25ff9a013f9ef40f89dc7814e5ba9 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Thu, 27 Aug 2020 22:18:53 -0700 Subject: [PATCH 1/6] wrapping translation recovery doesnt build --- gtsam/gtsam.i | 11 +++++++++++ python/gtsam/specializations.h | 1 + 2 files changed, 12 insertions(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index b9ecf6f3b..a3d5c1a41 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -3033,6 +3033,17 @@ class ShonanAveraging3 { pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; }; +#include +class TranslationRecovery { + TranslationRecovery(const BinaryMeasurementsUnit3& relativeTranslations, + const LevenbergMarquardtParams& lmParams); + TranslationRecovery( + const BinaryMeasurementsUnit3& + relativeTranslations); // default LevenbergMarquardtParams + gtsam::Values run(const double scale) const; + gtsam::Values run() const; // default scale = 1.0 +}; + //************************************************************************* // Navigation //************************************************************************* diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 3b60e42cb..52fffab6b 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -9,3 +9,4 @@ py::bind_vector >(m_, "Pose3Vector"); py::bind_vector > > >(m_, "BetweenFactorPose3s"); py::bind_vector > > >(m_, "BetweenFactorPose2s"); +py::bind_vector > >(m_, "BinaryMeasurementsUnit3"); From 7ffa54f896fc81c0ab175a45dac117c54809e770 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sun, 30 Aug 2020 21:53:12 -0700 Subject: [PATCH 2/6] TAvg wrapper builds --- gtsam/gtsam.i | 14 ++++++++++---- gtsam/slam/dataset.h | 1 + python/CMakeLists.txt | 6 ++++-- 3 files changed, 15 insertions(+), 6 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index a3d5c1a41..dd98c6f99 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2914,6 +2914,13 @@ class BinaryMeasurement { typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; +class BinaryMeasurementsUnit3 { + BinaryMeasurementsUnit3(); + size_t size() const; + gtsam::BinaryMeasurement at(size_t idx) const; + void push_back(const gtsam::BinaryMeasurement measurement); +}; + #include // TODO(frank): copy/pasta below until we have integer template paremeters in wrap! @@ -3035,11 +3042,10 @@ class ShonanAveraging3 { #include class TranslationRecovery { - TranslationRecovery(const BinaryMeasurementsUnit3& relativeTranslations, - const LevenbergMarquardtParams& lmParams); + TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations, + const gtsam::LevenbergMarquardtParams &lmParams); TranslationRecovery( - const BinaryMeasurementsUnit3& - relativeTranslations); // default LevenbergMarquardtParams + const gtsam::BinaryMeasurementsUnit3 & relativeTranslations); // default LevenbergMarquardtParams gtsam::Values run(const double scale) const; gtsam::Values run() const; // default scale = 1.0 }; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 7ceca00f4..2c1bb7a1c 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -354,6 +354,7 @@ parse3DFactors(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model = nullptr, size_t maxIndex = 0); +using BinaryMeasurementsUnit3 = std::vector>; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 inline boost::optional parseVertex(std::istream &is, const std::string &tag) { diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 23898f61d..fa6ec905f 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -30,7 +30,8 @@ set(ignore gtsam::BetweenFactorPose3s gtsam::Point2Vector gtsam::Pose3Vector - gtsam::KeyVector) + gtsam::KeyVector + gtsam::BinaryMeasurementsUnit3) pybind_wrap(gtsam_py # target ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i # interface_header @@ -72,7 +73,8 @@ set(ignore gtsam::Point2Vector gtsam::Pose3Vector gtsam::KeyVector - gtsam::FixedLagSmootherKeyTimestampMapValue) + gtsam::FixedLagSmootherKeyTimestampMapValue + gtsam::BinaryMeasurementsUnit3) pybind_wrap(gtsam_unstable_py # target ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header "gtsam_unstable.cpp" # generated_cpp From 0fb5c0d2283b2c1d6d9d8727a681ba3c3dc31257 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sun, 6 Sep 2020 11:56:13 -0700 Subject: [PATCH 3/6] translation recovery py test --- .../gtsam/tests/test_TranslationRecovery.py | 48 +++++++++++++++++++ 1 file changed, 48 insertions(+) create mode 100644 python/gtsam/tests/test_TranslationRecovery.py diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py new file mode 100644 index 000000000..d49ce8e11 --- /dev/null +++ b/python/gtsam/tests/test_TranslationRecovery.py @@ -0,0 +1,48 @@ +from __future__ import print_function + +import numpy as np +import unittest + +import gtsam + +def SimulateMeasurements(gt_poses, graph_edges): + measurements = gtsam.BinaryMeasurementsUnit3() + for edge in graph_edges: + Ta = gt_poses.atPose3(edge[0]).translation() + Tb = gt_poses.atPose3(edge[1]).translation() + measurements.append(BinaryMeasurementUnit3( \ + edge[0], edge[1], gtsam.Unit3(Tb - Ta), \ + gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) + return measurements + +# Hard-coded values from dubrovnik-3-7-pre.txt +def ExampleValues(): + T = [] + T.append(gtsam.Point3(np.array([7.3030e-01, -2.6490e-01, -1.7127e+00]))) + T.append(gtsam.Point3(np.array([-1.0590e+00, -3.6017e-02, -1.5720e+00]))) + T.append(gtsam.Point3(np.array([8.5034e+00, 6.7499e+00, -3.6383e+00]))) + + data = gtsam.Values() + for i in range(len(T)): + data.insert(i, gtsam.Pose3(R[i], T[i])) + return data + +class TestTranslationRecovery(unittest.TestCase): + """Test selected Translation Recovery methods.""" + + def test_constructor(self): + """Construct from binary measurements.""" + algorithm = gtsam.TranslationRecovery(gtsam.BinaryMeasurementsUnit3()) + self.assertIsInstance(algorithm, gtsam.TranslationRecovery) + + def test_run(self): + gt_poses = ExampleValues() + measurements = SimulateMeasurements(gt_poses, [[0, 1], [0, 2], [1, 2]]) + algorithm = gtsam.TranslationRecovery(measurements) + result = algorithm.run(2.0) + for i in range(3): + self.gtsamAssertEquals(result.atPoint3(i), 2*gt_poses.atPose3(i).translation(), 1e-6) + +if __name__ == "__main__": + unittest.main() + From 556531f8b7bf2a157782a7f5db35f9ef2a5d07ef Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Sun, 6 Sep 2020 23:54:27 +0000 Subject: [PATCH 4/6] translation recovery unit tests pass --- gtsam/gtsam.i | 2 +- .../gtsam/tests/test_TranslationRecovery.py | 42 ++++++++++++------- 2 files changed, 27 insertions(+), 17 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index dd98c6f99..c2aa39352 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2918,7 +2918,7 @@ class BinaryMeasurementsUnit3 { BinaryMeasurementsUnit3(); size_t size() const; gtsam::BinaryMeasurement at(size_t idx) const; - void push_back(const gtsam::BinaryMeasurement measurement); + void push_back(const gtsam::BinaryMeasurement& measurement); }; #include diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py index d49ce8e11..ec6d6a83f 100644 --- a/python/gtsam/tests/test_TranslationRecovery.py +++ b/python/gtsam/tests/test_TranslationRecovery.py @@ -5,28 +5,30 @@ import unittest import gtsam -def SimulateMeasurements(gt_poses, graph_edges): - measurements = gtsam.BinaryMeasurementsUnit3() - for edge in graph_edges: - Ta = gt_poses.atPose3(edge[0]).translation() - Tb = gt_poses.atPose3(edge[1]).translation() - measurements.append(BinaryMeasurementUnit3( \ - edge[0], edge[1], gtsam.Unit3(Tb - Ta), \ - gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) - return measurements - -# Hard-coded values from dubrovnik-3-7-pre.txt +""" Returns example pose values of 3 points A, B and C in the world frame """ def ExampleValues(): T = [] - T.append(gtsam.Point3(np.array([7.3030e-01, -2.6490e-01, -1.7127e+00]))) + T.append(gtsam.Point3(np.array([3.14, 1.59, 2.65]))) T.append(gtsam.Point3(np.array([-1.0590e+00, -3.6017e-02, -1.5720e+00]))) T.append(gtsam.Point3(np.array([8.5034e+00, 6.7499e+00, -3.6383e+00]))) data = gtsam.Values() for i in range(len(T)): - data.insert(i, gtsam.Pose3(R[i], T[i])) + data.insert(i, gtsam.Pose3(gtsam.Rot3(), T[i])) return data +""" Returns binary measurements for the points in the given edges.""" +def SimulateMeasurements(gt_poses, graph_edges): + measurements = gtsam.BinaryMeasurementsUnit3() + for edge in graph_edges: + Ta = gt_poses.atPose3(edge[0]).translation() + Tb = gt_poses.atPose3(edge[1]).translation() + measurements.append(gtsam.BinaryMeasurementUnit3( \ + edge[0], edge[1], gtsam.Unit3(Tb - Ta), \ + gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) + return measurements + +""" Tests for the translation recovery class """ class TestTranslationRecovery(unittest.TestCase): """Test selected Translation Recovery methods.""" @@ -39,9 +41,17 @@ class TestTranslationRecovery(unittest.TestCase): gt_poses = ExampleValues() measurements = SimulateMeasurements(gt_poses, [[0, 1], [0, 2], [1, 2]]) algorithm = gtsam.TranslationRecovery(measurements) - result = algorithm.run(2.0) - for i in range(3): - self.gtsamAssertEquals(result.atPoint3(i), 2*gt_poses.atPose3(i).translation(), 1e-6) + scale = 2.0 + result = algorithm.run(scale) + + w_aTc = gt_poses.atPose3(2).translation() - gt_poses.atPose3(0).translation() + w_aTb = gt_poses.atPose3(1).translation() - gt_poses.atPose3(0).translation() + w_aTc_expected = w_aTc*scale/np.linalg.norm(w_aTb) + w_aTb_expected = w_aTb*scale/np.linalg.norm(w_aTb) + + np.testing.assert_array_almost_equal(result.atPoint3(0), np.array([0,0,0]), 6, "Origin result is incorrect.") + np.testing.assert_array_almost_equal(result.atPoint3(1), w_aTb_expected, 6, "Point B result is incorrect.") + np.testing.assert_array_almost_equal(result.atPoint3(2), w_aTc_expected, 6, "Point C result is incorrect.") if __name__ == "__main__": unittest.main() From 15151a7ee9bde1855cab015948c429e5ee912547 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 8 Sep 2020 03:06:43 +0000 Subject: [PATCH 5/6] remove binarymeasurementsunit3 class in wrapper --- gtsam/gtsam.i | 7 ------- 1 file changed, 7 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index c2aa39352..f778fd742 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2914,13 +2914,6 @@ class BinaryMeasurement { typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; -class BinaryMeasurementsUnit3 { - BinaryMeasurementsUnit3(); - size_t size() const; - gtsam::BinaryMeasurement at(size_t idx) const; - void push_back(const gtsam::BinaryMeasurement& measurement); -}; - #include // TODO(frank): copy/pasta below until we have integer template paremeters in wrap! From 57eb143a6f629496ddcbc4cb08ac1f2c0f85a32b Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Tue, 8 Sep 2020 22:24:44 -0700 Subject: [PATCH 6/6] adding binarymeasurementsunit3 --- gtsam/gtsam.i | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 6c61f3008..f52b6cedd 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2961,6 +2961,13 @@ class BinaryMeasurement { typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; +class BinaryMeasurementsUnit3 { + BinaryMeasurementsUnit3(); + size_t size() const; + gtsam::BinaryMeasurement at(size_t idx) const; + void push_back(const gtsam::BinaryMeasurement& measurement); +}; + #include // TODO(frank): copy/pasta below until we have integer template paremeters in wrap!