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

unit test for regularization

release/4.3a0
Luca 2014-05-16 13:34:00 -04:00
parent a0d0243e36
commit 3522e6e394
1 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
examples/tests/testPlanarSLAMExample_lago.cpp
View File

@ -259,7 +259,7 @@ GaussianFactorGraph buildOrientationGraph(const vector<size_t>& spanningTree, co
} }
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, sumOverLoops ) { TEST( Lago, regularizedMeasurements ) {
NonlinearFactorGraph g = simple::graph(); NonlinearFactorGraph g = simple::graph();
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g); BetweenFactor<Pose2> >(g);
@ -273,12 +273,16 @@ TEST( Lago, sumOverLoops ) {
map<Key, double> orientationsToRoot = computeThetasToRoot(keysInBinary, deltaThetaMap, tree); map<Key, double> orientationsToRoot = computeThetasToRoot(keysInBinary, deltaThetaMap, tree);
GaussianFactorGraph lagoGraph = buildOrientationGraph(spanningTree, chords, g, orientationsToRoot); GaussianFactorGraph lagoGraph = buildOrientationGraph(spanningTree, chords, g, orientationsToRoot);
std::pair<Matrix,Vector> actualAb = lagoGraph.jacobian();
Vector actual = actualAb.second;
// Vector2 expected; // This respects the order of the factors in the original graph
// expected[0]= 0.0; Vector expected = (Vector(5) << 1.570796326794897, 1.570796326794897, 1.570796326794897, -3.141592653589793, 4.712388980384690 );
// expected[1]= 0.0; // This arranges the spanning tree first and chords later
// DOUBLES_EQUAL(expected[x0], actual[x0], 1e-6); Vector orderedExpected = (Vector(5) << expected[spanningTree[0]], expected[spanningTree[1]], expected[spanningTree[2]],
// DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6); expected[chords[0]], expected[chords[1]] );
EXPECT(assert_equal(orderedExpected, actual, 1e-6));
} }
/* *************************************************************************** */ /* *************************************************************************** */