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

Revived generic eliminate and eliminateOne functions for partial elimination

release/4.3a0
Richard Roberts 2012-03-03 20:23:03 +00:00
parent 8c6d21f97f
commit 79b4b74930
15 changed files with 310 additions and 166 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/inference/FactorGraph.h
View File

@ -43,6 +43,7 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
class FactorGraph {
public:
typedef FACTOR FactorType;
typedef typename FACTOR::KeyType KeyType;
typedef boost::shared_ptr<FactorGraph<FACTOR> > shared_ptr;
typedef typename boost::shared_ptr<FACTOR> sharedFactor;
typedef typename std::vector<sharedFactor>::iterator iterator;
@ -56,6 +57,11 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
/** typedef for an eliminate subroutine */
typedef boost::function<EliminationResult(const FactorGraph<FACTOR>&, size_t)> Eliminate;
/** Typedef for the result of factorization */
typedef std::pair<
boost::shared_ptr<typename FACTOR::ConditionalType>,
FactorGraph<FACTOR> > FactorizationResult;
protected:
/** concept check */

2
gtsam/inference/GenericSequentialSolver-inl.h
View File

@ -89,7 +89,7 @@ namespace gtsam {
const std::vector<Index>& js, Eliminate function) const {
// Compute a COLAMD permutation with the marginal variable constrained to the end.
Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(*structure_, js));
Permutation::shared_ptr permutation(inference::PermutationCOLAMD(*structure_, js));
Permutation::shared_ptr permutationInverse(permutation->inverse());
// Permute the factors - NOTE that this permutes the original factors, not

26
gtsam/inference/IndexConditional.cpp
View File

@ -31,12 +31,12 @@ namespace gtsam {
// Checks for uniqueness of keys
Base::assertInvariants();
#ifndef NDEBUG
// Check that separator keys are sorted
FastSet<Index> uniquesorted(beginFrontals(), endFrontals());
assert(uniquesorted.size() == nrFrontals() && std::equal(uniquesorted.begin(), uniquesorted.end(), beginFrontals()));
// Check that separator keys are less than parent keys
//BOOST_FOREACH(Index j, frontals()) {
// assert(find_if(beginParents(), endParents(), _1 < j) == endParents()); }
// Check that frontal keys are sorted
//FastSet<Index> uniquesorted(beginFrontals(), endFrontals());
//assert(uniquesorted.size() == nrFrontals() && std::equal(uniquesorted.begin(), uniquesorted.end(), beginFrontals()));
//// Check that separator keys are less than parent keys
////BOOST_FOREACH(Index j, frontals()) {
//// assert(find_if(beginParents(), endParents(), _1 < j) == endParents()); }
#endif
}
@ -60,13 +60,13 @@ namespace gtsam {
/* ************************************************************************* */
void IndexConditional::permuteWithInverse(const Permutation& inversePermutation) {
// The permutation may not move the separators into the frontals
#ifndef NDEBUG
BOOST_FOREACH(const KeyType frontal, this->frontals()) {
BOOST_FOREACH(const KeyType separator, this->parents()) {
assert(inversePermutation[frontal] < inversePermutation[separator]);
}
}
#endif
// #ifndef NDEBUG
// BOOST_FOREACH(const KeyType frontal, this->frontals()) {
// BOOST_FOREACH(const KeyType separator, this->parents()) {
// assert(inversePermutation[frontal] < inversePermutation[separator]);
// }
// }
// #endif
BOOST_FOREACH(Index& key, keys())
key = inversePermutation[key];
assertInvariants();

4
gtsam/inference/Permutation.h
View File

@ -26,8 +26,6 @@
namespace gtsam {
class Inference;
/**
* A permutation reorders variables, for example to reduce fill-in during
* elimination. To save computation, the permutation can be applied to
@ -162,8 +160,6 @@ protected:
void check(Index variable) const { assert(variable < rangeIndices_.size()); }
/// @}
friend class Inference;
};

2
gtsam/inference/VariableIndex.h
View File

@ -26,8 +26,6 @@
namespace gtsam {
class Inference;
/**
* The VariableIndex class computes and stores the block column structure of a
* factor graph. The factor graph stores a collection of factors, each of

119
gtsam/inference/inference-inl.h Normal file
View File

@ -0,0 +1,119 @@
/* ----------------------------------------------------------------------------
* 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 inference-inl.h
* @brief
* @author Richard Roberts
* @date Mar 3, 2012
*/
#pragma once
#include <algorithm>
// Only for Eclipse parser, inference-inl.h (this file) is included at the bottom of inference.h
#include <gtsam/inference/inference.h>
#include <gtsam/base/FastSet.h>
namespace gtsam {
namespace inference {
/* ************************************************************************* */
template<typename CONSTRAINED>
Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) {
std::vector<int> cmember(variableIndex.size(), 0);
// If at least some variables are not constrained to be last, constrain the
// ones that should be constrained.
if(constrainLast.size() < variableIndex.size()) {
BOOST_FOREACH(Index var, constrainLast) {
assert(var < variableIndex.size());
cmember[var] = 1;
}
}
return PermutationCOLAMD_(variableIndex, cmember);
}
/* ************************************************************************* */
inline Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex) {
std::vector<int> cmember(variableIndex.size(), 0);
return PermutationCOLAMD_(variableIndex, cmember);
}
/* ************************************************************************* */
template<class Graph>
typename Graph::FactorizationResult eliminate(const Graph& factorGraph, const std::vector<typename Graph::KeyType>& variables,
const typename Graph::Eliminate& eliminateFcn, boost::optional<const VariableIndex&> variableIndex_) {
const VariableIndex& variableIndex = variableIndex_ ? *variableIndex_ : VariableIndex(factorGraph);
// First find the involved factors
Graph involvedFactors;
Index highestInvolvedVariable = 0; // Largest index of the variables in the involved factors
// First get the indices of the involved factors, but uniquely in a set
FastSet<size_t> involvedFactorIndices;
BOOST_FOREACH(Index variable, variables) {
involvedFactorIndices.insert(variableIndex[variable].begin(), variableIndex[variable].end()); }
// Add the factors themselves to involvedFactors and update largest index
involvedFactors.reserve(involvedFactorIndices.size());
BOOST_FOREACH(size_t factorIndex, involvedFactorIndices) {
const typename Graph::sharedFactor factor = factorGraph[factorIndex];
involvedFactors.push_back(factor); // Add involved factor
highestInvolvedVariable = std::max( // Updated largest index
highestInvolvedVariable,
*std::max_element(factor->begin(), factor->end()));
}
// Now permute the variables to be eliminated to the front of the ordering
Permutation toFront = Permutation::PullToFront(variables, highestInvolvedVariable+1);
Permutation toFrontInverse = *toFront.inverse();
involvedFactors.permuteWithInverse(toFrontInverse);
// Eliminate into conditional and remaining factor
typename Graph::EliminationResult eliminated = eliminateFcn(involvedFactors, variables.size());
boost::shared_ptr<typename Graph::FactorType::ConditionalType> conditional = eliminated.first;
typename Graph::sharedFactor remainingFactor = eliminated.second;
// Undo the permutation
conditional->permuteWithInverse(toFront);
remainingFactor->permuteWithInverse(toFront);
// Build the remaining graph, without the removed factors
Graph remainingGraph;
remainingGraph.reserve(factorGraph.size() - involvedFactors.size() + 1);
FastSet<size_t>::const_iterator involvedFactorIndexIt = involvedFactorIndices.begin();
for(size_t i = 0; i < factorGraph.size(); ++i) {
if(involvedFactorIndexIt != involvedFactorIndices.end() && *involvedFactorIndexIt == i)
++ involvedFactorIndexIt;
else
remainingGraph.push_back(factorGraph[i]);
}
// Add remaining factor
remainingGraph.push_back(remainingFactor);
return typename Graph::FactorizationResult(conditional, remainingGraph);
}
}
}

12
gtsam/inference/inference.cpp
View File

@ -29,7 +29,9 @@ using namespace std;
namespace gtsam {
Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember) {
namespace inference {
Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember) {
size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
// Convert to compressed column major format colamd wants it in (== MATLAB format!)
int Alen = ccolamd_recommended(nEntries, nFactors, nVars); /* colamd arg 3: size of the array A */
@ -79,9 +81,9 @@ Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& varia
// Convert elimination ordering in p to an ordering
Permutation::shared_ptr permutation(new Permutation(nVars));
for (Index j = 0; j < nVars; j++) {
// if(p[j] == -1)
// permutation->operator[](j) = j;
// else
// if(p[j] == -1)
// permutation->operator[](j) = j;
// else
permutation->operator[](j) = p[j];
if(debug) cout << "COLAMD: " << j << "->" << p[j] << endl;
}
@ -91,3 +93,5 @@ Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& varia
}
}
}

73
gtsam/inference/inference.h
View File

@ -22,58 +22,59 @@
#include <gtsam/inference/Permutation.h>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>
#include <deque>
namespace gtsam {
class Inference {
private:
/* Static members only, private constructor */
Inference() {}
namespace inference {
public:
/**
/**
* Compute a permutation (variable ordering) using colamd
*/
static Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex);
Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex);
/**
/**
* Compute a permutation (variable ordering) using constrained colamd
*/
template<typename CONSTRAINED>
static Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast);
template<typename CONSTRAINED>
Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast);
/**
/**
* Compute a CCOLAMD permutation using the constraint groups in cmember.
*/
static Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember);
Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember);
};
/** Factor the factor graph into a conditional and a remaining factor graph.
* Given the factor graph \f$ f(X) \f$, and \c variables to factorize out
* \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where
* \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is
* a probability density or likelihood, the factorization produces a
* conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$.
*
* For efficiency, this function treats the variables to eliminate
* \c variables as fully-connected, so produces a dense (fully-connected)
* conditional on all of the variables in \c variables, instead of a sparse
* BayesNet. If the variables are not fully-connected, it is more efficient
* to sequentially factorize multiple times.
*/
template<class Graph>
typename Graph::FactorizationResult eliminate(const Graph& factorGraph, const std::vector<typename Graph::KeyType>& variables,
const typename Graph::Eliminate& eliminateFcn, boost::optional<const VariableIndex&> variableIndex = boost::none);
/* ************************************************************************* */
template<typename CONSTRAINED>
Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) {
/** Eliminate a single variable, by calling
* eliminate(const Graph&, const std::vector<typename Graph::KeyType>&, const typename Graph::Eliminate&, boost::optional<const VariableIndex&>)
*/
template<class Graph>
typename Graph::FactorizationResult eliminateOne(const Graph& factorGraph, typename Graph::KeyType variable,
const typename Graph::Eliminate& eliminateFcn, boost::optional<const VariableIndex&> variableIndex = boost::none) {
std::vector<size_t> variables(1, variable);
return eliminate(factorGraph, variables, eliminateFcn, variableIndex);
}
std::vector<int> cmember(variableIndex.size(), 0);
// If at least some variables are not constrained to be last, constrain the
// ones that should be constrained.
if(constrainLast.size() < variableIndex.size()) {
BOOST_FOREACH(Index var, constrainLast) {
assert(var < variableIndex.size());
cmember[var] = 1;
}
}
return PermutationCOLAMD_(variableIndex, cmember);
}
/* ************************************************************************* */
inline Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex) {
std::vector<int> cmember(variableIndex.size(), 0);
return PermutationCOLAMD_(variableIndex, cmember);
}
}
} // namespace gtsam
#include <gtsam/inference/inference-inl.h>

