251 lines
9.2 KiB
C++
251 lines
9.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 testGaussianJunctionTreeB.cpp
|
|
* @date Jul 8, 2010
|
|
* @author nikai
|
|
*/
|
|
|
|
#include <tests/smallExample.h>
|
|
#include <gtsam/sam/BearingRangeFactor.h>
|
|
#include <gtsam/slam/BetweenFactor.h>
|
|
#include <gtsam/nonlinear/PriorFactor.h>
|
|
#include <gtsam/geometry/Pose2.h>
|
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
|
#include <gtsam/nonlinear/Values.h>
|
|
#include <gtsam/linear/GaussianBayesNet.h>
|
|
#include <gtsam/linear/GaussianConditional.h>
|
|
#include <gtsam/linear/GaussianFactor.h>
|
|
#include <gtsam/linear/GaussianFactorGraph.h>
|
|
#include <gtsam/linear/GaussianEliminationTree.h>
|
|
#include <gtsam/linear/GaussianJunctionTree.h>
|
|
#include <gtsam/linear/HessianFactor.h>
|
|
#include <gtsam/linear/JacobianFactor.h>
|
|
#include <gtsam/linear/NoiseModel.h>
|
|
#include <gtsam/linear/VectorValues.h>
|
|
#include <gtsam/symbolic/SymbolicEliminationTree.h>
|
|
#include <gtsam/inference/BayesTree.h>
|
|
#include <gtsam/inference/ClusterTree.h>
|
|
#include <gtsam/inference/Ordering.h>
|
|
#include <gtsam/inference/Symbol.h>
|
|
#include <gtsam/base/Matrix.h>
|
|
#include <gtsam/base/Testable.h>
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include <cmath>
|
|
#include <list>
|
|
#include <utility>
|
|
#include <vector>
|
|
|
|
#include <iostream>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
using namespace example;
|
|
|
|
using symbol_shorthand::X;
|
|
using symbol_shorthand::L;
|
|
|
|
/* ************************************************************************* *
|
|
Bayes tree for smoother with "nested dissection" ordering:
|
|
C1 x5 x6 x4
|
|
C2 x3 x2 : x4
|
|
C3 x1 : x2
|
|
C4 x7 : x6
|
|
*/
|
|
TEST( GaussianJunctionTreeB, constructor2 ) {
|
|
// create a graph
|
|
const auto [nlfg, values] = createNonlinearSmoother(7);
|
|
SymbolicFactorGraph::shared_ptr symbolic = nlfg.symbolic();
|
|
|
|
// linearize
|
|
GaussianFactorGraph::shared_ptr fg = nlfg.linearize(values);
|
|
|
|
const Ordering ordering {X(1), X(3), X(5), X(7), X(2), X(6), X(4)};
|
|
|
|
// create an ordering
|
|
GaussianEliminationTree etree(*fg, ordering);
|
|
SymbolicEliminationTree stree(*symbolic, ordering);
|
|
GaussianJunctionTree actual(etree);
|
|
|
|
const Ordering o324{X(3), X(2), X(4)}, o56{X(5), X(6)}, o7{X(7)}, o1{X(1)};
|
|
|
|
GaussianJunctionTree::sharedNode x324 = actual.roots().front();
|
|
LONGS_EQUAL(2, x324->children.size());
|
|
GaussianJunctionTree::sharedNode x1 = x324->children.front();
|
|
GaussianJunctionTree::sharedNode x56 = x324->children.back();
|
|
if (x1->children.size() > 0)
|
|
x1.swap(x56); // makes it work with different tie-breakers
|
|
|
|
LONGS_EQUAL(0, x1->children.size());
|
|
LONGS_EQUAL(1, x56->children.size());
|
|
GaussianJunctionTree::sharedNode x7 = x56->children[0];
|
|
LONGS_EQUAL(0, x7->children.size());
|
|
|
|
EXPECT(assert_equal(o324, x324->orderedFrontalKeys));
|
|
EXPECT_LONGS_EQUAL(5, x324->factors.size());
|
|
EXPECT_LONGS_EQUAL(9, x324->problemSize_);
|
|
|
|
EXPECT(assert_equal(o56, x56->orderedFrontalKeys));
|
|
EXPECT_LONGS_EQUAL(4, x56->factors.size());
|
|
EXPECT_LONGS_EQUAL(9, x56->problemSize_);
|
|
|
|
EXPECT(assert_equal(o7, x7->orderedFrontalKeys));
|
|
EXPECT_LONGS_EQUAL(2, x7->factors.size());
|
|
EXPECT_LONGS_EQUAL(4, x7->problemSize_);
|
|
|
|
EXPECT(assert_equal(o1, x1->orderedFrontalKeys));
|
|
EXPECT_LONGS_EQUAL(2, x1->factors.size());
|
|
EXPECT_LONGS_EQUAL(4, x1->problemSize_);
|
|
}
|
|
|
|
///* ************************************************************************* */
|
|
TEST(GaussianJunctionTreeB, OptimizeMultiFrontal) {
|
|
// create a graph
|
|
const auto fg = createSmoother(7);
|
|
|
|
// optimize the graph
|
|
const VectorValues actual = fg.optimize(&EliminateQR);
|
|
|
|
// verify
|
|
VectorValues expected;
|
|
const Vector v = Vector2(0., 0.);
|
|
for (int i = 1; i <= 7; i++) expected.emplace(X(i), v);
|
|
EXPECT(assert_equal(expected, actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(GaussianJunctionTreeB, optimizeMultiFrontal2) {
|
|
// create a graph
|
|
const auto nlfg = createNonlinearFactorGraph();
|
|
const auto noisy = createNoisyValues();
|
|
const auto fg = *nlfg.linearize(noisy);
|
|
|
|
// optimize the graph
|
|
VectorValues actual = fg.optimize(&EliminateQR);
|
|
|
|
// verify
|
|
VectorValues expected = createCorrectDelta(); // expected solution
|
|
EXPECT(assert_equal(expected, actual));
|
|
}
|
|
|
|
///* ************************************************************************* */
|
|
//TEST(GaussianJunctionTreeB, slamlike) {
|
|
// Values init;
|
|
// NonlinearFactorGraph newfactors;
|
|
// NonlinearFactorGraph fullgraph;
|
|
// SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, M_PI/100.0));
|
|
// SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, 0.1));
|
|
//
|
|
// size_t i = 0;
|
|
//
|
|
// newfactors = NonlinearFactorGraph();
|
|
// newfactors.add(PriorFactor<Pose2>(X(0), Pose2(0.0, 0.0, 0.0), odoNoise));
|
|
// init.insert(X(0), Pose2(0.01, 0.01, 0.01));
|
|
// fullgraph.push_back(newfactors);
|
|
//
|
|
// for( ; i<5; ++i) {
|
|
// newfactors = NonlinearFactorGraph();
|
|
// newfactors.add(BetweenFactor<Pose2>(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise));
|
|
// init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
|
|
// fullgraph.push_back(newfactors);
|
|
// }
|
|
//
|
|
// newfactors = NonlinearFactorGraph();
|
|
// newfactors.add(BetweenFactor<Pose2>(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise));
|
|
// newfactors.add(BearingRangeFactor<Pose2,Point2>(X(i), L(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise));
|
|
// newfactors.add(BearingRangeFactor<Pose2,Point2>(X(i), L(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise));
|
|
// init.insert(X(i+1), Pose2(1.01, 0.01, 0.01));
|
|
// init.insert(L(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
|
|
// init.insert(L(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
|
|
// fullgraph.push_back(newfactors);
|
|
// ++ i;
|
|
//
|
|
// for( ; i<5; ++i) {
|
|
// newfactors = NonlinearFactorGraph();
|
|
// newfactors.add(BetweenFactor<Pose2>(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise));
|
|
// init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
|
|
// fullgraph.push_back(newfactors);
|
|
// }
|
|
//
|
|
// newfactors = NonlinearFactorGraph();
|
|
// newfactors.add(BetweenFactor<Pose2>(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise));
|
|
// newfactors.add(BearingRangeFactor<Pose2,Point2>(X(i), L(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise));
|
|
// newfactors.add(BearingRangeFactor<Pose2,Point2>(X(i), L(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise));
|
|
// init.insert(X(i+1), Pose2(6.9, 0.1, 0.01));
|
|
// fullgraph.push_back(newfactors);
|
|
// ++ i;
|
|
//
|
|
// // Compare solutions
|
|
// Ordering ordering = *fullgraph.orderingCOLAMD(init);
|
|
// GaussianFactorGraph linearized = *fullgraph.linearize(init, ordering);
|
|
//
|
|
// GaussianJunctionTree gjt(linearized);
|
|
// VectorValues deltaactual = gjt.optimize(&EliminateQR);
|
|
// Values actual = init.retract(deltaactual, ordering);
|
|
//
|
|
// GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate();
|
|
// VectorValues delta = optimize(gbn);
|
|
// Values expected = init.retract(delta, ordering);
|
|
//
|
|
// EXPECT(assert_equal(expected, actual));
|
|
//}
|
|
//
|
|
///* ************************************************************************* */
|
|
//TEST(GaussianJunctionTreeB, simpleMarginal) {
|
|
//
|
|
// typedef BayesTree<GaussianConditional> GaussianBayesTree;
|
|
//
|
|
// // Create a simple graph
|
|
// NonlinearFactorGraph fg;
|
|
// fg.add(PriorFactor<Pose2>(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0)));
|
|
// fg.add(BetweenFactor<Pose2>(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector3(10.0, 1.0, 1.0))));
|
|
//
|
|
// Values init;
|
|
// init.insert(X(0), Pose2());
|
|
// init.insert(X(1), Pose2(1.0, 0.0, 0.0));
|
|
//
|
|
// const Ordering ordering{X(1), X(0)};
|
|
//
|
|
// GaussianFactorGraph gfg = *fg.linearize(init, ordering);
|
|
//
|
|
// // Compute marginals with both sequential and multifrontal
|
|
// Matrix expected = GaussianSequentialSolver(gfg).marginalCovariance(1);
|
|
//
|
|
// Matrix actual1 = GaussianMultifrontalSolver(gfg).marginalCovariance(1);
|
|
//
|
|
// // Compute marginal directly from marginal factor
|
|
// GaussianFactor::shared_ptr marginalFactor = GaussianMultifrontalSolver(gfg).marginalFactor(1);
|
|
// JacobianFactor::shared_ptr marginalJacobian = std::dynamic_pointer_cast<JacobianFactor>(marginalFactor);
|
|
// Matrix actual2 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin()));
|
|
//
|
|
// // Compute marginal directly from BayesTree
|
|
// GaussianBayesTree gbt;
|
|
// gbt.insert(GaussianJunctionTree(gfg).eliminate(EliminateCholesky));
|
|
// marginalFactor = gbt.marginalFactor(1, EliminateCholesky);
|
|
// marginalJacobian = std::dynamic_pointer_cast<JacobianFactor>(marginalFactor);
|
|
// Matrix actual3 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin()));
|
|
//
|
|
// EXPECT(assert_equal(expected, actual1));
|
|
// EXPECT(assert_equal(expected, actual2));
|
|
// EXPECT(assert_equal(expected, actual3));
|
|
//}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
/* ************************************************************************* */
|
|
|