From 5a6bfed42c8d397be578b4e8512d37ef018dd1d8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 18 Aug 2020 22:27:02 -0400 Subject: [PATCH 1/4] Fix override warning --- gtsam_unstable/slam/BetweenFactorEM.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index d551209c9..43607ac41 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -114,7 +114,7 @@ public: /** implement functions needed to derive from Factor */ /* ************************************************************************* */ - virtual double error(const Values& x) const { + double error(const Values &x) const override { return whitenedError(x).squaredNorm(); } @@ -125,8 +125,7 @@ public: * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ /* This version of linearize recalculates the noise model each time */ - virtual boost::shared_ptr linearize( - const Values& x) const { + boost::shared_ptr linearize(const Values &x) const override { // Only linearize if the factor is active if (!this->active(x)) return boost::shared_ptr(); From fc7944b42d32ff9ff9f7b8d176b73c03f20b18c7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 18 Aug 2020 22:28:04 -0400 Subject: [PATCH 2/4] Recreated initializeRandomlyAt --- gtsam/gtsam.i | 2 ++ gtsam/sfm/ShonanAveraging.cpp | 17 +++++++++++++++-- gtsam/sfm/ShonanAveraging.h | 9 +++++++++ 3 files changed, 26 insertions(+), 2 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 058f039a2..d67b74812 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2960,6 +2960,7 @@ class ShonanAveraging2 { // Advanced API gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; + gtsam::Values initializeRandomlyAt(size_t p) const; double costAt(size_t p, const gtsam::Values& values) const; pair computeMinEigenVector(const gtsam::Values& values) const; bool checkOptimality(const gtsam::Values& values) const; @@ -3004,6 +3005,7 @@ class ShonanAveraging3 { // Advanced API gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; + gtsam::Values initializeRandomlyAt(size_t p) const; double costAt(size_t p, const gtsam::Values& values) const; pair computeMinEigenVector(const gtsam::Values& values) const; bool checkOptimality(const gtsam::Values& values) const; diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index d4f189b0b..2485418cf 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -749,10 +749,23 @@ Values ShonanAveraging::initializeRandomly(std::mt19937 &rng) const { return initial; } +/* ************************************************************************* */ +template Values ShonanAveraging::initializeRandomly() const { + return initializeRandomly(kRandomNumberGenerator); +} + /* ************************************************************************* */ template -Values ShonanAveraging::initializeRandomly() const { - return ShonanAveraging::initializeRandomly(kRandomNumberGenerator); +Values ShonanAveraging::initializeRandomlyAt(size_t p, + std::mt19937 &rng) const { + const Values randomRotations = initializeRandomly(rng); + return LiftTo(p, randomRotations); // lift to p! +} + +/* ************************************************************************* */ +template +Values ShonanAveraging::initializeRandomlyAt(size_t p) const { + return initializeRandomlyAt(p, kRandomNumberGenerator); } /* ************************************************************************* */ diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index 90c0bda63..ed94329a2 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -235,6 +235,15 @@ public: */ NonlinearFactorGraph buildGraphAt(size_t p) const; + /** + * Create initial Values of type SO(p) + * @param p the dimensionality of the rotation manifold + */ + Values initializeRandomlyAt(size_t p, std::mt19937 &rng) const; + + /// Version of initializeRandomlyAt with fixed random seed. + Values initializeRandomlyAt(size_t p) const; + /** * Calculate cost for SO(p) * Values should be of type SO(p) From cb2a8cd9501680d313797b850eefb8bdf45cad03 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 18 Aug 2020 23:19:19 -0400 Subject: [PATCH 3/4] Added Shonan Averaging unit tests --- python/gtsam/tests/test_ShonanAveraging.py | 139 +++++++++++++++++++++ 1 file changed, 139 insertions(+) create mode 100644 python/gtsam/tests/test_ShonanAveraging.py diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py new file mode 100644 index 000000000..4c423574d --- /dev/null +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -0,0 +1,139 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for Shonan Rotation Averaging. +Author: Frank Dellaert +""" +# pylint: disable=invalid-name, no-name-in-module, no-member + +import unittest + +import gtsam +from gtsam import ShonanAveraging3, ShonanAveragingParameters3 +from gtsam.utils.test_case import GtsamTestCase + +DEFAULT_PARAMS = ShonanAveragingParameters3( + gtsam.LevenbergMarquardtParams.CeresDefaults()) + + +def fromExampleName(name: str, parameters=DEFAULT_PARAMS): + g2oFile = gtsam.findExampleDataFile(name) + return ShonanAveraging3(g2oFile, parameters) + + +class TestShonanAveraging(GtsamTestCase): + """Tests for Shonan Rotation Averaging.""" + + def setUp(self): + """Set up common variables.""" + self.shonan = fromExampleName("toyExample.g2o") + + def test_checkConstructor(self): + self.assertEqual(5, self.shonan.nrUnknowns()) + + D = self.shonan.denseD() + self.assertEqual((15, 15), D.shape) + + Q = self.shonan.denseQ() + self.assertEqual((15, 15), Q.shape) + + L = self.shonan.denseL() + self.assertEqual((15, 15), L.shape) + + def test_buildGraphAt(self): + graph = self.shonan.buildGraphAt(5) + self.assertEqual(7, graph.size()) + + def test_checkOptimality(self): + random = self.shonan.initializeRandomlyAt(4) + lambdaMin = self.shonan.computeMinEigenValue(random) + self.assertAlmostEqual(-414.87376657555996, + lambdaMin, places=3) # Regression test + self.assertFalse(self.shonan.checkOptimality(random)) + + def test_tryOptimizingAt3(self): + initial = self.shonan.initializeRandomlyAt(3) + self.assertFalse(self.shonan.checkOptimality(initial)) + result = self.shonan.tryOptimizingAt(3, initial) + self.assertTrue(self.shonan.checkOptimality(result)) + lambdaMin = self.shonan.computeMinEigenValue(result) + self.assertAlmostEqual(-5.427688831332745e-07, + lambdaMin, places=3) # Regression test + self.assertAlmostEqual(0, self.shonan.costAt(3, result), places=3) + SO3Values = self.shonan.roundSolution(result) + self.assertAlmostEqual(0, self.shonan.cost(SO3Values), places=3) + + def test_tryOptimizingAt4(self): + random = self.shonan.initializeRandomlyAt(4) + result = self.shonan.tryOptimizingAt(4, random) + self.assertTrue(self.shonan.checkOptimality(result)) + self.assertAlmostEqual(0, self.shonan.costAt(4, result), places=2) + lambdaMin = self.shonan.computeMinEigenValue(result) + self.assertAlmostEqual(-5.427688831332745e-07, + lambdaMin, places=3) # Regression test + SO3Values = self.shonan.roundSolution(result) + self.assertAlmostEqual(0, self.shonan.cost(SO3Values), places=3) + + def test_initializeWithDescent(self): + random = self.shonan.initializeRandomlyAt(3) + Qstar3 = self.shonan.tryOptimizingAt(3, random) + lambdaMin, minEigenVector = self.shonan.computeMinEigenVector(Qstar3) + initialQ4 = self.shonan.initializeWithDescent( + 4, Qstar3, minEigenVector, lambdaMin) + self.assertAlmostEqual(5, initialQ4.size()) + + def test_run(self): + initial = self.shonan.initializeRandomly() + result, lambdaMin = self.shonan.run(initial, 5, 10) + self.assertAlmostEqual(0, self.shonan.cost(result), places=2) + self.assertAlmostEqual(-5.427688831332745e-07, + lambdaMin, places=3) # Regression test + + def test_runKlausKarcher(self): + # Load 2D toy example + lmParams = gtsam.LevenbergMarquardtParams.CeresDefaults() + # lmParams.setVerbosityLM("SUMMARY") + g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") + parameters = gtsam.ShonanAveragingParameters2(lmParams) + shonan = gtsam.ShonanAveraging2(g2oFile, parameters) + self.assertAlmostEqual(4, shonan.nrUnknowns()) + + # Check graph building + graph = shonan.buildGraphAt(2) + self.assertAlmostEqual(6, graph.size()) + initial = shonan.initializeRandomly() + result, lambdaMin = shonan.run(initial, 2, 10) + self.assertAlmostEqual(0.0008211, shonan.cost(result), places=5) + self.assertAlmostEqual(0, lambdaMin, places=9) # certificate! + + # Test alpha/beta/gamma prior weighting. + def test_PriorWeights(self): + lmParams = gtsam.LevenbergMarquardtParams.CeresDefaults() + params = ShonanAveragingParameters3(lmParams) + self.assertAlmostEqual(0, params.getAnchorWeight(), 1e-9) + self.assertAlmostEqual(1, params.getKarcherWeight(), 1e-9) + self.assertAlmostEqual(0, params.getGaugesWeight(), 1e-9) + alpha, beta, gamma = 100.0, 200.0, 300.0 + params.setAnchorWeight(alpha) + params.setKarcherWeight(beta) + params.setGaugesWeight(gamma) + self.assertAlmostEqual(alpha, params.getAnchorWeight(), 1e-9) + self.assertAlmostEqual(beta, params.getKarcherWeight(), 1e-9) + self.assertAlmostEqual(gamma, params.getGaugesWeight(), 1e-9) + params.setKarcherWeight(0) + shonan = fromExampleName("Klaus3.g2o", params) + + initial = gtsam.Values() + for i in range(3): + initial.insert(i, gtsam.Rot3()) + self.assertAlmostEqual(3.0756, shonan.cost(initial), places=3) + result, _lambdaMin = shonan.run(initial, 3, 3) + self.assertAlmostEqual(0.0015, shonan.cost(result), places=3) + + +if __name__ == '__main__': + unittest.main() From 5e803e917e75a6b7065da40a64321255528cbd24 Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Wed, 19 Aug 2020 09:29:09 -0400 Subject: [PATCH 4/4] Fixed example printing --- examples/IMUKittiExampleGPS.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/IMUKittiExampleGPS.cpp b/examples/IMUKittiExampleGPS.cpp index f1d89b47a..e2ca49647 100644 --- a/examples/IMUKittiExampleGPS.cpp +++ b/examples/IMUKittiExampleGPS.cpp @@ -287,7 +287,7 @@ int main(int argc, char* argv[]) { new_values.insert(current_pose_key, gps_pose); printf("################ POSE INCLUDED AT TIME %lf ################\n", t); - gps_pose.translation().print(); + cout << gps_pose.translation(); printf("\n\n"); } else { new_values.insert(current_pose_key, current_pose_global);