12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #480 from borglab/fix/shonan_unit_tests 

Fix/shonan unit tests
release/4.3a0
Frank Dellaert 2020-08-19 10:29:18 -04:00 committed by GitHub
parent 8154dfd10b 5e803e917e
commit 905a13c940
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 168 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/IMUKittiExampleGPS.cpp
View File

@ -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);

2
gtsam/gtsam.i
View File

@ -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<double, Vector> 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<double, Vector> computeMinEigenVector(const gtsam::Values& values) const;
bool checkOptimality(const gtsam::Values& values) const;

17
gtsam/sfm/ShonanAveraging.cpp
View File

@ -749,10 +749,23 @@ Values ShonanAveraging<d>::initializeRandomly(std::mt19937 &rng) const {
return initial;
}
/* ************************************************************************* */
template <size_t d> Values ShonanAveraging<d>::initializeRandomly() const {
return initializeRandomly(kRandomNumberGenerator);
}
/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::initializeRandomly() const {
return ShonanAveraging<d>::initializeRandomly(kRandomNumberGenerator);
Values ShonanAveraging<d>::initializeRandomlyAt(size_t p,
std::mt19937 &rng) const {
const Values randomRotations = initializeRandomly(rng);
return LiftTo<Rot3>(p, randomRotations); // lift to p!
}
/* ************************************************************************* */
template <size_t d>
Values ShonanAveraging<d>::initializeRandomlyAt(size_t p) const {
return initializeRandomlyAt(p, kRandomNumberGenerator);
}
/* ************************************************************************* */

9
gtsam/sfm/ShonanAveraging.h
View File

@ -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)

5
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -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<GaussianFactor> linearize(
const Values& x) const {
boost::shared_ptr<GaussianFactor> linearize(const Values &x) const override {
// Only linearize if the factor is active
if (!this->active(x))
return boost::shared_ptr<JacobianFactor>();

139
python/gtsam/tests/test_ShonanAveraging.py Normal file
View File

@ -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()