4
gtsam/inference/tests/testInference.cpp
View File

@ -25,7 +25,7 @@
using namespace gtsam;
/* ************************************************************************* */
TEST(Inference, UnobservedVariables) {
TEST(inference, UnobservedVariables) {
SymbolicFactorGraph sfg;
// Create a factor graph that skips some variables
@ -35,7 +35,7 @@ TEST(Inference, UnobservedVariables) {
VariableIndex variableIndex(sfg);
Permutation::shared_ptr colamd(Inference::PermutationCOLAMD(variableIndex));
Permutation::shared_ptr colamd(inference::PermutationCOLAMD(variableIndex));
}
/* ************************************************************************* */

2
gtsam/nonlinear/ISAM2-impl-inl.h
View File

@ -303,7 +303,7 @@ ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
}
}
}
Permutation::shared_ptr affectedColamd(Inference::PermutationCOLAMD_(affectedFactorsIndex, cmember));
Permutation::shared_ptr affectedColamd(inference::PermutationCOLAMD_(affectedFactorsIndex, cmember));
toc(3,"ccolamd");
tic(4,"ccolamd permutations");
Permutation::shared_ptr affectedColamdInverse(affectedColamd->inverse());

2
gtsam/nonlinear/ISAM2-inl.h
View File

@ -238,7 +238,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
if(theta_.size() > constrainedKeysSet.size()) {
BOOST_FOREACH(Index var, constrainedKeysSet) { cmember[var] = 1; }
}
Permutation::shared_ptr colamd(Inference::PermutationCOLAMD_(variableIndex_, cmember));
Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember));
Permutation::shared_ptr colamdInverse(colamd->inverse());
toc(1,"CCOLAMD");

