71 lines
2.2 KiB
C++
71 lines
2.2 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file testGaussianISAM.cpp
|
|
* @brief Unit tests for GaussianISAM
|
|
* @author Michael Kaess
|
|
*/
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include <tests/smallExample.h>
|
|
#include <gtsam/inference/Symbol.h>
|
|
#include <gtsam/linear/GaussianISAM.h>
|
|
#include <gtsam/inference/Ordering.h>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
using namespace example;
|
|
|
|
using symbol_shorthand::X;
|
|
using symbol_shorthand::L;
|
|
|
|
/* ************************************************************************* */
|
|
TEST( ISAM, iSAM_smoother )
|
|
{
|
|
Ordering ordering;
|
|
for (int t = 1; t <= 7; t++) ordering.push_back(X(t));
|
|
|
|
// Create smoother with 7 nodes
|
|
GaussianFactorGraph smoother = createSmoother(7);
|
|
|
|
// run iSAM for every factor
|
|
GaussianISAM actual;
|
|
for(std::shared_ptr<GaussianFactor> factor: smoother) {
|
|
GaussianFactorGraph factorGraph;
|
|
factorGraph.push_back(factor);
|
|
actual.update(factorGraph);
|
|
}
|
|
|
|
// Create expected Bayes Tree by solving smoother with "natural" ordering
|
|
GaussianBayesTree expected = *smoother.eliminateMultifrontal(ordering);
|
|
|
|
// Verify sigmas in the bayes tree
|
|
for (const auto& [key, clique] : expected.nodes()) {
|
|
GaussianConditional::shared_ptr conditional = clique->conditional();
|
|
EXPECT(!conditional->get_model());
|
|
}
|
|
|
|
// Check whether BayesTree is correct
|
|
EXPECT(assert_equal(GaussianFactorGraph(expected).augmentedHessian(), GaussianFactorGraph(actual).augmentedHessian()));
|
|
|
|
// obtain solution
|
|
VectorValues e; // expected solution
|
|
for (int t = 1; t <= 7; t++) e.insert(X(t), Vector::Zero(2));
|
|
VectorValues optimized = actual.optimize(); // actual solution
|
|
EXPECT(assert_equal(e, optimized));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
|
/* ************************************************************************* */
|