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

Added tests for each summarization mode

release/4.3a0
Alex Cunningham 2013-05-08 10:14:19 +00:00
parent c74de0136c
commit 4e0dbbc2b1
1 changed files with 137 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

117
tests/testSummarization.cpp
View File

@ -45,7 +45,8 @@ TEST( testSummarization, example_from_ddf1 ) {
gtsam::noiseModel::Base::shared_ptr model2 = noiseModel::Unit::Create(2);
gtsam::noiseModel::Base::shared_ptr model3 = noiseModel::Unit::Create(3);
SharedDiagonal model = noiseModel::Unit::Create(4);
SharedDiagonal diagmodel2 = noiseModel::Unit::Create(2);
SharedDiagonal diagmodel4 = noiseModel::Unit::Create(4);
Pose2 pose0;
Pose2 pose1(1.0, 0.0, 0.0);
@ -72,6 +73,7 @@ TEST( testSummarization, example_from_ddf1 ) {
KeySet saved_keys;
saved_keys += lA3, lA5;
{
// Summarize to a linear system
GaussianFactorGraph actLinGraph; Ordering actOrdering;
SummarizationMode mode = PARTIAL_QR;
@ -95,7 +97,7 @@ TEST( testSummarization, example_from_ddf1 ) {
0.13586, 0.301096,
0.268667, 0.31703,
0.0, -0.131698),
zero(4), model);
zero(4), diagmodel4);
EXPECT(assert_equal(expLinGraph, actLinGraph, tol));
// Summarize directly from a nonlinear graph to another nonlinear graph
@ -103,6 +105,117 @@ TEST( testSummarization, example_from_ddf1 ) {
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering);
EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
}
{
// Summarize to a linear system using cholesky - compare to previous version
GaussianFactorGraph actLinGraph; Ordering actOrdering;
SummarizationMode mode = PARTIAL_CHOLESKY;
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);
Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
EXPECT(assert_equal(expSumOrdering, actOrdering));
// Does not split out subfactors where possible
GaussianFactorGraph expLinGraph;
expLinGraph.add(HessianFactor(JacobianFactor(
expSumOrdering[lA3],
Matrix_(4,2,
0.595867, 0.605092,
0.0, -0.406109,
0.0, 0.0,
0.0, 0.0),
expSumOrdering[lA5],
Matrix_(4,2,
-0.125971, -0.160052,
0.13586, 0.301096,
0.268667, 0.31703,
0.0, -0.131698),
zero(4), diagmodel4)));
EXPECT(assert_equal(expLinGraph, actLinGraph, tol));
// Summarize directly from a nonlinear graph to another nonlinear graph
NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode);
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering);
EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
}
{
// Summarize to a linear system with joint factor graph version
GaussianFactorGraph actLinGraph; Ordering actOrdering;
SummarizationMode mode = SEQUENTIAL_QR;
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);
Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
EXPECT(assert_equal(expSumOrdering, actOrdering));
// Does not split out subfactors where possible
GaussianFactorGraph expLinGraph;
expLinGraph.add(
expSumOrdering[lA3],
Matrix_(2,2,
0.595867, 0.605092,
0.0, 0.406109),
expSumOrdering[lA5],
Matrix_(2,2,
-0.125971, -0.160052,
-0.13586, -0.301096),
zero(2), diagmodel2);
expLinGraph.add(
expSumOrdering[lA5],
Matrix_(2,2,
0.268667, 0.31703,
0.0, 0.131698),
zero(2), diagmodel2);
EXPECT(assert_equal(expLinGraph, actLinGraph, tol));
// Summarize directly from a nonlinear graph to another nonlinear graph
NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode);
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering);
EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
}
{
// Summarize to a linear system with joint factor graph version
GaussianFactorGraph actLinGraph; Ordering actOrdering;
SummarizationMode mode = SEQUENTIAL_CHOLESKY;
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);
Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
EXPECT(assert_equal(expSumOrdering, actOrdering));
// Does not split out subfactors where possible
GaussianFactorGraph expLinGraph;
expLinGraph.add(
expSumOrdering[lA3],
Matrix_(2,2,
0.595867, 0.605092,
0.0, 0.406109),
expSumOrdering[lA5],
Matrix_(2,2,
-0.125971, -0.160052,
-0.13586, -0.301096),
zero(2), diagmodel2);
expLinGraph.add(
expSumOrdering[lA5],
Matrix_(2,2,
0.268667, 0.31703,
0.0, 0.131698),
zero(2), diagmodel2);
EXPECT(assert_equal(expLinGraph, actLinGraph, tol));
// Summarize directly from a nonlinear graph to another nonlinear graph
NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode);
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering);
EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
}
}
/* ************************************************************************* */