Cleaned up typedefs in FactorGraph.h (and removed FactorizationResult), and also made sure ::shared_ptr was never assumed to exist for a FACTOR template argument. Should it exist, ever?
Frank Dellaert
parent
f9db53fdb8
commit
80e2179a8d
|
|
@ -132,7 +132,7 @@ typename EliminationTree<FACTOR>::shared_ptr EliminationTree<FACTOR>::Create(
|
|||
|
||||
// Hang factors in right places
|
||||
tic(3, "hang factors");
|
||||
BOOST_FOREACH(const typename DERIVEDFACTOR::shared_ptr& derivedFactor, factorGraph) {
|
||||
BOOST_FOREACH(const typename boost::shared_ptr<DERIVEDFACTOR>& derivedFactor, factorGraph) {
|
||||
// Here we upwards-cast to the factor type of this EliminationTree. This
|
||||
// allows performing symbolic elimination on, for example, GaussianFactors.
|
||||
if(derivedFactor) {
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@ public:
|
|||
|
||||
typedef EliminationTree<FACTOR> This; ///< This class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
|
||||
typedef typename FACTOR::shared_ptr sharedFactor; ///< Shared pointer to a factor
|
||||
typedef typename boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
|
||||
typedef gtsam::BayesNet<typename FACTOR::ConditionalType> BayesNet; ///< The BayesNet corresponding to FACTOR
|
||||
|
||||
/** Typedef for an eliminate subroutine */
|
||||
|
|
|
|||
|
|
@ -36,6 +36,16 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
template<class CONDITIONAL>
|
||||
FactorGraph<FACTOR>::FactorGraph(const BayesNet<CONDITIONAL>& bayesNet) {
|
||||
factors_.reserve(bayesNet.size());
|
||||
BOOST_FOREACH(const typename CONDITIONAL::shared_ptr& cond, bayesNet) {
|
||||
this->push_back(cond->toFactor());
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
void FactorGraph<FACTOR>::print(const std::string& s) const {
|
||||
|
|
@ -50,7 +60,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
bool FactorGraph<FACTOR>::equals(const FactorGraph<FACTOR>& fg, double tol) const {
|
||||
bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
|
||||
/** check whether the two factor graphs have the same number of factors_ */
|
||||
if (factors_.size() != fg.size()) return false;
|
||||
|
||||
|
|
@ -111,20 +121,20 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Recursive function to add factors in cliques to vector of factors_io
|
||||
template<class FACTOR, class CONDITIONAL, class CLIQUE>
|
||||
void _FactorGraph_BayesTree_adder(
|
||||
std::vector<typename FactorGraph<FACTOR>::sharedFactor>& factors,
|
||||
std::vector<typename boost::shared_ptr<FACTOR> >& factors_io,
|
||||
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& clique) {
|
||||
|
||||
if(clique) {
|
||||
// Add factor from this clique
|
||||
factors.push_back((*clique)->toFactor());
|
||||
factors_io.push_back((*clique)->toFactor());
|
||||
|
||||
// Traverse children
|
||||
typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
|
||||
BOOST_FOREACH(const sharedClique& child, clique->children()) {
|
||||
_FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL,CLIQUE>(factors, child);
|
||||
}
|
||||
BOOST_FOREACH(const sharedClique& child, clique->children())
|
||||
_FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL,CLIQUE>(factors_io, child);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -21,14 +21,13 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
#include <boost/function.hpp>
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/FastMap.h>
|
||||
#include <gtsam/inference/BayesNet.h>
|
||||
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
#include <boost/function.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
|
|
@ -41,30 +40,28 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
|
|||
*/
|
||||
template<class FACTOR>
|
||||
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 FACTOR FactorType; ///< factor type
|
||||
typedef typename FACTOR::KeyType KeyType; ///< type of Keys we use to index factors with
|
||||
typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
|
||||
typedef boost::shared_ptr<typename FACTOR::ConditionalType> sharedConditional; ///< Shared pointer to a conditional
|
||||
|
||||
typedef FactorGraph<FACTOR> This; ///< Typedef for this class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer for this class
|
||||
typedef typename std::vector<sharedFactor>::iterator iterator;
|
||||
typedef typename std::vector<sharedFactor>::const_iterator const_iterator;
|
||||
|
||||
/** typedef for elimination result */
|
||||
typedef std::pair<
|
||||
boost::shared_ptr<typename FACTOR::ConditionalType>,
|
||||
typename FACTOR::shared_ptr> EliminationResult;
|
||||
/** typedef for elimination result */
|
||||
typedef std::pair<sharedConditional, sharedFactor> EliminationResult;
|
||||
|
||||
/** 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;
|
||||
/** typedef for an eliminate subroutine */
|
||||
typedef boost::function<EliminationResult(const This&, size_t)> Eliminate;
|
||||
|
||||
protected:
|
||||
|
||||
/** concept check */
|
||||
/** concept check, makes sure FACTOR defines print and equals */
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
|
||||
|
||||
/** Collection of factors */
|
||||
|
|
@ -82,7 +79,11 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
|
|||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
/** convert from Bayes net */
|
||||
/**
|
||||
* @brief Constructor from a Bayes net
|
||||
* @param bayesNet the Bayes net to convert, type CONDITIONAL must yield compatible factor
|
||||
* @return a factor graph with all the conditionals, as factors
|
||||
*/
|
||||
template<class CONDITIONAL>
|
||||
FactorGraph(const BayesNet<CONDITIONAL>& bayesNet);
|
||||
|
||||
|
|
@ -113,7 +114,7 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
|
|||
}
|
||||
|
||||
/** push back many factors */
|
||||
void push_back(const FactorGraph<FACTOR>& factors) {
|
||||
void push_back(const This& factors) {
|
||||
factors_.insert(end(), factors.begin(), factors.end());
|
||||
}
|
||||
|
||||
|
|
@ -123,9 +124,14 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
|
|||
factors_.insert(end(), firstFactor, lastFactor);
|
||||
}
|
||||
|
||||
/** push back many factors stored in a vector*/
|
||||
/**
|
||||
* @brief Add a vector of derived factors
|
||||
* @param factors to add
|
||||
*/
|
||||
template<typename DERIVEDFACTOR>
|
||||
void push_back(const std::vector<boost::shared_ptr<DERIVEDFACTOR> >& factors);
|
||||
void push_back(const std::vector<typename boost::shared_ptr<DERIVEDFACTOR> >& factors) {
|
||||
factors_.insert(end(), factors.begin(), factors.end());
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
|
|
@ -135,7 +141,7 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
|
|||
void print(const std::string& s = "FactorGraph") const;
|
||||
|
||||
/** Check equality */
|
||||
bool equals(const FactorGraph<FACTOR>& fg, double tol = 1e-9) const;
|
||||
bool equals(const This& fg, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
|
|
@ -252,31 +258,6 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
|
|||
template<class FACTORGRAPH>
|
||||
FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2);
|
||||
|
||||
/*
|
||||
* These functions are defined here because they are templated on an
|
||||
* additional parameter. Putting them in the -inl.h file would mean these
|
||||
* would have to be explicitly instantiated for any possible derived factor
|
||||
* type.
|
||||
*/
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
template<class CONDITIONAL>
|
||||
FactorGraph<FACTOR>::FactorGraph(const BayesNet<CONDITIONAL>& bayesNet) {
|
||||
factors_.reserve(bayesNet.size());
|
||||
BOOST_FOREACH(const typename CONDITIONAL::shared_ptr& cond, bayesNet) {
|
||||
this->push_back(cond->toFactor());
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
template<class DERIVEDFACTOR>
|
||||
void FactorGraph<FACTOR>::push_back(const std::vector<boost::shared_ptr<DERIVEDFACTOR> >& factors) {
|
||||
BOOST_FOREACH(const boost::shared_ptr<DERIVEDFACTOR>& factor, factors)
|
||||
this->push_back(factor);
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
#include <gtsam/inference/FactorGraph-inl.h>
|
||||
|
|
|
|||
|
|
@ -78,7 +78,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class F, class JT>
|
||||
typename F::shared_ptr GenericMultifrontalSolver<F, JT>::marginalFactor(
|
||||
typename boost::shared_ptr<F> GenericMultifrontalSolver<F, JT>::marginalFactor(
|
||||
Index j, Eliminate function) const {
|
||||
return eliminate(function)->marginalFactor(j, function);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -102,7 +102,7 @@ namespace gtsam {
|
|||
* Compute the marginal density over a variable, by integrating out
|
||||
* all of the other variables. This function returns the result as a factor.
|
||||
*/
|
||||
typename FACTOR::shared_ptr marginalFactor(Index j,
|
||||
typename boost::shared_ptr<FACTOR> marginalFactor(Index j,
|
||||
Eliminate function) const;
|
||||
|
||||
/// @}
|
||||
|
|
|
|||
|
|
@ -95,7 +95,7 @@ namespace gtsam {
|
|||
// Permute the factors - NOTE that this permutes the original factors, not
|
||||
// copies. Other parts of the code may hold shared_ptr's to these factors so
|
||||
// we must undo the permutation before returning.
|
||||
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *factors_)
|
||||
BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *factors_)
|
||||
if (factor) factor->permuteWithInverse(*permutationInverse);
|
||||
|
||||
// Eliminate all variables
|
||||
|
|
@ -103,7 +103,7 @@ namespace gtsam {
|
|||
bayesNet(EliminationTree<FACTOR>::Create(*factors_)->eliminate(function));
|
||||
|
||||
// Undo the permuation on the original factors and on the structure.
|
||||
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *factors_)
|
||||
BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *factors_)
|
||||
if (factor) factor->permuteWithInverse(*permutation);
|
||||
|
||||
// Take the joint marginal from the Bayes net.
|
||||
|
|
@ -116,7 +116,7 @@ namespace gtsam {
|
|||
joint->push_back((*(conditional++))->toFactor());
|
||||
|
||||
// Undo the permutation on the eliminated joint marginal factors
|
||||
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *joint)
|
||||
BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *joint)
|
||||
factor->permuteWithInverse(*permutation);
|
||||
|
||||
return joint;
|
||||
|
|
@ -124,7 +124,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
typename FACTOR::shared_ptr //
|
||||
typename boost::shared_ptr<FACTOR> //
|
||||
GenericSequentialSolver<FACTOR>::marginalFactor(Index j, Eliminate function) const {
|
||||
// Create a container for the one variable index
|
||||
std::vector<Index> js(1);
|
||||
|
|
|
|||
|
|
@ -130,7 +130,7 @@ namespace gtsam {
|
|||
* Compute the marginal Gaussian density over a variable, by integrating out
|
||||
* all of the other variables. This function returns the result as a factor.
|
||||
*/
|
||||
typename FACTOR::shared_ptr marginalFactor(Index j, Eliminate function) const;
|
||||
typename boost::shared_ptr<FACTOR> marginalFactor(Index j, Eliminate function) const;
|
||||
|
||||
/// @}
|
||||
|
||||
|
|
|
|||
|
|
@ -31,7 +31,8 @@ namespace inference {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<typename CONSTRAINED>
|
||||
Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) {
|
||||
Permutation::shared_ptr PermutationCOLAMD(
|
||||
const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) {
|
||||
|
||||
std::vector<int> cmember(variableIndex.size(), 0);
|
||||
|
||||
|
|
@ -55,10 +56,14 @@ inline Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIn
|
|||
|
||||
/* ************************************************************************* */
|
||||
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_) {
|
||||
std::pair<typename Graph::sharedConditional, Graph> 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);
|
||||
const VariableIndex& variableIndex =
|
||||
variableIndex_ ? *variableIndex_ : VariableIndex(factorGraph);
|
||||
|
||||
// First find the involved factors
|
||||
Graph involvedFactors;
|
||||
|
|
@ -108,13 +113,11 @@ typename Graph::FactorizationResult eliminate(const Graph& factorGraph, const st
|
|||
if(remainingFactor->size() != 0)
|
||||
remainingGraph.push_back(remainingFactor);
|
||||
|
||||
return typename Graph::FactorizationResult(conditional, remainingGraph);
|
||||
return std::make_pair(conditional, remainingGraph);
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
} // eliminate
|
||||
|
||||
} // namespace inference
|
||||
} // namespace gtsam
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -29,52 +29,60 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
namespace inference {
|
||||
namespace inference {
|
||||
|
||||
/**
|
||||
* Compute a permutation (variable ordering) using colamd
|
||||
*/
|
||||
Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex);
|
||||
/**
|
||||
* Compute a permutation (variable ordering) using colamd
|
||||
*/
|
||||
Permutation::shared_ptr PermutationCOLAMD(
|
||||
const VariableIndex& variableIndex);
|
||||
|
||||
/**
|
||||
* Compute a permutation (variable ordering) using constrained colamd
|
||||
*/
|
||||
template<typename CONSTRAINED>
|
||||
Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast);
|
||||
/**
|
||||
* Compute a permutation (variable ordering) using constrained colamd
|
||||
*/
|
||||
template<typename CONSTRAINED>
|
||||
Permutation::shared_ptr PermutationCOLAMD(
|
||||
const VariableIndex& variableIndex, const CONSTRAINED& constrainLast);
|
||||
|
||||
/**
|
||||
* Compute a CCOLAMD permutation using the constraint groups in cmember.
|
||||
*/
|
||||
Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember);
|
||||
/**
|
||||
* Compute a CCOLAMD permutation using the constraint groups in 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);
|
||||
/** 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>
|
||||
std::pair<typename Graph::sharedConditional, Graph> eliminate(
|
||||
const Graph& factorGraph,
|
||||
const std::vector<typename Graph::KeyType>& variables,
|
||||
const typename Graph::Eliminate& eliminateFcn,
|
||||
boost::optional<const VariableIndex&> variableIndex = boost::none);
|
||||
|
||||
/** 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);
|
||||
}
|
||||
/** 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>
|
||||
std::pair<typename Graph::sharedConditional, Graph> 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);
|
||||
}
|
||||
|
||||
}
|
||||
} // namespace inference
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -45,7 +45,7 @@ namespace gtsam {
|
|||
template<class FACTOR>
|
||||
boost::shared_ptr<Errors> gaussianErrors_(const FactorGraph<FACTOR>& fg, const VectorValues& x) {
|
||||
boost::shared_ptr<Errors> e(new Errors);
|
||||
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, fg) {
|
||||
BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, fg) {
|
||||
e->push_back(factor->error_vector(x));
|
||||
}
|
||||
return e;
|
||||
|
|
|
|||
|
|
@ -200,14 +200,16 @@ TEST( GaussianFactorGraph, eliminateOne_x1 )
|
|||
Ordering ordering; ordering += X(1),L(1),X(2);
|
||||
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
|
||||
|
||||
GaussianFactorGraph::FactorizationResult result = inference::eliminateOne(fg, 0, EliminateQR);
|
||||
GaussianConditional::shared_ptr conditional;
|
||||
GaussianFactorGraph remaining;
|
||||
boost::tie(conditional,remaining) = 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[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma);
|
||||
|
||||
EXPECT(assert_equal(expected,*result.first,tol));
|
||||
EXPECT(assert_equal(expected,*conditional,tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -247,9 +249,9 @@ TEST( GaussianFactorGraph, eliminateOne_x1_fast )
|
|||
{
|
||||
Ordering ordering; ordering += X(1),L(1),X(2);
|
||||
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
|
||||
GaussianFactorGraph::FactorizationResult result = inference::eliminateOne(fg, ordering[X(1)], EliminateQR);
|
||||
GaussianConditional::shared_ptr conditional = result.first;
|
||||
GaussianFactorGraph remaining = result.second;
|
||||
GaussianConditional::shared_ptr conditional;
|
||||
GaussianFactorGraph remaining;
|
||||
boost::tie(conditional,remaining) = inference::eliminateOne(fg, ordering[X(1)], EliminateQR);
|
||||
|
||||
// create expected Conditional Gaussian
|
||||
Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
|
||||
|
|
|
|||
Loading…
Reference in New Issue