2
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -78,7 +78,7 @@ namespace gtsam {
"orderingCOLAMD: some variables in the graph are not constrained!");
// Compute a fill-reducing ordering with COLAMD
Permutation::shared_ptr colamdPerm(Inference::PermutationCOLAMD(
Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMD(
variableIndex));
// Permute the Ordering with the COLAMD ordering

200
tests/testGaussianFactorGraph.cpp
View File

@ -192,96 +192,116 @@ TEST( GaussianFactorGraph, equals ) {
// EXPECT(assert_equal(expected,*actual));
//}
///* ************************************************************************* */
//TEST( GaussianFactorGraph, eliminateOne_x1 )
//{
// Ordering ordering; ordering += kx(1),kl(1),kx(2);
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
// GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1);
//
// // create expected Conditional Gaussian
// Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
// Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
// GaussianConditional expected(ordering[kx(1)],15*d,R11,ordering[kl(1)],S12,ordering[kx(2)],S13,sigma);
//
// EXPECT(assert_equal(expected,*actual,tol));
//}
//
///* ************************************************************************* */
//TEST( GaussianFactorGraph, eliminateOne_x2 )
//{
// Ordering ordering; ordering += kx(2),kl(1),kx(1);
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
// GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1);
//
// // create expected Conditional Gaussian
// double sig = 0.0894427;
// Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
// Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
// GaussianConditional expected(ordering[kx(2)],d,R11,ordering[kl(1)],S12,ordering[kx(1)],S13,sigma);
//
// EXPECT(assert_equal(expected,*actual,tol));
//}
//
///* ************************************************************************* */
//TEST( GaussianFactorGraph, eliminateOne_l1 )
//{
// Ordering ordering; ordering += kl(1),kx(1),kx(2);
// GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
// GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1);
//
// // create expected Conditional Gaussian
// double sig = sqrt(2)/10.;
// Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
// Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
// GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma);
//
// EXPECT(assert_equal(expected,*actual,tol));
//}
/* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_x1 )
{
Ordering ordering; ordering += kx(1),kl(1),kx(2);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
///* ************************************************************************* */
//TEST( GaussianFactorGraph, eliminateOne_x1_fast )
//{
// GaussianFactorGraph fg = createGaussianFactorGraph();
// GaussianConditional::shared_ptr actual = fg.eliminateOne(kx(1), false);
//
// // create expected Conditional Gaussian
// Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
// Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
// GaussianConditional expected(kx(1),15*d,R11,kl(1),S12,kx(2),S13,sigma);
//
// EXPECT(assert_equal(expected,*actual,tol));
//}
//
///* ************************************************************************* */
//TEST( GaussianFactorGraph, eliminateOne_x2_fast )
//{
// GaussianFactorGraph fg = createGaussianFactorGraph();
// GaussianConditional::shared_ptr actual = fg.eliminateOne(kx(2), false);
//
// // create expected Conditional Gaussian
// double sig = 0.0894427;
// Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
// Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
// GaussianConditional expected(kx(2),d,R11,kl(1),S12,kx(1),S13,sigma);
//
// EXPECT(assert_equal(expected,*actual,tol));
//}
//
///* ************************************************************************* */
//TEST( GaussianFactorGraph, eliminateOne_l1_fast )
//{
// GaussianFactorGraph fg = createGaussianFactorGraph();
// GaussianConditional::shared_ptr actual = fg.eliminateOne(kl(1), false);
//
// // create expected Conditional Gaussian
// double sig = sqrt(2)/10.;
// Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
// Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
// GaussianConditional expected(kl(1),d,R11,kx(1),S12,kx(2),S13,sigma);
//
// EXPECT(assert_equal(expected,*actual,tol));
//}
GaussianFactorGraph::FactorizationResult result = inference::eliminateOne(fg, 0, EliminateQR);
// create expected Conditional Gaussian
Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
GaussianConditional expected(ordering[kx(1)],15*d,R11,ordering[kl(1)],S12,ordering[kx(2)],S13,sigma);
EXPECT(assert_equal(expected,*result.first,tol));
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_x2 )
{
Ordering ordering; ordering += kx(2),kl(1),kx(1);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first;
// create expected Conditional Gaussian
double sig = 0.0894427;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
GaussianConditional expected(ordering[kx(2)],d,R11,ordering[kl(1)],S12,ordering[kx(1)],S13,sigma);
EXPECT(assert_equal(expected,*actual,tol));
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_l1 )
{
Ordering ordering; ordering += kl(1),kx(1),kx(2);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first;
// create expected Conditional Gaussian
double sig = sqrt(2)/10.;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma);
EXPECT(assert_equal(expected,*actual,tol));
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_x1_fast )
{
Ordering ordering; ordering += kx(1),kl(1),kx(2);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianFactorGraph::FactorizationResult result = inference::eliminateOne(fg, ordering[kx(1)], EliminateQR);
GaussianConditional::shared_ptr conditional = result.first;
GaussianFactorGraph remaining = result.second;
// create expected Conditional Gaussian
Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
GaussianConditional expected(ordering[kx(1)],15*d,R11,ordering[kl(1)],S12,ordering[kx(2)],S13,sigma);
// Create expected remaining new factor
JacobianFactor expectedFactor(1, Matrix_(4,2,
4.714045207910318, 0.,
0., 4.714045207910318,
0., 0.,
0., 0.),
2, Matrix_(4,2,
-2.357022603955159, 0.,
0., -2.357022603955159,
7.071067811865475, 0.,
0., 7.071067811865475),
Vector_(4, -0.707106781186547, 0.942809041582063, 0.707106781186547, -1.414213562373094), sharedUnit(4));
EXPECT(assert_equal(expected,*conditional,tol));
EXPECT(assert_equal((const GaussianFactor&)expectedFactor,*remaining.back(),tol));
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_x2_fast )
{
Ordering ordering; ordering += kx(1),kl(1),kx(2);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[kx(2)], EliminateQR).first;
// create expected Conditional Gaussian
double sig = 0.0894427;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
GaussianConditional expected(ordering[kx(2)],d,R11,ordering[kx(1)],S13,ordering[kl(1)],S12,sigma);
EXPECT(assert_equal(expected,*actual,tol));
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_l1_fast )
{
Ordering ordering; ordering += kx(1),kl(1),kx(2);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[kl(1)], EliminateQR).first;
// create expected Conditional Gaussian
double sig = sqrt(2)/10.;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma);
EXPECT(assert_equal(expected,*actual,tol));
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateAll )
@ -439,7 +459,7 @@ TEST( GaussianFactorGraph, getOrdering)
{
Ordering original; original += kl(1),kx(1),kx(2);
FactorGraph<IndexFactor> symbolic(createGaussianFactorGraph(original));
Permutation perm(*Inference::PermutationCOLAMD(VariableIndex(symbolic)));
Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic)));
Ordering actual = original; actual.permuteWithInverse((*perm.inverse()));
Ordering expected; expected += kl(1),kx(2),kx(1);
EXPECT(assert_equal(expected,actual));

