118 lines
3.8 KiB
C++
118 lines
3.8 KiB
C++
/**
|
|
* @file testSummarization.cpp
|
|
*
|
|
* @brief Test ported from MastSLAM for a simple batch summarization technique
|
|
*
|
|
* @date May 7, 2013
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#include <boost/assign/std/set.hpp>
|
|
#include <boost/assign/std/vector.hpp>
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include <gtsam/base/TestableAssertions.h>
|
|
|
|
#include <gtsam/geometry/Pose2.h>
|
|
|
|
#include <gtsam/nonlinear/LabeledSymbol.h>
|
|
#include <gtsam/nonlinear/summarization.h>
|
|
#include <gtsam/nonlinear/LinearContainerFactor.h>
|
|
|
|
#include <gtsam/slam/PriorFactor.h>
|
|
#include <gtsam/slam/BetweenFactor.h>
|
|
#include <gtsam/slam/BearingRangeFactor.h>
|
|
|
|
//#include <ddfsam/domains/planarDDFSystem.h>
|
|
//
|
|
//#include <ddfsam/ddf1/DDF1SummarizedMap.h>
|
|
|
|
using namespace std;
|
|
using namespace boost::assign;
|
|
using namespace gtsam;
|
|
//using namespace ddfsam;
|
|
//using namespace planarDDF;
|
|
|
|
const double tol=1e-5;
|
|
|
|
typedef gtsam::PriorFactor<gtsam::Pose2> PosePrior;
|
|
typedef gtsam::BetweenFactor<gtsam::Pose2> PoseBetween;
|
|
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2> PosePointBearingRange;
|
|
|
|
/* ************************************************************************* */
|
|
TEST( testSummarization, example_from_ddf1 ) {
|
|
|
|
Key xA0 = LabeledSymbol('x', 'A', 0),
|
|
xA1 = LabeledSymbol('x', 'A', 1),
|
|
xA2 = LabeledSymbol('x', 'A', 2);
|
|
Key lA3 = LabeledSymbol('l', 'A', 3), lA5 = LabeledSymbol('l', 'A', 5);
|
|
|
|
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(2);
|
|
|
|
Pose2 pose0;
|
|
Pose2 pose1(1.0, 0.0, 0.0);
|
|
Pose2 pose2(2.0, 0.0, 0.0);
|
|
Point2 landmark3(3.0, 3.0);
|
|
Point2 landmark5(5.0, 5.0);
|
|
|
|
Values values;
|
|
values.insert(xA0, pose0);
|
|
values.insert(xA1, pose1);
|
|
values.insert(xA2, pose2);
|
|
values.insert(lA3, landmark3);
|
|
values.insert(lA5, landmark5);
|
|
|
|
// build from nonlinear graph/values
|
|
NonlinearFactorGraph graph;
|
|
graph.add(PosePrior(xA0, Pose2(), model3));
|
|
graph.add(PoseBetween(xA0, xA1, pose0.between(pose1), model3));
|
|
graph.add(PoseBetween(xA1, xA2, pose1.between(pose2), model3));
|
|
graph.add(PosePointBearingRange(xA0, lA3, pose0.bearing(landmark3), pose0.range(landmark3), model2));
|
|
graph.add(PosePointBearingRange(xA1, lA3, pose1.bearing(landmark3), pose1.range(landmark3), model2));
|
|
graph.add(PosePointBearingRange(xA2, lA5, pose2.bearing(landmark5), pose2.range(landmark5), model2));
|
|
|
|
KeySet saved_keys;
|
|
saved_keys += lA3, lA5;
|
|
|
|
// Summarize to a linear system
|
|
GaussianFactorGraph actLinGraph; Ordering actOrdering;
|
|
bool useQR = true;
|
|
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, useQR);
|
|
|
|
Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
|
|
EXPECT(assert_equal(expSumOrdering, actOrdering));
|
|
|
|
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), model);
|
|
expLinGraph.add(
|
|
expSumOrdering[lA5],
|
|
Matrix_(2,2,
|
|
0.268667, 0.31703,
|
|
0.0, 0.131698),
|
|
zero(2), model);
|
|
EXPECT(assert_equal(expLinGraph, actLinGraph, tol));
|
|
|
|
// Summarize directly from a nonlinear graph to another nonlinear graph
|
|
NonlinearFactorGraph actContainerGraph = summarizeAsNonlinearContainer(graph, values, saved_keys, useQR);
|
|
NonlinearFactorGraph expContainerGraph = LinearContainerFactor::convertLinearGraph(expLinGraph, expSumOrdering);
|
|
|
|
EXPECT(assert_equal(expContainerGraph, actContainerGraph, tol));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
|
/* ************************************************************************* */
|