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

161
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 model2 = noiseModel::Unit::Create(2);
gtsam::noiseModel::Base::shared_ptr model3 = noiseModel::Unit::Create(3); 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 pose0;
Pose2 pose1(1.0, 0.0, 0.0); Pose2 pose1(1.0, 0.0, 0.0);
@ -72,37 +73,149 @@ TEST( testSummarization, example_from_ddf1 ) {
KeySet saved_keys; KeySet saved_keys;
saved_keys += lA3, lA5; saved_keys += lA3, lA5;
// Summarize to a linear system {
GaussianFactorGraph actLinGraph; Ordering actOrdering; // Summarize to a linear system
SummarizationMode mode = PARTIAL_QR; GaussianFactorGraph actLinGraph; Ordering actOrdering;
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); SummarizationMode mode = PARTIAL_QR;
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);
Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5; Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
EXPECT(assert_equal(expSumOrdering, actOrdering)); EXPECT(assert_equal(expSumOrdering, actOrdering));
// Does not split out subfactors where possible // Does not split out subfactors where possible
GaussianFactorGraph expLinGraph; GaussianFactorGraph expLinGraph;
expLinGraph.add( expLinGraph.add(
expSumOrdering[lA3], expSumOrdering[lA3],
Matrix_(4,2, Matrix_(4,2,
0.595867, 0.605092, 0.595867, 0.605092,
0.0, -0.406109, 0.0, -0.406109,
0.0, 0.0, 0.0, 0.0,
0.0, 0.0), 0.0, 0.0),
expSumOrdering[lA5], expSumOrdering[lA5],
Matrix_(4,2, 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 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.125971, -0.160052,
0.13586, 0.301096, 0.13586, 0.301096,
0.268667, 0.31703, 0.268667, 0.31703,
0.0, -0.131698), 0.0, -0.131698),
zero(4), model); zero(4), diagmodel4)));
EXPECT(assert_equal(expLinGraph, actLinGraph, tol)); EXPECT(assert_equal(expLinGraph, actLinGraph, tol));
// Summarize directly from a nonlinear graph to another nonlinear graph // Summarize directly from a nonlinear graph to another nonlinear graph
NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode); NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, mode);
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering); NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering);
EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol)); 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));
}
} }
/* ************************************************************************* */ /* ************************************************************************* */