4
tests/testGaussianISAM.cpp
View File

@ -85,7 +85,7 @@ TEST_UNSAFE( ISAM, iSAM_smoother )
// Ordering ord; ord += kx(4),kx(3),kx(2),kx(1);
// GaussianFactorGraph factors1;
// for (int i=0;i<7;i++) factors1.push_back(smoother[i]);
// GaussianISAM actual(*Inference::Eliminate(factors1));
// GaussianISAM actual(*inference::Eliminate(factors1));
//
// // run iSAM with remaining factors
// GaussianFactorGraph factors2;
@ -298,7 +298,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts )
// varIndex.permute(toFront);
// BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
// factor->permuteWithInverse(toFrontInverse); }
// GaussianBayesNet actual = *Inference::EliminateUntil(marginal, C3->keys().size(), varIndex);
// GaussianBayesNet actual = *inference::EliminateUntil(marginal, C3->keys().size(), varIndex);
// actual.permuteWithInverse(toFront);
// EXPECT(assert_equal(expected,actual,tol));
//}

4
tests/testInference.cpp
View File

@ -30,7 +30,7 @@ using namespace gtsam;
/* ************************************************************************* */
/* ************************************************************************* */
TEST( Inference, marginals )
TEST( inference, marginals )
{
using namespace example;
// create and marginalize a small Bayes net on "x"
@ -45,7 +45,7 @@ TEST( Inference, marginals )
}
/* ************************************************************************* */
TEST( Inference, marginals2)
TEST( inference, marginals2)
{
planarSLAM::Graph fg;
SharedDiagonal poseModel(sharedSigma(3, 0.1));