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

Removed obsolete Ordered classes and unit tests that have already been converted

release/4.3a0
Richard Roberts 2013-08-02 22:09:40 +00:00
parent 682eddf3ef
commit be0b27a003
88 changed files with 0 additions and 18562 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

192
gtsam/inference/BayesNetOrdered-inl.h
View File

@ -1,192 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 BayesNet-inl.h
* @brief Bayes net template definitions
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/base/Testable.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/format.hpp>
#include <boost/assign/std/vector.hpp> // for +=
using boost::assign::operator+=;
#include <algorithm>
#include <iostream>
#include <fstream>
namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNetOrdered<CONDITIONAL>::print(const std::string& s,
const IndexFormatter& formatter) const {
std::cout << s;
BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_)
conditional->print("Conditional", formatter);
}
/* ************************************************************************* */
template<class CONDITIONAL>
bool BayesNetOrdered<CONDITIONAL>::equals(const BayesNetOrdered& cbn, double tol) const {
if (size() != cbn.size())
return false;
return std::equal(conditionals_.begin(), conditionals_.end(),
cbn.conditionals_.begin(), equals_star<CONDITIONAL>(tol));
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNetOrdered<CONDITIONAL>::printStats(const std::string& s) const {
const size_t n = conditionals_.size();
size_t max_size = 0;
size_t total = 0;
BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_) {
max_size = std::max(max_size, conditional->size());
total += conditional->size();
}
std::cout << s
<< "maximum conditional size = " << max_size << std::endl
<< "average conditional size = " << total / n << std::endl
<< "density = " << 100.0 * total / (double) (n*(n+1)/2) << " %" << std::endl;
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNetOrdered<CONDITIONAL>::saveGraph(const std::string &s,
const IndexFormatter& indexFormatter) const {
std::ofstream of(s.c_str());
of << "digraph G{\n";
BOOST_REVERSE_FOREACH(typename CONDITIONAL::shared_ptr conditional, conditionals_) {
typename CONDITIONAL::Frontals frontals = conditional->frontals();
Index me = frontals.front();
typename CONDITIONAL::Parents parents = conditional->parents();
BOOST_FOREACH(Index p, parents)
of << p << "->" << me << std::endl;
}
of << "}";
of.close();
}
/* ************************************************************************* */
template<class CONDITIONAL>
typename BayesNetOrdered<CONDITIONAL>::const_iterator BayesNetOrdered<CONDITIONAL>::find(
Index key) const {
for (const_iterator it = begin(); it != end(); ++it)
if (std::find((*it)->beginFrontals(), (*it)->endFrontals(), key)
!= (*it)->endFrontals())
return it;
return end();
}
/* ************************************************************************* */
template<class CONDITIONAL>
typename BayesNetOrdered<CONDITIONAL>::iterator BayesNetOrdered<CONDITIONAL>::find(
Index key) {
for (iterator it = begin(); it != end(); ++it)
if (std::find((*it)->beginFrontals(), (*it)->endFrontals(), key)
!= (*it)->endFrontals())
return it;
return end();
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNetOrdered<CONDITIONAL>::permuteWithInverse(
const Permutation& inversePermutation) {
BOOST_FOREACH(sharedConditional conditional, conditionals_) {
conditional->permuteWithInverse(inversePermutation);
}
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNetOrdered<CONDITIONAL>::push_back(const BayesNetOrdered<CONDITIONAL>& bn) {
BOOST_FOREACH(sharedConditional conditional,bn.conditionals_)
push_back(conditional);
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNetOrdered<CONDITIONAL>::push_front(const BayesNetOrdered<CONDITIONAL>& bn) {
BOOST_FOREACH(sharedConditional conditional,bn.conditionals_)
push_front(conditional);
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNetOrdered<CONDITIONAL>::popLeaf(iterator conditional) {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
BOOST_FOREACH(typename CONDITIONAL::shared_ptr checkConditional, conditionals_) {
BOOST_FOREACH(Index key, (*conditional)->frontals()) {
if(std::find(checkConditional->beginParents(), checkConditional->endParents(), key) != checkConditional->endParents())
throw std::invalid_argument(
"Debug mode exception: in BayesNet::popLeaf, the requested conditional is not a leaf.");
}
}
#endif
conditionals_.erase(conditional);
}
/* ************************************************************************* */
template<class CONDITIONAL>
FastList<Index> BayesNetOrdered<CONDITIONAL>::ordering() const {
FastList<Index> ord;
BOOST_FOREACH(sharedConditional conditional,conditionals_)
ord.insert(ord.begin(), conditional->beginFrontals(),
conditional->endFrontals());
return ord;
}
// /* ************************************************************************* */
// template<class CONDITIONAL>
// void BayesNet<CONDITIONAL>::saveGraph(const std::string &s) const {
// ofstream of(s.c_str());
// of<< "digraph G{\n";
// BOOST_FOREACH(const_sharedConditional conditional,conditionals_) {
// Index child = conditional->key();
// BOOST_FOREACH(Index parent, conditional->parents()) {
// of << parent << "->" << child << endl;
// }
// }
// of<<"}";
// of.close();
// }
//
/* ************************************************************************* */
template<class CONDITIONAL>
typename BayesNetOrdered<CONDITIONAL>::sharedConditional BayesNetOrdered<CONDITIONAL>::operator[](
Index key) const {
BOOST_FOREACH(typename CONDITIONAL::shared_ptr conditional, conditionals_) {
typename CONDITIONAL::const_iterator it = std::find(
conditional->beginFrontals(), conditional->endFrontals(), key);
if (it != conditional->endFrontals()) {
return conditional;
}
}
throw(std::invalid_argument(
(boost::format("BayesNet::operator['%1%']: not found") % key).str()));
}
/* ************************************************************************* */
} // namespace gtsam

244
gtsam/inference/BayesNetOrdered.h
View File

@ -1,244 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 BayesNet.h
* @brief Bayes network
* @author Frank Dellaert
*/
// \callgraph
#pragma once
#include <list>
#include <boost/shared_ptr.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/assign/list_inserter.hpp>
#include <boost/lexical_cast.hpp>
#include <gtsam/global_includes.h>
#include <gtsam/base/FastList.h>
#include <gtsam/inference/PermutationOrdered.h>
namespace gtsam {
/**
* A BayesNet is a list of conditionals, stored in elimination order, i.e.
* leaves first, parents last. GaussianBayesNet and SymbolicBayesNet are
* defined as typedefs of this class, using GaussianConditional and
* IndexConditional as the CONDITIONAL template argument.
*
* todo: Symbolic using Index is a misnomer.
* todo: how to handle Bayes nets with an optimize function? Currently using global functions.
* \nosubgrouping
*/
template<class CONDITIONAL>
class BayesNetOrdered {
public:
typedef typename boost::shared_ptr<BayesNetOrdered<CONDITIONAL> > shared_ptr;
/** We store shared pointers to Conditional densities */
typedef typename CONDITIONAL::KeyType KeyType;
typedef typename boost::shared_ptr<CONDITIONAL> sharedConditional;
typedef typename boost::shared_ptr<const CONDITIONAL> const_sharedConditional;
typedef typename std::list<sharedConditional> Conditionals;
typedef typename Conditionals::iterator iterator;
typedef typename Conditionals::reverse_iterator reverse_iterator;
typedef typename Conditionals::const_iterator const_iterator;
typedef typename Conditionals::const_reverse_iterator const_reverse_iterator;
protected:
/**
* Conditional densities are stored in reverse topological sort order (i.e., leaves first,
* parents last), which corresponds to the elimination ordering if so obtained,
* and is consistent with the column (block) ordering of an upper triangular matrix.
*/
Conditionals conditionals_;
public:
/// @name Standard Constructors
/// @{
/** Default constructor as an empty BayesNet */
BayesNetOrdered() {};
/** convert from a derived type */
template<class DERIVEDCONDITIONAL>
BayesNetOrdered(const BayesNetOrdered<DERIVEDCONDITIONAL>& bn) {
conditionals_.assign(bn.begin(), bn.end());
}
/** BayesNet with 1 conditional */
explicit BayesNetOrdered(const sharedConditional& conditional) { push_back(conditional); }
/// @}
/// @name Testable
/// @{
/** print */
void print(const std::string& s = "",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** check equality */
bool equals(const BayesNetOrdered& other, double tol = 1e-9) const;
/// @}
/// @name Standard Interface
/// @{
/** size is the number of nodes */
size_t size() const {
return conditionals_.size();
}
/** @return true if there are no conditionals/nodes */
bool empty() const {
return conditionals_.empty();
}
/** print statistics */
void printStats(const std::string& s = "") const;
/** save dot graph */
void saveGraph(const std::string &s, const IndexFormatter& indexFormatter =
DefaultIndexFormatter) const;
/** return keys in reverse topological sort order, i.e., elimination order */
FastList<Index> ordering() const;
/** SLOW O(n) random access to Conditional by key */
sharedConditional operator[](Index key) const;
/** return last node in ordering */
boost::shared_ptr<const CONDITIONAL> front() const { return conditionals_.front(); }
/** return last node in ordering */
boost::shared_ptr<const CONDITIONAL> back() const { return conditionals_.back(); }
/** return iterators. FD: breaks encapsulation? */
const_iterator begin() const {return conditionals_.begin();} ///<TODO: comment
const_iterator end() const {return conditionals_.end();} ///<TODO: comment
const_reverse_iterator rbegin() const {return conditionals_.rbegin();} ///<TODO: comment
const_reverse_iterator rend() const {return conditionals_.rend();} ///<TODO: comment
/** Find an iterator pointing to the conditional where the specified key
* appears as a frontal variable, or end() if no conditional contains this
* key. Running time is approximately \f$ O(n) \f$ in the number of
* conditionals in the BayesNet.
* @param key The index to find in the frontal variables of a conditional.
*/
const_iterator find(Index key) const;
/// @}
/// @name Advanced Interface
/// @{
/**
* Remove any leaf conditional. The conditional to remove is specified by
* iterator. To find the iterator pointing to the conditional containing a
* particular key, use find(), which has \f$ O(n) \f$ complexity. The
* popLeaf function by itself has \f$ O(1) \f$ complexity.
*
* If gtsam is compiled with GTSAM_EXTRA_CONSISTENCY_CHECKS defined, this
* function will check that the node is indeed a leaf, but otherwise will
* not check, because the check has \f$ O(n^2) \f$ complexity.
*
* Example 1:
\code
// Remove a leaf node with a known conditional
GaussianBayesNet gbn = ...
GaussianBayesNet::iterator leafConditional = ...
gbn.popLeaf(leafConditional);
\endcode
* Example 2:
\code
// Remove the leaf node containing variable index 14
GaussianBayesNet gbn = ...
gbn.popLeaf(gbn.find(14));
\endcode
* @param conditional The iterator pointing to the leaf conditional to remove
*/
void popLeaf(iterator conditional);
/** return last node in ordering */
sharedConditional& front() { return conditionals_.front(); }
/** return last node in ordering */
sharedConditional& back() { return conditionals_.back(); }
/** Find an iterator pointing to the conditional where the specified key
* appears as a frontal variable, or end() if no conditional contains this
* key. Running time is approximately \f$ O(n) \f$ in the number of
* conditionals in the BayesNet.
* @param key The index to find in the frontal variables of a conditional.
*/
iterator find(Index key);
/** push_back: use reverse topological sort (i.e. parents last / elimination order) */
inline void push_back(const sharedConditional& conditional) {
conditionals_.push_back(conditional);
}
/** push_front: use topological sort (i.e. parents first / reverse elimination order) */
inline void push_front(const sharedConditional& conditional) {
conditionals_.push_front(conditional);
}
/// push_back an entire Bayes net
void push_back(const BayesNetOrdered<CONDITIONAL>& bn);
/// push_front an entire Bayes net
void push_front(const BayesNetOrdered<CONDITIONAL>& bn);
/** += syntax for push_back, e.g. bayesNet += c1, c2, c3
* @param conditional The conditional to add to the back of the BayesNet
*/
boost::assign::list_inserter<boost::assign_detail::call_push_back<BayesNetOrdered<CONDITIONAL> >, sharedConditional>
operator+=(const sharedConditional& conditional) {
return boost::assign::make_list_inserter(boost::assign_detail::call_push_back<BayesNetOrdered<CONDITIONAL> >(*this))(conditional); }
/**
* pop_front: remove node at the bottom, used in marginalization
* For example P(ABC)=P(A|BC)P(B|C)P(C) becomes P(BC)=P(B|C)P(C)
*/
void pop_front() {conditionals_.pop_front();}
/** Permute the variables in the BayesNet */
void permuteWithInverse(const Permutation& inversePermutation);
iterator begin() {return conditionals_.begin();} ///<TODO: comment
iterator end() {return conditionals_.end();} ///<TODO: comment
reverse_iterator rbegin() {return conditionals_.rbegin();} ///<TODO: comment
reverse_iterator rend() {return conditionals_.rend();} ///<TODO: comment
/** saves the bayes to a text file in GraphViz format */
// void saveGraph(const std::string& s) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(conditionals_);
}
/// @}
}; // BayesNet
} // namespace gtsam
#include <gtsam/inference/BayesNetOrdered-inl.h>

301
gtsam/inference/BayesTreeCliqueBaseOrdered-inl.h
View File

@ -1,301 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 BayesTreeCliqueBase-inl.h
* @brief Base class for cliques of a BayesTree
* @author Richard Roberts and Frank Dellaert
*/
#pragma once
#include <gtsam/inference/GenericSequentialSolver.h>
#include <boost/foreach.hpp>
namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::assertInvariants() const {
}
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
std::vector<Index> BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::separator_setminus_B(
derived_ptr B) const {
sharedConditional p_F_S = this->conditional();
std::vector<Index> &indicesB = B->conditional()->keys();
std::vector<Index> S_setminus_B;
std::set_difference(p_F_S->beginParents(), p_F_S->endParents(), //
indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
return S_setminus_B;
}
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
std::vector<Index> BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::shortcut_indices(
derived_ptr B, const FactorGraphOrdered<FactorType>& p_Cp_B) const
{
gttic(shortcut_indices);
std::set<Index> allKeys = p_Cp_B.keys();
std::vector<Index> &indicesB = B->conditional()->keys();
std::vector<Index> S_setminus_B = separator_setminus_B(B); // TODO, get as argument?
std::vector<Index> keep;
// keep = S\B intersect allKeys
std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), //
allKeys.begin(), allKeys.end(), back_inserter(keep));
// keep += B intersect allKeys
std::set_intersection(indicesB.begin(), indicesB.end(), //
allKeys.begin(), allKeys.end(), back_inserter(keep));
// BOOST_FOREACH(Index j, keep) std::cout << j << " "; std::cout << std::endl;
return keep;
}
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::BayesTreeCliqueBaseOrdered(
const sharedConditional& conditional) :
conditional_(conditional) {
assertInvariants();
}
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::BayesTreeCliqueBaseOrdered(
const std::pair<sharedConditional,
boost::shared_ptr<typename ConditionalType::FactorType> >& result) :
conditional_(result.first) {
assertInvariants();
}
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::print(const std::string& s,
const IndexFormatter& indexFormatter) const {
conditional_->print(s, indexFormatter);
}
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
size_t BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::treeSize() const {
size_t size = 1;
BOOST_FOREACH(const derived_ptr& child, children_)
size += child->treeSize();
return size;
}
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
size_t BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::numCachedSeparatorMarginals() const {
if (!cachedSeparatorMarginal_)
return 0;
size_t subtree_count = 1;
BOOST_FOREACH(const derived_ptr& child, children_)
subtree_count += child->numCachedSeparatorMarginals();
return subtree_count;
}
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::printTree(
const std::string& indent, const IndexFormatter& indexFormatter) const {
asDerived(this)->print(indent, indexFormatter);
BOOST_FOREACH(const derived_ptr& child, children_)
child->printTree(indent + " ", indexFormatter);
}
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::permuteWithInverse(
const Permutation& inversePermutation) {
conditional_->permuteWithInverse(inversePermutation);
BOOST_FOREACH(const derived_ptr& child, children_) {
child->permuteWithInverse(inversePermutation);
}
assertInvariants();
}
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
bool BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::reduceSeparatorWithInverse(
const internal::Reduction& inverseReduction)
{
bool changed = conditional_->reduceSeparatorWithInverse(inverseReduction);
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
if(!changed) {
BOOST_FOREACH(const derived_ptr& child, children_) {
assert(child->reduceSeparatorWithInverse(inverseReduction) == false); }
}
#endif
if(changed) {
BOOST_FOREACH(const derived_ptr& child, children_) {
(void) child->reduceSeparatorWithInverse(inverseReduction); }
}
assertInvariants();
return changed;
}
/* ************************************************************************* */
// The shortcut density is a conditional P(S|R) of the separator of this
// clique on the root. We can compute it recursively from the parent shortcut
// P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
BayesNetOrdered<CONDITIONAL> BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::shortcut(
derived_ptr B, Eliminate function) const
{
gttic(BayesTreeCliqueBase_shortcut);
// We only calculate the shortcut when this clique is not B
// and when the S\B is not empty
std::vector<Index> S_setminus_B = separator_setminus_B(B);
if (B.get() != this && !S_setminus_B.empty()) {
// Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
derived_ptr parent(parent_.lock());
gttoc(BayesTreeCliqueBase_shortcut);
FactorGraphOrdered<FactorType> p_Cp_B(parent->shortcut(B, function)); // P(Sp||B)
gttic(BayesTreeCliqueBase_shortcut);
p_Cp_B.push_back(parent->conditional()->toFactor()); // P(Fp|Sp)
// Determine the variables we want to keepSet, S union B
std::vector<Index> keep = shortcut_indices(B, p_Cp_B);
// Reduce the variable indices to start at zero
gttic(Reduce);
const Permutation reduction = internal::createReducingPermutation(p_Cp_B.keys());
internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_Cp_B) {
if(factor) factor->reduceWithInverse(inverseReduction); }
inverseReduction.applyInverse(keep);
gttoc(Reduce);
// Create solver that will marginalize for us
GenericSequentialSolver<FactorType> solver(p_Cp_B);
// Finally, we only want to have S\B variables in the Bayes net, so
size_t nrFrontals = S_setminus_B.size();
BayesNetOrdered<CONDITIONAL> result = *solver.conditionalBayesNet(keep, nrFrontals, function);
// Undo the reduction
gttic(Undo_Reduce);
BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, p_Cp_B) {
if (factor) factor->permuteWithInverse(reduction); }
result.permuteWithInverse(reduction);
gttoc(Undo_Reduce);
assertInvariants();
return result;
} else {
return BayesNetOrdered<CONDITIONAL>();
}
}
/* ************************************************************************* */
// separator marginal, uses separator marginal of parent recursively
// P(C) = P(F|S) P(S)
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
FactorGraphOrdered<typename BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::FactorType> BayesTreeCliqueBaseOrdered<
DERIVED, CONDITIONAL>::separatorMarginal(derived_ptr R, Eliminate function) const
{
gttic(BayesTreeCliqueBase_separatorMarginal);
// Check if the Separator marginal was already calculated
if (!cachedSeparatorMarginal_) {
gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
// If this is the root, there is no separator
if (R.get() == this) {
// we are root, return empty
FactorGraphOrdered<FactorType> empty;
cachedSeparatorMarginal_ = empty;
} else {
// Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp)
// initialize P(Cp) with the parent separator marginal
derived_ptr parent(parent_.lock());
gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss); // Flatten recursion in timing outline
gttoc(BayesTreeCliqueBase_separatorMarginal);
FactorGraphOrdered<FactorType> p_Cp(parent->separatorMarginal(R, function)); // P(Sp)
gttic(BayesTreeCliqueBase_separatorMarginal);
gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
// now add the parent conditional
p_Cp.push_back(parent->conditional()->toFactor()); // P(Fp|Sp)
// Reduce the variable indices to start at zero
gttic(Reduce);
const Permutation reduction = internal::createReducingPermutation(p_Cp.keys());
internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_Cp) {
if(factor) factor->reduceWithInverse(inverseReduction); }
// The variables we want to keepSet are exactly the ones in S
sharedConditional p_F_S = this->conditional();
std::vector<Index> indicesS(p_F_S->beginParents(), p_F_S->endParents());
inverseReduction.applyInverse(indicesS);
gttoc(Reduce);
// Create solver that will marginalize for us
GenericSequentialSolver<FactorType> solver(p_Cp);
cachedSeparatorMarginal_ = *(solver.jointBayesNet(indicesS, function));
// Undo the reduction
gttic(Undo_Reduce);
BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, p_Cp) {
if (factor) factor->permuteWithInverse(reduction); }
BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, *cachedSeparatorMarginal_) {
if (factor) factor->permuteWithInverse(reduction); }
gttoc(Undo_Reduce);
}
} else {
gttic(BayesTreeCliqueBase_separatorMarginal_cachehit);
}
// return the shortcut P(S||B)
return *cachedSeparatorMarginal_; // return the cached version
}
/* ************************************************************************* */
// marginal2, uses separator marginal of parent recursively
// P(C) = P(F|S) P(S)
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
FactorGraphOrdered<typename BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::FactorType> BayesTreeCliqueBaseOrdered<
DERIVED, CONDITIONAL>::marginal2(derived_ptr R, Eliminate function) const
{
gttic(BayesTreeCliqueBase_marginal2);
// initialize with separator marginal P(S)
FactorGraphOrdered<FactorType> p_C(this->separatorMarginal(R, function));
// add the conditional P(F|S)
p_C.push_back(this->conditional()->toFactor());
return p_C;
}
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>::deleteCachedShortcuts() {
// When a shortcut is requested, all of the shortcuts between it and the
// root are also generated. So, if this clique's cached shortcut is set,
// recursively call over all child cliques. Otherwise, it is unnecessary.
if (cachedSeparatorMarginal_) {
BOOST_FOREACH(derived_ptr& child, children_) {
child->deleteCachedShortcuts();
}
//Delete CachedShortcut for this clique
cachedSeparatorMarginal_ = boost::none;
}
}
}

273
gtsam/inference/BayesTreeCliqueBaseOrdered.h
View File

@ -1,273 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 BayesTreeCliqueBase.h
* @brief Base class for cliques of a BayesTree
* @author Richard Roberts and Frank Dellaert
*/
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/weak_ptr.hpp>
#include <gtsam/global_includes.h>
#include <gtsam/inference/FactorGraphOrdered.h>
#include <gtsam/inference/BayesNetOrdered.h>
namespace gtsam {
template<class CONDITIONAL, class CLIQUE> class BayesTreeOrdered;
}
namespace gtsam {
/**
* This is the base class for BayesTree cliques. The default and standard
* derived type is BayesTreeClique, but some algorithms, like iSAM2, use a
* different clique type in order to store extra data along with the clique.
*
* This class is templated on the derived class (i.e. the curiously recursive
* template pattern). The advantage of this over using virtual classes is
* that it avoids the need for casting to get the derived type. This is
* possible because all cliques in a BayesTree are the same type - if they
* were not then we'd need a virtual class.
*
* @tparam DERIVED The derived clique type.
* @tparam CONDITIONAL The conditional type.
* \nosubgrouping
*/
template<class DERIVED, class CONDITIONAL>
struct BayesTreeCliqueBaseOrdered {
public:
typedef BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL> This;
typedef DERIVED DerivedType;
typedef CONDITIONAL ConditionalType;
typedef boost::shared_ptr<ConditionalType> sharedConditional;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
typedef boost::shared_ptr<DerivedType> derived_ptr;
typedef boost::weak_ptr<DerivedType> derived_weak_ptr;
typedef typename ConditionalType::FactorType FactorType;
typedef typename FactorGraphOrdered<FactorType>::Eliminate Eliminate;
protected:
/// @name Standard Constructors
/// @{
/** Default constructor */
BayesTreeCliqueBaseOrdered() {}
/** Construct from a conditional, leaving parent and child pointers uninitialized */
BayesTreeCliqueBaseOrdered(const sharedConditional& conditional);
/** Construct from an elimination result, which is a pair<CONDITIONAL,FACTOR> */
BayesTreeCliqueBaseOrdered(
const std::pair<sharedConditional,
boost::shared_ptr<typename ConditionalType::FactorType> >& result);
/// @}
/// This stores the Cached separator margnal P(S)
mutable boost::optional<FactorGraphOrdered<FactorType> > cachedSeparatorMarginal_;
public:
sharedConditional conditional_;
derived_weak_ptr parent_;
FastList<derived_ptr> children_;
/// @name Testable
/// @{
/** check equality */
bool equals(const This& other, double tol = 1e-9) const {
return (!conditional_ && !other.conditional())
|| conditional_->equals(*other.conditional(), tol);
}
/** print this node */
void print(const std::string& s = "", const IndexFormatter& indexFormatter =
DefaultIndexFormatter) const;
/** print this node and entire subtree below it */
void printTree(const std::string& indent = "",
const IndexFormatter& indexFormatter = DefaultIndexFormatter) const;
/// @}
/// @name Standard Interface
/// @{
/** Access the conditional */
const sharedConditional& conditional() const {
return conditional_;
}
/** is this the root of a Bayes tree ? */
inline bool isRoot() const {
return parent_.expired();
}
/** The size of subtree rooted at this clique, i.e., nr of Cliques */
size_t treeSize() const;
/** Collect number of cliques with cached separator marginals */
size_t numCachedSeparatorMarginals() const;
/** The arrow operator accesses the conditional */
const ConditionalType* operator->() const {
return conditional_.get();
}
/** return the const reference of children */
const std::list<derived_ptr>& children() const {
return children_;
}
/** return a shared_ptr to the parent clique */
derived_ptr parent() const {
return parent_.lock();
}
/// @}
/// @name Advanced Interface
/// @{
/** The arrow operator accesses the conditional */
ConditionalType* operator->() {
return conditional_.get();
}
/** return the reference of children non-const version*/
FastList<derived_ptr>& children() {
return children_;
}
/** Construct shared_ptr from a conditional, leaving parent and child pointers uninitialized */
static derived_ptr Create(const sharedConditional& conditional) {
return boost::make_shared<DerivedType>(conditional);
}
/** Construct shared_ptr from a FactorGraph<FACTOR>::EliminationResult. In this class
* the conditional part is kept and the factor part is ignored, but in derived clique
* types, such as ISAM2Clique, the factor part is kept as a cached factor.
* @param result An elimination result, which is a pair<CONDITIONAL,FACTOR>
*/
static derived_ptr Create(const std::pair<sharedConditional,
boost::shared_ptr<typename ConditionalType::FactorType> >& result) {
return boost::make_shared<DerivedType>(result);
}
/** Returns a new clique containing a copy of the conditional but without
* the parent and child clique pointers.
*/
derived_ptr clone() const {
return Create(sharedConditional(new ConditionalType(*conditional_)));
}
/** Permute the variables in the whole subtree rooted at this clique */
void permuteWithInverse(const Permutation& inversePermutation);
/** Permute variables when they only appear in the separators. In this
* case the running intersection property will be used to prevent always
* traversing the whole tree. Returns whether any separator variables in
* this subtree were reordered.
*/
bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction);
/** return the conditional P(S|Root) on the separator given the root */
BayesNetOrdered<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
/** return the marginal P(S) on the separator */
FactorGraphOrdered<FactorType> separatorMarginal(derived_ptr root, Eliminate function) const;
/** return the marginal P(C) of the clique, using marginal caching */
FactorGraphOrdered<FactorType> marginal2(derived_ptr root, Eliminate function) const;
/**
* This deletes the cached shortcuts of all cliques (subtree) below this clique.
* This is performed when the bayes tree is modified.
*/
void deleteCachedShortcuts();
const boost::optional<FactorGraphOrdered<FactorType> >& cachedSeparatorMarginal() const {
return cachedSeparatorMarginal_; }
friend class BayesTreeOrdered<ConditionalType, DerivedType> ;
protected:
/// assert invariants that have to hold in a clique
void assertInvariants() const;
/// Calculate set \f$ S \setminus B \f$ for shortcut calculations
std::vector<Index> separator_setminus_B(derived_ptr B) const;
/// Calculate set \f$ S_p \cap B \f$ for shortcut calculations
std::vector<Index> parent_separator_intersection_B(derived_ptr B) const;
/**
* Determine variable indices to keep in recursive separator shortcut calculation
* The factor graph p_Cp_B has keys from the parent clique Cp and from B.
* But we only keep the variables not in S union B.
*/
std::vector<Index> shortcut_indices(derived_ptr B,
const FactorGraphOrdered<FactorType>& p_Cp_B) const;
/** Non-recursive delete cached shortcuts and marginals - internal only. */
void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }
private:
/**
* Cliques cannot be copied except by the clone() method, which does not
* copy the parent and child pointers.
*/
BayesTreeCliqueBaseOrdered(const This& other) {
assert(false);
}
/** Cliques cannot be copied except by the clone() method, which does not
* copy the parent and child pointers.
*/
This& operator=(const This& other) {
assert(false);
return *this;
}
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(conditional_);
ar & BOOST_SERIALIZATION_NVP(parent_);
ar & BOOST_SERIALIZATION_NVP(children_);
}
/// @}
};
// \struct Clique
template<class DERIVED, class CONDITIONAL>
const DERIVED* asDerived(
const BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>* base) {
return static_cast<const DERIVED*>(base);
}
template<class DERIVED, class CONDITIONAL>
DERIVED* asDerived(BayesTreeCliqueBaseOrdered<DERIVED, CONDITIONAL>* base) {
return static_cast<DERIVED*>(base);
}
}

804
gtsam/inference/BayesTreeOrdered-inl.h
View File

@ -1,804 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 BayesTree-inl.h
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
* @author Frank Dellaert
* @author Michael Kaess
* @author Viorela Ila
* @author Richard Roberts
*/
#pragma once
#include <gtsam/base/FastList.h>
#include <gtsam/base/FastSet.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/BayesTreeOrdered.h>
#include <gtsam/inference/inferenceOrdered.h>
#include <gtsam/inference/GenericSequentialSolver.h>
#include <fstream>
#include <iostream>
#include <algorithm>
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using boost::assign::operator+=;
#include <boost/format.hpp>
namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::CliqueData
BayesTreeOrdered<CONDITIONAL,CLIQUE>::getCliqueData() const {
CliqueData data;
getCliqueData(data, root_);
return data;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::getCliqueData(CliqueData& data, sharedClique clique) const {
data.conditionalSizes.push_back((*clique)->nrFrontals());
data.separatorSizes.push_back((*clique)->nrParents());
BOOST_FOREACH(sharedClique c, clique->children_) {
getCliqueData(data, c);
}
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
size_t BayesTreeOrdered<CONDITIONAL,CLIQUE>::numCachedSeparatorMarginals() const {
return (root_) ? root_->numCachedSeparatorMarginals() : 0;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::saveGraph(const std::string &s, const IndexFormatter& indexFormatter) const {
if (!root_.get()) throw std::invalid_argument("the root of Bayes tree has not been initialized!");
std::ofstream of(s.c_str());
of<< "digraph G{\n";
saveGraph(of, root_, indexFormatter);
of<<"}";
of.close();
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, int parentnum) const {
static int num = 0;
bool first = true;
std::stringstream out;
out << num;
std::string parent = out.str();
parent += "[label=\"";
BOOST_FOREACH(Index index, clique->conditional_->frontals()) {
if(!first) parent += ","; first = false;
parent += indexFormatter(index);
}
if( clique != root_){
parent += " : ";
s << parentnum << "->" << num << "\n";
}
first = true;
BOOST_FOREACH(Index sep, clique->conditional_->parents()) {
if(!first) parent += ","; first = false;
parent += indexFormatter(sep);
}
parent += "\"];\n";
s << parent;
parentnum = num;
BOOST_FOREACH(sharedClique c, clique->children_) {
num++;
saveGraph(s, c, indexFormatter, parentnum);
}
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::CliqueStats::print(const std::string& s) const {
std::cout << s
<<"avg Conditional Size: " << avgConditionalSize << std::endl
<< "max Conditional Size: " << maxConditionalSize << std::endl
<< "avg Separator Size: " << avgSeparatorSize << std::endl
<< "max Separator Size: " << maxSeparatorSize << std::endl;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::CliqueStats
BayesTreeOrdered<CONDITIONAL,CLIQUE>::CliqueData::getStats() const {
CliqueStats stats;
double sum = 0.0;
size_t max = 0;
BOOST_FOREACH(size_t s, conditionalSizes) {
sum += (double)s;
if(s > max) max = s;
}
stats.avgConditionalSize = sum / (double)conditionalSizes.size();
stats.maxConditionalSize = max;
sum = 0.0;
max = 1;
BOOST_FOREACH(size_t s, separatorSizes) {
sum += (double)s;
if(s > max) max = s;
}
stats.avgSeparatorSize = sum / (double)separatorSizes.size();
stats.maxSeparatorSize = max;
return stats;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::Cliques::print(const std::string& s, const IndexFormatter& indexFormatter) const {
std::cout << s << ":\n";
BOOST_FOREACH(sharedClique clique, *this)
clique->printTree("", indexFormatter);
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
bool BayesTreeOrdered<CONDITIONAL,CLIQUE>::Cliques::equals(const Cliques& other, double tol) const {
return other == *this;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique
BayesTreeOrdered<CONDITIONAL,CLIQUE>::addClique(const sharedConditional& conditional, const sharedClique& parent_clique) {
sharedClique new_clique(new Clique(conditional));
addClique(new_clique, parent_clique);
return new_clique;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
nodes_.resize(std::max((*clique)->lastFrontalKey()+1, nodes_.size()));
BOOST_FOREACH(Index j, (*clique)->frontals())
nodes_[j] = clique;
if (parent_clique != NULL) {
clique->parent_ = parent_clique;
parent_clique->children_.push_back(clique);
} else {
assert(!root_);
root_ = clique;
}
clique->assertInvariants();
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique BayesTreeOrdered<CONDITIONAL,CLIQUE>::addClique(
const sharedConditional& conditional, std::list<sharedClique>& child_cliques) {
sharedClique new_clique(new Clique(conditional));
nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size()));
BOOST_FOREACH(Index j, conditional->frontals())
nodes_[j] = new_clique;
new_clique->children_ = child_cliques;
BOOST_FOREACH(sharedClique& child, child_cliques)
child->parent_ = new_clique;
new_clique->assertInvariants();
return new_clique;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::permuteWithInverse(const Permutation& inversePermutation) {
// recursively permute the cliques and internal conditionals
if (root_)
root_->permuteWithInverse(inversePermutation);
// need to know what the largest key is to get the right number of cliques
Index maxIndex = *std::max_element(inversePermutation.begin(), inversePermutation.end());
// Update the nodes structure
typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::Nodes newNodes(maxIndex+1);
// inversePermutation.applyToCollection(newNodes, nodes_); // Uses the forward, rather than inverse permutation
for(size_t i = 0; i < nodes_.size(); ++i)
newNodes[inversePermutation[i]] = nodes_[i];
nodes_ = newNodes;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
inline void BayesTreeOrdered<CONDITIONAL,CLIQUE>::addToCliqueFront(BayesTreeOrdered<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional, const sharedClique& clique) {
static const bool debug = false;
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
// Debug check to make sure the conditional variable is ordered lower than
// its parents and that all of its parents are present either in this
// clique or its separator.
BOOST_FOREACH(Index parent, conditional->parents()) {
assert(parent > conditional->lastFrontalKey());
const Clique& cliquer(*clique);
assert(find(cliquer->begin(), cliquer->end(), parent) != cliquer->end());
}
#endif
if(debug) conditional->print("Adding conditional ");
if(debug) clique->print("To clique ");
Index j = conditional->lastFrontalKey();
bayesTree.nodes_.resize(std::max(j+1, bayesTree.nodes_.size()));
bayesTree.nodes_[j] = clique;
FastVector<Index> newIndices((*clique)->size() + 1);
newIndices[0] = j;
std::copy((*clique)->begin(), (*clique)->end(), newIndices.begin()+1);
clique->conditional_ = CONDITIONAL::FromKeys(newIndices, (*clique)->nrFrontals() + 1);
if(debug) clique->print("Expanded clique is ");
clique->assertInvariants();
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::removeClique(sharedClique clique) {
if (clique->isRoot())
root_.reset();
else { // detach clique from parent
sharedClique parent = clique->parent_.lock();
typename FastList<typename CLIQUE::shared_ptr>::iterator child = std::find(parent->children().begin(), parent->children().end(), clique);
assert(child != parent->children().end());
parent->children().erase(child);
}
// orphan my children
BOOST_FOREACH(sharedClique child, clique->children_)
child->parent_ = typename Clique::weak_ptr();
BOOST_FOREACH(Index j, clique->conditional()->frontals()) {
nodes_[j].reset();
}
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditionalOrdered> >& symbolic,
const std::vector<boost::shared_ptr<CONDITIONAL> >& conditionals,
const typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique& parent) {
// Helper function to build a non-symbolic tree (e.g. Gaussian) using a
// symbolic tree, used in the BT(BN) constructor.
// Build the current clique
FastList<typename CONDITIONAL::shared_ptr> cliqueConditionals;
BOOST_FOREACH(Index j, symbolic->conditional()->frontals()) {
cliqueConditionals.push_back(conditionals[j]); }
typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique thisClique(new CLIQUE(CONDITIONAL::Combine(cliqueConditionals.begin(), cliqueConditionals.end())));
// Add the new clique with the current parent
this->addClique(thisClique, parent);
// Build the children, whose parent is the new clique
BOOST_FOREACH(const BayesTreeOrdered<IndexConditionalOrdered>::sharedClique& child, symbolic->children()) {
this->recursiveTreeBuild(child, conditionals, thisClique); }
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
BayesTreeOrdered<CONDITIONAL,CLIQUE>::BayesTreeOrdered(const BayesNetOrdered<CONDITIONAL>& bayesNet) {
// First generate symbolic BT to determine clique structure
BayesTreeOrdered<IndexConditionalOrdered> sbt(bayesNet);
// Build index of variables to conditionals
std::vector<boost::shared_ptr<CONDITIONAL> > conditionals(sbt.root()->conditional()->frontals().back() + 1);
BOOST_FOREACH(const boost::shared_ptr<CONDITIONAL>& c, bayesNet) {
if(c->nrFrontals() != 1)
throw std::invalid_argument("BayesTree constructor from BayesNet only supports single frontal variable conditionals");
if(c->firstFrontalKey() >= conditionals.size())
throw std::invalid_argument("An inconsistent BayesNet was passed into the BayesTree constructor!");
if(conditionals[c->firstFrontalKey()])
throw std::invalid_argument("An inconsistent BayesNet with duplicate frontal variables was passed into the BayesTree constructor!");
conditionals[c->firstFrontalKey()] = c;
}
// Build the new tree
this->recursiveTreeBuild(sbt.root(), conditionals, sharedClique());
}
/* ************************************************************************* */
template<>
inline BayesTreeOrdered<IndexConditionalOrdered>::BayesTreeOrdered(const BayesNetOrdered<IndexConditionalOrdered>& bayesNet) {
BayesNetOrdered<IndexConditionalOrdered>::const_reverse_iterator rit;
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
insert(*this, *rit);
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
BayesTreeOrdered<CONDITIONAL,CLIQUE>::BayesTreeOrdered(const BayesNetOrdered<CONDITIONAL>& bayesNet, std::list<BayesTreeOrdered<CONDITIONAL,CLIQUE> > subtrees) {
if (bayesNet.size() == 0)
throw std::invalid_argument("BayesTree::insert: empty bayes net!");
// get the roots of child subtrees and merge their nodes_
std::list<sharedClique> childRoots;
typedef BayesTreeOrdered<CONDITIONAL,CLIQUE> Tree;
BOOST_FOREACH(const Tree& subtree, subtrees) {
nodes_.assign(subtree.nodes_.begin(), subtree.nodes_.end());
childRoots.push_back(subtree.root());
}
// create a new clique and add all the conditionals to the clique
sharedClique new_clique;
typename BayesNetOrdered<CONDITIONAL>::sharedConditional conditional;
BOOST_REVERSE_FOREACH(conditional, bayesNet) {
if (!new_clique.get())
new_clique = addClique(conditional,childRoots);
else
addToCliqueFront(*this, conditional, new_clique);
}
root_ = new_clique;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
BayesTreeOrdered<CONDITIONAL,CLIQUE>::BayesTreeOrdered(const This& other) {
*this = other;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
BayesTreeOrdered<CONDITIONAL,CLIQUE>& BayesTreeOrdered<CONDITIONAL,CLIQUE>::operator=(const This& other) {
this->clear();
other.cloneTo(*this);
return *this;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::print(const std::string& s, const IndexFormatter& indexFormatter) const {
if (root_.use_count() == 0) {
std::cout << "WARNING: BayesTree.print encountered a forest..." << std::endl;
return;
}
std::cout << s << ": clique size == " << size() << ", node size == " << nodes_.size() << std::endl;
if (nodes_.empty()) return;
root_->printTree("", indexFormatter);
}
/* ************************************************************************* */
// binary predicate to test equality of a pair for use in equals
template<class CONDITIONAL, class CLIQUE>
bool check_sharedCliques(
const typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique& v1,
const typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique& v2
) {
return (!v1 && !v2) || (v1 && v2 && v1->equals(*v2));
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
bool BayesTreeOrdered<CONDITIONAL,CLIQUE>::equals(const BayesTreeOrdered<CONDITIONAL,CLIQUE>& other,
double tol) const {
return size()==other.size() &&
std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CONDITIONAL,CLIQUE>);
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
template<class CONTAINER>
inline Index BayesTreeOrdered<CONDITIONAL,CLIQUE>::findParentClique(const CONTAINER& parents) const {
typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end());
assert(lowestOrderedParent != parents.end());
return *lowestOrderedParent;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::insert(BayesTreeOrdered<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional)
{
static const bool debug = false;
// get indices and parents
const typename CONDITIONAL::Parents& parents = conditional->parents();
if(debug) conditional->print("Adding conditional ");
// if no parents, start a new root clique
if (parents.empty()) {
if(debug) std::cout << "No parents so making root" << std::endl;
bayesTree.root_ = bayesTree.addClique(conditional);
return;
}
// otherwise, find the parent clique by using the index data structure
// to find the lowest-ordered parent
Index parentRepresentative = bayesTree.findParentClique(parents);
if(debug) std::cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << std::endl;
sharedClique parent_clique = bayesTree[parentRepresentative];
if(debug) parent_clique->print("Parent clique is ");
// if the parents and parent clique have the same size, add to parent clique
if ((*parent_clique)->size() == size_t(parents.size())) {
if(debug) std::cout << "Adding to parent clique" << std::endl;
addToCliqueFront(bayesTree, conditional, parent_clique);
} else {
if(debug) std::cout << "Starting new clique" << std::endl;
// otherwise, start a new clique and add it to the tree
bayesTree.addClique(conditional,parent_clique);
}
}
/* ************************************************************************* */
//TODO: remove this function after removing TSAM.cpp
template<class CONDITIONAL, class CLIQUE>
typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique BayesTreeOrdered<CONDITIONAL,CLIQUE>::insert(
const sharedConditional& clique, std::list<sharedClique>& children, bool isRootClique) {
if (clique->nrFrontals() == 0)
throw std::invalid_argument("BayesTree::insert: empty clique!");
// create a new clique and add all the conditionals to the clique
sharedClique new_clique = addClique(clique, children);
if (isRootClique) root_ = new_clique;
return new_clique;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::fillNodesIndex(const sharedClique& subtree) {
// Add each frontal variable of this root node
BOOST_FOREACH(const Index& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; }
// Fill index for each child
typedef typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& child, subtree->children_) {
fillNodesIndex(child); }
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::insert(const sharedClique& subtree) {
if(subtree) {
// Find the parent clique of the new subtree. By the running intersection
// property, those separator variables in the subtree that are ordered
// lower than the highest frontal variable of the subtree root will all
// appear in the separator of the subtree root.
if(subtree->conditional()->parents().empty()) {
assert(!root_);
root_ = subtree;
} else {
Index parentRepresentative = findParentClique(subtree->conditional()->parents());
sharedClique parent = (*this)[parentRepresentative];
parent->children_ += subtree;
subtree->parent_ = parent; // set new parent!
}
// Now fill in the nodes index
if(nodes_.size() == 0 ||
*std::max_element(subtree->conditional()->beginFrontals(), subtree->conditional()->endFrontals()) > (nodes_.size() - 1)) {
nodes_.resize(subtree->conditional()->lastFrontalKey() + 1);
}
fillNodesIndex(subtree);
}
}
/* ************************************************************************* */
// First finds clique marginal then marginalizes that
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename CONDITIONAL::FactorType::shared_ptr BayesTreeOrdered<CONDITIONAL,CLIQUE>::marginalFactor(
Index j, Eliminate function) const
{
gttic(BayesTree_marginalFactor);
// get clique containing Index j
sharedClique clique = (*this)[j];
// calculate or retrieve its marginal P(C) = P(F,S)
#ifdef OLD_SHORTCUT_MARGINALS
FactorGraphOrdered<FactorType> cliqueMarginal = clique->marginal(root_,function);
#else
FactorGraphOrdered<FactorType> cliqueMarginal = clique->marginal2(root_,function);
#endif
// Reduce the variable indices to start at zero
gttic(Reduce);
const Permutation reduction = internal::createReducingPermutation(cliqueMarginal.keys());
internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, cliqueMarginal) {
if(factor) factor->reduceWithInverse(inverseReduction); }
gttoc(Reduce);
// now, marginalize out everything that is not variable j
GenericSequentialSolver<FactorType> solver(cliqueMarginal);
boost::shared_ptr<FactorType> result = solver.marginalFactor(inverseReduction[j], function);
// Undo the reduction
gttic(Undo_Reduce);
result->permuteWithInverse(reduction);
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, cliqueMarginal) {
if(factor) factor->permuteWithInverse(reduction); }
gttoc(Undo_Reduce);
return result;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename BayesNetOrdered<CONDITIONAL>::shared_ptr BayesTreeOrdered<CONDITIONAL,CLIQUE>::marginalBayesNet(
Index j, Eliminate function) const
{
gttic(BayesTree_marginalBayesNet);
// calculate marginal as a factor graph
FactorGraphOrdered<FactorType> fg;
fg.push_back(this->marginalFactor(j,function));
// Reduce the variable indices to start at zero
gttic(Reduce);
const Permutation reduction = internal::createReducingPermutation(fg.keys());
internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, fg) {
if(factor) factor->reduceWithInverse(inverseReduction); }
gttoc(Reduce);
// eliminate factor graph marginal to a Bayes net
boost::shared_ptr<BayesNetOrdered<CONDITIONAL> > bn = GenericSequentialSolver<FactorType>(fg).eliminate(function);
// Undo the reduction
gttic(Undo_Reduce);
bn->permuteWithInverse(reduction);
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, fg) {
if(factor) factor->permuteWithInverse(reduction); }
gttoc(Undo_Reduce);
return bn;
}
/* ************************************************************************* */
// Find two cliques, their joint, then marginalizes
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename FactorGraphOrdered<typename CONDITIONAL::FactorType>::shared_ptr
BayesTreeOrdered<CONDITIONAL,CLIQUE>::joint(Index j1, Index j2, Eliminate function) const {
gttic(BayesTree_joint);
// get clique C1 and C2
sharedClique C1 = (*this)[j1], C2 = (*this)[j2];
gttic(Lowest_common_ancestor);
// Find lowest common ancestor clique
sharedClique B; {
// Build two paths to the root
FastList<sharedClique> path1, path2; {
sharedClique p = C1;
while(p) {
path1.push_front(p);
p = p->parent();
}
} {
sharedClique p = C2;
while(p) {
path2.push_front(p);
p = p->parent();
}
}
// Find the path intersection
B = this->root();
typename FastList<sharedClique>::const_iterator p1 = path1.begin(), p2 = path2.begin();
while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) {
B = *p1;
++p1;
++p2;
}
}
gttoc(Lowest_common_ancestor);
// Compute marginal on lowest common ancestor clique
gttic(LCA_marginal);
FactorGraphOrdered<FactorType> p_B = B->marginal2(this->root(), function);
gttoc(LCA_marginal);
// Compute shortcuts of the requested cliques given the lowest common ancestor
gttic(Clique_shortcuts);
BayesNetOrdered<CONDITIONAL> p_C1_Bred = C1->shortcut(B, function);
BayesNetOrdered<CONDITIONAL> p_C2_Bred = C2->shortcut(B, function);
gttoc(Clique_shortcuts);
// Factor the shortcuts to be conditioned on the full root
// Get the set of variables to eliminate, which is C1\B.
gttic(Full_root_factoring);
sharedConditional p_C1_B; {
std::vector<Index> C1_minus_B; {
FastSet<Index> C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
BOOST_FOREACH(const Index j, *B->conditional()) {
C1_minus_B_set.erase(j); }
C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end());
}
// Factor into C1\B | B.
FactorGraphOrdered<FactorType> temp_remaining;
boost::tie(p_C1_B, temp_remaining) = FactorGraphOrdered<FactorType>(p_C1_Bred).eliminate(C1_minus_B, function);
}
sharedConditional p_C2_B; {
std::vector<Index> C2_minus_B; {
FastSet<Index> C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
BOOST_FOREACH(const Index j, *B->conditional()) {
C2_minus_B_set.erase(j); }
C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end());
}
// Factor into C2\B | B.
FactorGraphOrdered<FactorType> temp_remaining;
boost::tie(p_C2_B, temp_remaining) = FactorGraphOrdered<FactorType>(p_C2_Bred).eliminate(C2_minus_B, function);
}
gttoc(Full_root_factoring);
gttic(Variable_joint);
// Build joint on all involved variables
FactorGraphOrdered<FactorType> p_BC1C2;
p_BC1C2.push_back(p_B);
p_BC1C2.push_back(p_C1_B->toFactor());
p_BC1C2.push_back(p_C2_B->toFactor());
if(C1 != B)
p_BC1C2.push_back(C1->conditional()->toFactor());
if(C2 != B)
p_BC1C2.push_back(C2->conditional()->toFactor());
// Reduce the variable indices to start at zero
gttic(Reduce);
const Permutation reduction = internal::createReducingPermutation(p_BC1C2.keys());
internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_BC1C2) {
if(factor) factor->reduceWithInverse(inverseReduction); }
std::vector<Index> js; js.push_back(inverseReduction[j1]); js.push_back(inverseReduction[j2]);
gttoc(Reduce);
// now, marginalize out everything that is not variable j
GenericSequentialSolver<FactorType> solver(p_BC1C2);
boost::shared_ptr<FactorGraphOrdered<FactorType> > result = solver.jointFactorGraph(js, function);
// Undo the reduction
gttic(Undo_Reduce);
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, *result) {
if(factor) factor->permuteWithInverse(reduction); }
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_BC1C2) {
if(factor) factor->permuteWithInverse(reduction); }
gttoc(Undo_Reduce);
return result;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename BayesNetOrdered<CONDITIONAL>::shared_ptr BayesTreeOrdered<CONDITIONAL,CLIQUE>::jointBayesNet(
Index j1, Index j2, Eliminate function) const {
// eliminate factor graph marginal to a Bayes net
return GenericSequentialSolver<FactorType> (
*this->joint(j1, j2, function)).eliminate(function);
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::clear() {
// Remove all nodes and clear the root pointer
nodes_.clear();
root_.reset();
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::removePath(sharedClique clique,
BayesNetOrdered<CONDITIONAL>& bn, typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::Cliques& orphans) {
// base case is NULL, if so we do nothing and return empties above
if (clique!=NULL) {
// remove the clique from orphans in case it has been added earlier
orphans.remove(clique);
// remove me
this->removeClique(clique);
// remove path above me
this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, orphans);
// add children to list of orphans (splice also removed them from clique->children_)
orphans.splice(orphans.begin(), clique->children_);
bn.push_back(clique->conditional());
}
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
template<class CONTAINER>
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::removeTop(const CONTAINER& keys,
BayesNetOrdered<CONDITIONAL>& bn, typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::Cliques& orphans) {
// process each key of the new factor
BOOST_FOREACH(const Index& j, keys) {
// get the clique
if(j < nodes_.size()) {
const sharedClique& clique(nodes_[j]);
if(clique) {
// remove path from clique to root
this->removePath(clique, bn, orphans);
}
}
}
// Delete cachedShortcuts for each orphan subtree
//TODO: Consider Improving
BOOST_FOREACH(sharedClique& orphan, orphans)
orphan->deleteCachedShortcuts();
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::Cliques BayesTreeOrdered<CONDITIONAL,CLIQUE>::removeSubtree(
const sharedClique& subtree)
{
// Result clique list
Cliques cliques;
cliques.push_back(subtree);
// Remove the first clique from its parents
if(!subtree->isRoot())
subtree->parent()->children().remove(subtree);
else
root_.reset();
// Add all subtree cliques and erase the children and parent of each
for(typename Cliques::iterator clique = cliques.begin(); clique != cliques.end(); ++clique)
{
// Add children
BOOST_FOREACH(const sharedClique& child, (*clique)->children()) {
cliques.push_back(child); }
// Delete cached shortcuts
(*clique)->deleteCachedShortcutsNonRecursive();
// Remove this node from the nodes index
BOOST_FOREACH(Index j, (*clique)->conditional()->frontals()) {
nodes_[j].reset(); }
// Erase the parent and children pointers
(*clique)->parent_.reset();
(*clique)->children_.clear();
}
return cliques;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::cloneTo(This& newTree) const {
if(root())
cloneTo(newTree, root(), sharedClique());
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTreeOrdered<CONDITIONAL,CLIQUE>::cloneTo(
This& newTree, const sharedClique& subtree, const sharedClique& parent) const {
sharedClique newClique(subtree->clone());
newTree.addClique(newClique, parent);
BOOST_FOREACH(const sharedClique& childClique, subtree->children()) {
cloneTo(newTree, childClique, newClique);
}
}
/* ************************************************************************* */
} // \namespace gtsam

418
gtsam/inference/BayesTreeOrdered.h
View File

@ -1,418 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 BayesTree.h
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
* @author Frank Dellaert
*/
// \callgraph
#pragma once
#include <vector>
#include <stdexcept>
#include <deque>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/lexical_cast.hpp>
#include <gtsam/global_includes.h>
#include <gtsam/base/FastList.h>
#include <gtsam/inference/FactorGraphOrdered.h>
#include <gtsam/inference/BayesNetOrdered.h>
#include <gtsam/inference/BayesTreeCliqueBaseOrdered.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
#include <gtsam/linear/VectorValuesOrdered.h>
namespace gtsam {
// Forward declaration of BayesTreeClique which is defined below BayesTree in this file
template<class CONDITIONAL> struct BayesTreeClique;
/**
* Bayes tree
* @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain,
* which could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional.
* @tparam CLIQUE The type of the clique data structure, defaults to BayesTreeClique, normally do not change this
* as it is only used when developing special versions of BayesTree, e.g. for ISAM2.
*
* \addtogroup Multifrontal
* \nosubgrouping
*/
template<class CONDITIONAL, class CLIQUE=BayesTreeClique<CONDITIONAL> >
class BayesTreeOrdered {
public:
typedef BayesTreeOrdered<CONDITIONAL, CLIQUE> This;
typedef boost::shared_ptr<BayesTreeOrdered<CONDITIONAL, CLIQUE> > shared_ptr;
typedef boost::shared_ptr<CONDITIONAL> sharedConditional;
typedef boost::shared_ptr<BayesNetOrdered<CONDITIONAL> > sharedBayesNet;
typedef CONDITIONAL ConditionalType;
typedef typename CONDITIONAL::FactorType FactorType;
typedef typename FactorGraphOrdered<FactorType>::Eliminate Eliminate;
typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique
// typedef for shared pointers to cliques
typedef boost::shared_ptr<Clique> sharedClique;
// A convenience class for a list of shared cliques
struct Cliques : public FastList<sharedClique> {
void print(const std::string& s = "Cliques",
const IndexFormatter& indexFormatter = DefaultIndexFormatter) const;
bool equals(const Cliques& other, double tol = 1e-9) const;
};
/** clique statistics */
struct CliqueStats {
double avgConditionalSize;
std::size_t maxConditionalSize;
double avgSeparatorSize;
std::size_t maxSeparatorSize;
void print(const std::string& s = "") const ;
};
/** store all the sizes */
struct CliqueData {
std::vector<std::size_t> conditionalSizes;
std::vector<std::size_t> separatorSizes;
CliqueStats getStats() const;
};
/** Map from indices to Clique */
typedef std::vector<sharedClique> Nodes;
protected:
/** Map from indices to Clique */
Nodes nodes_;
/** Root clique */
sharedClique root_;
public:
/// @name Standard Constructors
/// @{
/** Create an empty Bayes Tree */
BayesTreeOrdered() {}
/** Create a Bayes Tree from a Bayes Net (requires CONDITIONAL is IndexConditional *or* CONDITIONAL::Combine) */
explicit BayesTreeOrdered(const BayesNetOrdered<CONDITIONAL>& bayesNet);
/** Copy constructor */
BayesTreeOrdered(const This& other);
/** Assignment operator */
This& operator=(const This& other);
/// @}
/// @name Advanced Constructors
/// @{
/**
* Create a Bayes Tree from a Bayes Net and some subtrees. The Bayes net corresponds to the
* new root clique and the subtrees are connected to the root clique.
*/
BayesTreeOrdered(const BayesNetOrdered<CONDITIONAL>& bayesNet, std::list<BayesTreeOrdered<CONDITIONAL,CLIQUE> > subtrees);
/** Destructor */
virtual ~BayesTreeOrdered() {}
/// @}
/// @name Testable
/// @{
/** check equality */
bool equals(const BayesTreeOrdered<CONDITIONAL,CLIQUE>& other, double tol = 1e-9) const;
/** print */
void print(const std::string& s = "",
const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const;
/// @}
/// @name Standard Interface
/// @{
/**
* Find parent clique of a conditional. It will look at all parents and
* return the one with the lowest index in the ordering.
*/
template<class CONTAINER>
Index findParentClique(const CONTAINER& parents) const;
/** number of cliques */
inline size_t size() const {
if(root_)
return root_->treeSize();
else
return 0;
}
/** number of nodes */
inline size_t nrNodes() const { return nodes_.size(); }
/** Check if there are any cliques in the tree */
inline bool empty() const {
return nodes_.empty();
}
/** return nodes */
const Nodes& nodes() const { return nodes_; }
/** return root clique */
const sharedClique& root() const { return root_; }
/** find the clique that contains the variable with Index j */
inline sharedClique operator[](Index j) const {
return nodes_.at(j);
}
/** alternate syntax for matlab: find the clique that contains the variable with Index j */
inline sharedClique clique(Index j) const {
return nodes_.at(j);
}
/** Gather data on all cliques */
CliqueData getCliqueData() const;
/** Collect number of cliques with cached separator marginals */
size_t numCachedSeparatorMarginals() const;
/** return marginal on any variable */
typename FactorType::shared_ptr marginalFactor(Index j, Eliminate function) const;
/**
* return marginal on any variable, as a Bayes Net
* NOTE: this function calls marginal, and then eliminates it into a Bayes Net
* This is more expensive than the above function
*/
typename BayesNetOrdered<CONDITIONAL>::shared_ptr marginalBayesNet(Index j, Eliminate function) const;
/**
* return joint on two variables
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
*/
typename FactorGraphOrdered<FactorType>::shared_ptr joint(Index j1, Index j2, Eliminate function) const;
/**
* return joint on two variables as a BayesNet
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
*/
typename BayesNetOrdered<CONDITIONAL>::shared_ptr jointBayesNet(Index j1, Index j2, Eliminate function) const;
/**
* Read only with side effects
*/
/** saves the Tree to a text file in GraphViz format */
void saveGraph(const std::string& s, const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const;
/// @}
/// @name Advanced Interface
/// @{
/** Access the root clique (non-const version) */
sharedClique& root() { return root_; }
/** Access the nodes (non-cost version) */
Nodes& nodes() { return nodes_; }
/** Remove all nodes */
void clear();
/** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */
void deleteCachedShortcuts() {
root_->deleteCachedShortcuts();
}
/** Apply a permutation to the full tree - also updates the nodes structure */
void permuteWithInverse(const Permutation& inversePermutation);
/**
* Remove path from clique to root and return that path as factors
* plus a list of orphaned subtree roots. Used in removeTop below.
*/
void removePath(sharedClique clique, BayesNetOrdered<CONDITIONAL>& bn, Cliques& orphans);
/**
* Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
* Factors and orphans are added to the in/out arguments.
*/
template<class CONTAINER>
void removeTop(const CONTAINER& indices, BayesNetOrdered<CONDITIONAL>& bn, Cliques& orphans);
/**
* Remove the requested subtree. */
Cliques removeSubtree(const sharedClique& subtree);
/**
* Hang a new subtree off of the existing tree. This finds the appropriate
* parent clique for the subtree (which may be the root), and updates the
* nodes index with the new cliques in the subtree. None of the frontal
* variables in the subtree may appear in the separators of the existing
* BayesTree.
*/
void insert(const sharedClique& subtree);
/** Insert a new conditional
* This function only applies for Symbolic case with IndexCondtional,
* We make it static so that it won't be compiled in GaussianConditional case.
* */
static void insert(BayesTreeOrdered<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional);
/**
* Insert a new clique corresponding to the given Bayes net.
* It is the caller's responsibility to decide whether the given Bayes net is a valid clique,
* i.e. all the variables (frontal and separator) are connected
*/
sharedClique insert(const sharedConditional& clique,
std::list<sharedClique>& children, bool isRootClique = false);
/**
* Create a clone of this object as a shared pointer
* Necessary for inheritance in matlab interface
*/
virtual shared_ptr clone() const {
return shared_ptr(new This(*this));
}
protected:
/** private helper method for saving the Tree to a text file in GraphViz format */
void saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter,
int parentnum = 0) const;
/** Gather data on a single clique */
void getCliqueData(CliqueData& stats, sharedClique clique) const;
/** remove a clique: warning, can result in a forest */
void removeClique(sharedClique clique);
/** add a clique (top down) */
sharedClique addClique(const sharedConditional& conditional, const sharedClique& parent_clique = sharedClique());
/** add a clique (top down) */
void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique());
/** add a clique (bottom up) */
sharedClique addClique(const sharedConditional& conditional, std::list<sharedClique>& child_cliques);
/**
* Add a conditional to the front of a clique, i.e. a conditional whose
* parents are already in the clique or its separators. This function does
* not check for this condition, it just updates the data structures.
*/
static void addToCliqueFront(BayesTreeOrdered<CONDITIONAL,CLIQUE>& bayesTree,
const sharedConditional& conditional, const sharedClique& clique);
/** Fill the nodes index for a subtree */
void fillNodesIndex(const sharedClique& subtree);
/** Helper function to build a non-symbolic tree (e.g. Gaussian) using a
* symbolic tree, used in the BT(BN) constructor.
*/
void recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditionalOrdered> >& symbolic,
const std::vector<boost::shared_ptr<CONDITIONAL> >& conditionals,
const typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique& parent);
private:
/** deep copy to another tree */
void cloneTo(This& newTree) const;
/** deep copy to another tree */
void cloneTo(This& newTree, const sharedClique& subtree, const sharedClique& parent) const;
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(nodes_);
ar & BOOST_SERIALIZATION_NVP(root_);
}
/// @}
}; // BayesTree
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void _BayesTree_dim_adder(
std::vector<size_t>& dims,
const typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique& clique) {
if(clique) {
// Add dims from this clique
for(typename CONDITIONAL::const_iterator it = (*clique)->beginFrontals(); it != (*clique)->endFrontals(); ++it)
dims[*it] = (*clique)->dim(it);
// Traverse children
typedef typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& child, clique->children()) {
_BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dims, child);
}
}
}
/* ************************************************************************* */
template<class CONDITIONAL,class CLIQUE>
boost::shared_ptr<VectorValuesOrdered> allocateVectorValues(const BayesTreeOrdered<CONDITIONAL,CLIQUE>& bt) {
std::vector<size_t> dimensions(bt.nodes().size(), 0);
_BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dimensions, bt.root());
return boost::shared_ptr<VectorValuesOrdered>(new VectorValuesOrdered(dimensions));
}
/* ************************************************************************* */
/**
* A Clique in the tree is an incomplete Bayes net: the variables
* in the Bayes net are the frontal nodes, and the variables conditioned
* on are the separator. We also have pointers up and down the tree.
*
* Since our Conditional class already handles multiple frontal variables,
* this Clique contains exactly 1 conditional.
*
* This is the default clique type in a BayesTree, but some algorithms, like
* iSAM2 (see ISAM2Clique), use a different clique type in order to store
* extra data along with the clique.
*/
template<class CONDITIONAL>
struct BayesTreeClique : public BayesTreeCliqueBaseOrdered<BayesTreeClique<CONDITIONAL>, CONDITIONAL> {
public:
typedef CONDITIONAL ConditionalType;
typedef BayesTreeClique<CONDITIONAL> This;
typedef BayesTreeCliqueBaseOrdered<This, CONDITIONAL> Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
BayesTreeClique() {}
BayesTreeClique(const typename ConditionalType::shared_ptr& conditional) : Base(conditional) {}
BayesTreeClique(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
};
} /// namespace gtsam
#include <gtsam/inference/BayesTreeOrdered-inl.h>
#include <gtsam/inference/BayesTreeCliqueBaseOrdered-inl.h>

112
gtsam/inference/ClusterTreeOrdered-inl.h
View File

@ -1,112 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 ClusterTree-inl.h
* @date July 13, 2010
* @author Kai Ni
* @author Frank Dellaert
* @brief: Collects factorgraph fragments defined on variable clusters, arranged in a tree
*/
#pragma once
#include <iostream>
#include <boost/foreach.hpp>
#include <gtsam/inference/ClusterTreeOrdered.h>
namespace gtsam {
/* ************************************************************************* *
* Cluster
* ************************************************************************* */
template<class FG>
template<class Iterator>
ClusterTreeOrdered<FG>::Cluster::Cluster(const FG& fg, Index key, Iterator firstSeparator, Iterator lastSeparator) :
FG(fg), frontal(1, key), separator(firstSeparator, lastSeparator) {}
/* ************************************************************************* */
template<class FG>
template<typename FRONTALIT, typename SEPARATORIT>
ClusterTreeOrdered<FG>::Cluster::Cluster(
const FG& fg, FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator) :
FG(fg), frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {}
/* ************************************************************************* */
template<class FG>
template<typename FRONTALIT, typename SEPARATORIT>
ClusterTreeOrdered<FG>::Cluster::Cluster(
FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator) :
frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {}
/* ************************************************************************* */
template<class FG>
void ClusterTreeOrdered<FG>::Cluster::addChild(typename ClusterTreeOrdered<FG>::Cluster::shared_ptr child) {
children_.push_back(child);
}
/* ************************************************************************* */
template<class FG>
bool ClusterTreeOrdered<FG>::Cluster::equals(const Cluster& other) const {
if (frontal != other.frontal) return false;
if (separator != other.separator) return false;
if (children_.size() != other.children_.size()) return false;
typename std::list<shared_ptr>::const_iterator it1 = children_.begin();
typename std::list<shared_ptr>::const_iterator it2 = other.children_.begin();
for (; it1 != children_.end(); it1++, it2++)
if (!(*it1)->equals(**it2)) return false;
return true;
}
/* ************************************************************************* */
template<class FG>
void ClusterTreeOrdered<FG>::Cluster::print(const std::string& indent,
const IndexFormatter& formatter) const {
std::cout << indent;
BOOST_FOREACH(const Index key, frontal)
std::cout << formatter(key) << " ";
std::cout << ": ";
BOOST_FOREACH(const Index key, separator)
std::cout << key << " ";
std::cout << std::endl;
}
/* ************************************************************************* */
template<class FG>
void ClusterTreeOrdered<FG>::Cluster::printTree(const std::string& indent,
const IndexFormatter& formatter) const {
print(indent, formatter);
BOOST_FOREACH(const shared_ptr& child, children_)
child->printTree(indent + " ", formatter);
}
/* ************************************************************************* *
* ClusterTree
* ************************************************************************* */
template<class FG>
void ClusterTreeOrdered<FG>::print(const std::string& str,
const IndexFormatter& formatter) const {
std::cout << str << std::endl;
if (root_) root_->printTree("", formatter);
}
/* ************************************************************************* */
template<class FG>
bool ClusterTreeOrdered<FG>::equals(const ClusterTreeOrdered<FG>& other, double tol) const {
if (!root_ && !other.root_) return true;
if (!root_ || !other.root_) return false;
return root_->equals(*other.root_);
}
} //namespace gtsam

141
gtsam/inference/ClusterTreeOrdered.h
View File

@ -1,141 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 ClusterTree.h
* @date July 13, 2010
* @author Kai Ni
* @author Frank Dellaert
* @brief: Collects factorgraph fragments defined on variable clusters, arranged in a tree
*/
#pragma once
#include <list>
#include <vector>
#include <string>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#include <boost/lexical_cast.hpp>
#include <gtsam/global_includes.h>
namespace gtsam {
/**
* A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman:
* each node k represents a subset \f$ C_k \sub X \f$, and the tree is family preserving, in that
* each factor \f$ f_i \f$ is associated with a single cluster and \f$ scope(f_i) \sub C_k \f$.
* \nosubgrouping
*/
template <class FG>
class ClusterTreeOrdered {
public:
// Access to factor types
typedef typename FG::KeyType KeyType;
protected:
// the class for subgraphs that also include the pointers to the parents and two children
class Cluster : public FG {
public:
typedef typename boost::shared_ptr<Cluster> shared_ptr;
typedef typename boost::weak_ptr<Cluster> weak_ptr;
const std::vector<Index> frontal; // the frontal variables
const std::vector<Index> separator; // the separator variables
protected:
weak_ptr parent_; // the parent cluster
std::list<shared_ptr> children_; // the child clusters
const typename FG::sharedFactor eliminated_; // the eliminated factor to pass on to the parent
public:
/// Construct empty clique
Cluster() {}
/** Create a node with a single frontal variable */
template<typename Iterator>
Cluster(const FG& fg, Index key, Iterator firstSeparator, Iterator lastSeparator);
/** Create a node with several frontal variables */
template<typename FRONTALIT, typename SEPARATORIT>
Cluster(const FG& fg, FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator);
/** Create a node with several frontal variables */
template<typename FRONTALIT, typename SEPARATORIT>
Cluster(FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator);
/// print
void print(const std::string& indent, const IndexFormatter& formatter = DefaultIndexFormatter) const;
/// print the enire tree
void printTree(const std::string& indent, const IndexFormatter& formatter = DefaultIndexFormatter) const;
/// check equality
bool equals(const Cluster& other) const;
/// get a reference to the children
const std::list<shared_ptr>& children() const { return children_; }
/// add a child
void addChild(shared_ptr child);
/// get or set the parent
weak_ptr& parent() { return parent_; }
};
/// @name Advanced Interface
/// @{
/// typedef for shared pointers to clusters
typedef typename Cluster::shared_ptr sharedCluster;
/// Root cluster
sharedCluster root_;
public:
/// @}
/// @name Standard Constructors
/// @{
/// constructor of empty tree
ClusterTreeOrdered() {}
/// @}
/// @name Standard Interface
/// @{
/// return the root cluster
sharedCluster root() const { return root_; }
/// @}
/// @name Testable
/// @{
/// print the object
void print(const std::string& str="", const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** check equality */
bool equals(const ClusterTreeOrdered<FG>& other, double tol = 1e-9) const;
/// @}
}; // ClusterTree
} // namespace gtsam
#include <gtsam/inference/ClusterTreeOrdered-inl.h>

266
gtsam/inference/ConditionalOrdered.h
View File

@ -1,266 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 Conditional.h
* @brief Base class for conditional densities
* @author Frank Dellaert
*/
// \callgraph
#pragma once
#include <gtsam/inference/FactorOrdered.h>
#include <boost/range.hpp>
#include <iostream>
#include <list>
namespace gtsam {
/**
* Base class for conditional densities, templated on KEY type. This class
* provides storage for the keys involved in a conditional, and iterators and
* access to the frontal and separator keys.
*
* Derived classes *must* redefine the Factor and shared_ptr typedefs to refer
* to the associated factor type and shared_ptr type of the derived class. See
* IndexConditional and GaussianConditional for examples.
* \nosubgrouping
*/
template<typename KEY>
class ConditionalOrdered: public gtsam::FactorOrdered<KEY> {
private:
/** Create keys by adding key in front */
template<typename ITERATOR>
static std::vector<KEY> MakeKeys(KEY key, ITERATOR firstParent,
ITERATOR lastParent) {
std::vector<KeyType> keys((lastParent - firstParent) + 1);
std::copy(firstParent, lastParent, keys.begin() + 1);
keys[0] = key;
return keys;
}
protected:
/** The first nFrontal variables are frontal and the rest are parents. */
size_t nrFrontals_;
// Calls the base class assertInvariants, which checks for unique keys
void assertInvariants() const {
FactorOrdered<KEY>::assertInvariants();
}
public:
typedef KEY KeyType;
typedef ConditionalOrdered<KeyType> This;
typedef FactorOrdered<KeyType> Base;
/**
* Typedef to the factor type that produces this conditional and that this
* conditional can be converted to using a factor constructor. Derived
* classes must redefine this.
*/
typedef gtsam::FactorOrdered<KeyType> FactorType;
/** A shared_ptr to this class. Derived classes must redefine this. */
typedef boost::shared_ptr<This> shared_ptr;
/** Iterator over keys */
typedef typename FactorType::iterator iterator;
/** Const iterator over keys */
typedef typename FactorType::const_iterator const_iterator;
/** View of the frontal keys (call frontals()) */
typedef boost::iterator_range<const_iterator> Frontals;
/** View of the separator keys (call parents()) */
typedef boost::iterator_range<const_iterator> Parents;
/// @name Standard Constructors
/// @{
/** Empty Constructor to make serialization possible */
ConditionalOrdered() :
nrFrontals_(0) {
assertInvariants();
}
/** No parents */
ConditionalOrdered(KeyType key) :
FactorType(key), nrFrontals_(1) {
assertInvariants();
}
/** Single parent */
ConditionalOrdered(KeyType key, KeyType parent) :
FactorType(key, parent), nrFrontals_(1) {
assertInvariants();
}
/** Two parents */
ConditionalOrdered(KeyType key, KeyType parent1, KeyType parent2) :
FactorType(key, parent1, parent2), nrFrontals_(1) {
assertInvariants();
}
/** Three parents */
ConditionalOrdered(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) :
FactorType(key, parent1, parent2, parent3), nrFrontals_(1) {
assertInvariants();
}
/// @}
/// @name Advanced Constructors
/// @{
/** Constructor from a frontal variable and a vector of parents */
ConditionalOrdered(KeyType key, const std::vector<KeyType>& parents) :
FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(
1) {
assertInvariants();
}
/** Constructor from keys and nr of frontal variables */
ConditionalOrdered(const std::vector<Index>& keys, size_t nrFrontals) :
FactorType(keys), nrFrontals_(nrFrontals) {
assertInvariants();
}
/** Constructor from keys and nr of frontal variables */
ConditionalOrdered(const std::list<Index>& keys, size_t nrFrontals) :
FactorType(keys.begin(),keys.end()), nrFrontals_(nrFrontals) {
assertInvariants();
}
/// @}
/// @name Testable
/// @{
/** print with optional formatter */
void print(const std::string& s = "Conditional",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** check equality */
template<class DERIVED>
bool equals(const DERIVED& c, double tol = 1e-9) const {
return nrFrontals_ == c.nrFrontals_ && FactorType::equals(c, tol);
}
/// @}
/// @name Standard Interface
/// @{
/** return the number of frontals */
size_t nrFrontals() const {
return nrFrontals_;
}
/** return the number of parents */
size_t nrParents() const {
return FactorType::size() - nrFrontals_;
}
/** Special accessor when there is only one frontal variable. */
KeyType firstFrontalKey() const {
assert(nrFrontals_>0);
return FactorType::front();
}
KeyType lastFrontalKey() const {
assert(nrFrontals_>0);
return *(endFrontals() - 1);
}
/** return a view of the frontal keys */
Frontals frontals() const {
return boost::make_iterator_range(beginFrontals(), endFrontals());
}
/** return a view of the parent keys */
Parents parents() const {
return boost::make_iterator_range(beginParents(), endParents());
}
/** Iterators over frontal and parent variables. */
const_iterator beginFrontals() const {
return FactorType::begin();
} ///<TODO: comment
const_iterator endFrontals() const {
return FactorType::begin() + nrFrontals_;
} ///<TODO: comment
const_iterator beginParents() const {
return FactorType::begin() + nrFrontals_;
} ///<TODO: comment
const_iterator endParents() const {
return FactorType::end();
} ///<TODO: comment
/// @}
/// @name Advanced Interface
/// @{
/** Mutable iterators and accessors */
iterator beginFrontals() {
return FactorType::begin();
} ///<TODO: comment
iterator endFrontals() {
return FactorType::begin() + nrFrontals_;
} ///<TODO: comment
iterator beginParents() {
return FactorType::begin() + nrFrontals_;
} ///<TODO: comment
iterator endParents() {
return FactorType::end();
} ///<TODO: comment
///TODO: comment
boost::iterator_range<iterator> frontals() {
return boost::make_iterator_range(beginFrontals(), endFrontals());
}
///TODO: comment
boost::iterator_range<iterator> parents() {
return boost::make_iterator_range(beginParents(), endParents());
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
}
/// @}
};
/* ************************************************************************* */
template<typename KEY>
void ConditionalOrdered<KEY>::print(const std::string& s,
const IndexFormatter& formatter) const {
std::cout << s << " P(";
BOOST_FOREACH(KeyType key, frontals())
std::cout << " " << formatter(key);
if (nrParents() > 0)
std::cout << " |";
BOOST_FOREACH(KeyType parent, parents())
std::cout << " " << formatter(parent);
std::cout << ")" << std::endl;
}
} // gtsam

234
gtsam/inference/EliminationTreeOrdered-inl.h
View File

@ -1,234 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 EliminationTree-inl.h
* @author Frank Dellaert
* @date Oct 13, 2010
*/
#pragma once
#include <gtsam/base/timing.h>
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/EliminationTreeOrdered.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/IndexFactorOrdered.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
#include <boost/foreach.hpp>
#include <boost/static_assert.hpp>
#include <iostream>
#include <set>
#include <vector>
namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
typename EliminationTreeOrdered<FACTOR>::sharedFactor EliminationTreeOrdered<FACTOR>::eliminate_(
Eliminate function, Conditionals& conditionals) const {
static const bool debug = false;
if(debug) std::cout << "ETree: eliminating " << this->key_ << std::endl;
if(this->key_ < conditionals.size()) { // If it is requested to eliminate the current variable
// Create the list of factors to be eliminated, initially empty, and reserve space
FactorGraphOrdered<FACTOR> factors;
factors.reserve(this->factors_.size() + this->subTrees_.size());
// Add all factors associated with the current node
factors.push_back(this->factors_.begin(), this->factors_.end());
// for all subtrees, eliminate into Bayes net and a separator factor, added to [factors]
BOOST_FOREACH(const shared_ptr& child, subTrees_)
factors.push_back(child->eliminate_(function, conditionals)); // TODO: spawn thread
// TODO: wait for completion of all threads
// Combine all factors (from this node and from subtrees) into a joint factor
typename FactorGraphOrdered<FACTOR>::EliminationResult
eliminated(function(factors, 1));
conditionals[this->key_] = eliminated.first;
if(debug) std::cout << "Eliminated " << this->key_ << " to get:\n";
if(debug) eliminated.first->print("Conditional: ");
if(debug) eliminated.second->print("Factor: ");
return eliminated.second;
} else {
// Eliminate each child but discard the result.
BOOST_FOREACH(const shared_ptr& child, subTrees_) {
(void)child->eliminate_(function, conditionals);
}
return sharedFactor(); // Return a NULL factor
}
}
/* ************************************************************************* */
template<class FACTOR>
std::vector<Index> EliminationTreeOrdered<FACTOR>::ComputeParents(const VariableIndexOrdered& structure) {
// Number of factors and variables
const size_t m = structure.nFactors();
const size_t n = structure.size();
static const Index none = std::numeric_limits<Index>::max();
// Allocate result parent vector and vector of last factor columns
std::vector<Index> parents(n, none);
std::vector<Index> prevCol(m, none);
// for column j \in 1 to n do
for (Index j = 0; j < n; j++) {
// for row i \in Struct[A*j] do
BOOST_FOREACH(const size_t i, structure[j]) {
if (prevCol[i] != none) {
Index k = prevCol[i];
// find root r of the current tree that contains k
Index r = k;
while (parents[r] != none)
r = parents[r];
if (r != j) parents[r] = j;
}
prevCol[i] = j;
}
}
return parents;
}
/* ************************************************************************* */
template<class FACTOR>
template<class DERIVEDFACTOR>
typename EliminationTreeOrdered<FACTOR>::shared_ptr EliminationTreeOrdered<FACTOR>::Create(
const FactorGraphOrdered<DERIVEDFACTOR>& factorGraph,
const VariableIndexOrdered& structure) {
static const bool debug = false;
gttic(ET_Create1);
gttic(ET_ComputeParents);
// Compute the tree structure
std::vector<Index> parents(ComputeParents(structure));
gttoc(ET_ComputeParents);
// Number of variables
const size_t n = structure.size();
static const Index none = std::numeric_limits<Index>::max();
// Create tree structure
gttic(assemble_tree);
std::vector<shared_ptr> trees(n);
for (Index k = 1; k <= n; k++) {
Index j = n - k; // Start at the last variable and loop down to 0
trees[j].reset(new EliminationTreeOrdered(j)); // Create a new node on this variable
if (parents[j] != none) // If this node has a parent, add it to the parent's children
trees[parents[j]]->add(trees[j]);
else if(!structure[j].empty() && j != n - 1) // If a node other than the last has no parents, this is a forest
throw DisconnectedGraphException();
}
gttoc(assemble_tree);
// Hang factors in right places
gttic(hang_factors);
BOOST_FOREACH(const typename boost::shared_ptr<DERIVEDFACTOR>& factor, factorGraph) {
if(factor && factor->size() > 0) {
Index j = *std::min_element(factor->begin(), factor->end());
if(j < structure.size())
trees[j]->add(factor);
}
}
gttoc(hang_factors);
if(debug)
trees.back()->print("ETree: ");
// Check that this is not null
assert(trees.back().get());
return trees.back();
}
/* ************************************************************************* */
template<class FACTOR>
template<class DERIVEDFACTOR>
typename EliminationTreeOrdered<FACTOR>::shared_ptr
EliminationTreeOrdered<FACTOR>::Create(const FactorGraphOrdered<DERIVEDFACTOR>& factorGraph) {
gttic(ET_Create2);
// Build variable index
const VariableIndexOrdered variableIndex(factorGraph);
// Build elimination tree
return Create(factorGraph, variableIndex);
}
/* ************************************************************************* */
template<class FACTORGRAPH>
void EliminationTreeOrdered<FACTORGRAPH>::print(const std::string& name,
const IndexFormatter& formatter) const {
std::cout << name << " (" << formatter(key_) << ")" << std::endl;
BOOST_FOREACH(const sharedFactor& factor, factors_) {
factor->print(name + " ", formatter); }
BOOST_FOREACH(const shared_ptr& child, subTrees_) {
child->print(name + " ", formatter); }
}
/* ************************************************************************* */
template<class FACTORGRAPH>
bool EliminationTreeOrdered<FACTORGRAPH>::equals(const EliminationTreeOrdered<FACTORGRAPH>& expected, double tol) const {
if(this->key_ == expected.key_ && this->factors_ == expected.factors_
&& this->subTrees_.size() == expected.subTrees_.size()) {
typename SubTrees::const_iterator this_subtree = this->subTrees_.begin();
typename SubTrees::const_iterator expected_subtree = expected.subTrees_.begin();
while(this_subtree != this->subTrees_.end())
if( ! (*(this_subtree++))->equals(**(expected_subtree++), tol))
return false;
return true;
} else
return false;
}
/* ************************************************************************* */
template<class FACTOR>
typename EliminationTreeOrdered<FACTOR>::BayesNet::shared_ptr
EliminationTreeOrdered<FACTOR>::eliminatePartial(typename EliminationTreeOrdered<FACTOR>::Eliminate function, size_t nrToEliminate) const {
// call recursive routine
gttic(ET_recursive_eliminate);
if(nrToEliminate > this->key_ + 1)
throw std::invalid_argument("Requested that EliminationTree::eliminatePartial eliminate more variables than exist");
Conditionals conditionals(nrToEliminate); // reserve a vector of conditional shared pointers
(void)eliminate_(function, conditionals); // modify in place
gttoc(ET_recursive_eliminate);
// Add conditionals to BayesNet
gttic(assemble_BayesNet);
typename BayesNet::shared_ptr bayesNet(new BayesNet);
BOOST_FOREACH(const typename BayesNet::sharedConditional& conditional, conditionals) {
if(conditional)
bayesNet->push_back(conditional);
}
gttoc(assemble_BayesNet);
return bayesNet;
}
/* ************************************************************************* */
template<class FACTOR>
typename EliminationTreeOrdered<FACTOR>::BayesNet::shared_ptr
EliminationTreeOrdered<FACTOR>::eliminate(Eliminate function) const {
size_t nrConditionals = this->key_ + 1; // root key has highest index
return eliminatePartial(function, nrConditionals);
}
}

187
gtsam/inference/EliminationTreeOrdered.h
View File

@ -1,187 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 EliminationTree.h
* @author Frank Dellaert
* @date Oct 13, 2010
*/
#pragma once
#include <utility>
#include <gtsam/base/FastSet.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/VariableIndexOrdered.h>
#include <gtsam/inference/FactorGraphOrdered.h>
class EliminationTreeOrderedTester; // for unit tests, see testEliminationTree
namespace gtsam { template<class CONDITIONAL> class BayesNetOrdered; }
namespace gtsam {
/**
* An elimination tree is a data structure used intermediately during
* elimination. In future versions it will be used to save work between
* multiple eliminations.
*
* When a variable is eliminated, a new factor is created by combining that
* variable's neighboring factors. The new combined factor involves the combined
* factors' involved variables. When the lowest-ordered one of those variables
* is eliminated, it consumes that combined factor. In the elimination tree,
* that lowest-ordered variable is the parent of the variable that was eliminated to
* produce the combined factor. This yields a tree in general, and not a chain
* because of the implicit sparse structure of the resulting Bayes net.
*
* This structure is examined even more closely in a JunctionTree, which
* additionally identifies cliques in the chordal Bayes net.
* \nosubgrouping
*/
template<class FACTOR>
class EliminationTreeOrdered {
public:
typedef EliminationTreeOrdered<FACTOR> This; ///< This class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
typedef typename boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
typedef gtsam::BayesNetOrdered<typename FACTOR::ConditionalType> BayesNet; ///< The BayesNet corresponding to FACTOR
typedef FACTOR Factor;
typedef typename FACTOR::KeyType KeyType;
/** Typedef for an eliminate subroutine */
typedef typename FactorGraphOrdered<FACTOR>::Eliminate Eliminate;
private:
/** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
typedef FastList<sharedFactor> Factors;
typedef FastList<shared_ptr> SubTrees;
typedef std::vector<typename FACTOR::ConditionalType::shared_ptr> Conditionals;
Index key_; ///< index associated with root // FIXME: doesn't this require that "Index" is the type of keys in the generic factor?
Factors factors_; ///< factors associated with root
SubTrees subTrees_; ///< sub-trees
public:
/// @name Standard Constructors
/// @{
/**
* Named constructor to build the elimination tree of a factor graph using
* pre-computed column structure.
* @param factorGraph The factor graph for which to build the elimination tree
* @param structure The set of factors involving each variable. If this is not
* precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
* named constructor instead.
* @return The elimination tree
*/
template<class DERIVEDFACTOR>
static shared_ptr Create(const FactorGraphOrdered<DERIVEDFACTOR>& factorGraph, const VariableIndexOrdered& structure);
/** Named constructor to build the elimination tree of a factor graph. Note
* that this has to compute the column structure as a VariableIndex, so if you
* already have this precomputed, use the Create(const FactorGraph<DERIVEDFACTOR>&, const VariableIndex&)
* named constructor instead.
* @param factorGraph The factor graph for which to build the elimination tree
*/
template<class DERIVEDFACTOR>
static shared_ptr Create(const FactorGraphOrdered<DERIVEDFACTOR>& factorGraph);
/// @}
/// @name Standard Interface
/// @{
/** Eliminate the factors to a Bayes Net
* @param function The function to use to eliminate, see the namespace functions
* in GaussianFactorGraph.h
* @return The BayesNet resulting from elimination
*/
typename BayesNet::shared_ptr eliminate(Eliminate function) const;
/** Eliminate the factors to a Bayes Net and return the remaining factor
* @param function The function to use to eliminate, see the namespace functions
* in GaussianFactorGraph.h
* @return The BayesNet resulting from elimination and the remaining factor
*/
typename BayesNet::shared_ptr eliminatePartial(Eliminate function, size_t nrToEliminate) const;
/// @}
/// @name Testable
/// @{
/** Print the tree to cout */
void print(const std::string& name = "EliminationTree: ",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** Test whether the tree is equal to another */
bool equals(const EliminationTreeOrdered& other, double tol = 1e-9) const;
/// @}
private:
/** default constructor, private, as you should use Create below */
EliminationTreeOrdered(Index key = 0) : key_(key) {}
/**
* Static internal function to build a vector of parent pointers using the
* algorithm of Gilbert et al., 2001, BIT.
*/
static std::vector<Index> ComputeParents(const VariableIndexOrdered& structure);
/** add a factor, for Create use only */
void add(const sharedFactor& factor) { factors_.push_back(factor); }
/** add a subtree, for Create use only */
void add(const shared_ptr& child) { subTrees_.push_back(child); }
/**
* Recursive routine that eliminates the factors arranged in an elimination tree
* @param Conditionals is a vector of shared pointers that will be modified in place
*/
sharedFactor eliminate_(Eliminate function, Conditionals& conditionals) const;
/// Allow access to constructor and add methods for testing purposes
friend class ::EliminationTreeOrderedTester;
};
/**
* An exception thrown when attempting to eliminate a disconnected factor
* graph, which is not currently possible in gtsam. If you need to work with
* disconnected graphs, a workaround is to create zero-information factors to
* bridge the disconnects. To do this, create any factor type (e.g.
* BetweenFactor or RangeFactor) with the noise model
* <tt>sharedPrecision(dim, 0.0)</tt>, where \c dim is the appropriate
* dimensionality for that factor.
*/
struct DisconnectedGraphException : public std::exception {
DisconnectedGraphException() {}
virtual ~DisconnectedGraphException() throw() {}
/// Returns the string "Attempting to eliminate a disconnected graph - this is not currently possible in gtsam."
virtual const char* what() const throw() {
return
"Attempting to eliminate a disconnected graph - this is not currently possible in\n"
"GTSAM. You may add \"empty\" BetweenFactor's to join disconnected graphs, these\n"
"will affect the symbolic structure and solving complexity of the graph but not\n"
"the solution. To do this, create BetweenFactor's with zero-precision noise\n"
"models, i.e. noiseModel::Isotropic::Precision(n, 0.0);\n"; }
};
}
#include <gtsam/inference/EliminationTreeOrdered-inl.h>

288
gtsam/inference/FactorGraphOrdered-inl.h
View File

@ -1,288 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 FactorGraph-inl.h
* This is a template definition file, include it where needed (only!)
* so that the appropriate code is generated and link errors avoided.
* @brief Factor Graph Base Class
* @author Carlos Nieto
* @author Frank Dellaert
* @author Alireza Fathi
* @author Michael Kaess
*/
#pragma once
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/BayesTreeOrdered.h>
#include <gtsam/inference/VariableIndexOrdered.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/format.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <stdio.h>
#include <list>
#include <sstream>
#include <stdexcept>
namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
template<class CONDITIONAL>
FactorGraphOrdered<FACTOR>::FactorGraphOrdered(const BayesNetOrdered<CONDITIONAL>& bayesNet) {
factors_.reserve(bayesNet.size());
BOOST_FOREACH(const typename CONDITIONAL::shared_ptr& cond, bayesNet) {
this->push_back(cond->toFactor());
}
}
/* ************************************************************************* */
template<class FACTOR>
void FactorGraphOrdered<FACTOR>::print(const std::string& s,
const IndexFormatter& formatter) const {
std::cout << s << std::endl;
std::cout << "size: " << size() << std::endl;
for (size_t i = 0; i < factors_.size(); i++) {
std::stringstream ss;
ss << "factor " << i << ": ";
if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter);
}
}
/* ************************************************************************* */
template<class FACTOR>
bool FactorGraphOrdered<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;
/** check whether the factors_ are the same */
for (size_t i = 0; i < factors_.size(); i++) {
// TODO: Doesn't this force order of factor insertion?
sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
if (f1 == NULL && f2 == NULL) continue;
if (f1 == NULL || f2 == NULL) return false;
if (!f1->equals(*f2, tol)) return false;
}
return true;
}
/* ************************************************************************* */
template<class FACTOR>
size_t FactorGraphOrdered<FACTOR>::nrFactors() const {
size_t size_ = 0;
BOOST_FOREACH(const sharedFactor& factor, factors_)
if (factor) size_++;
return size_;
}
/* ************************************************************************* */
template<class FACTOR>
std::set<typename FACTOR::KeyType> FactorGraphOrdered<FACTOR>::keys() const {
std::set<KeyType> allKeys;
BOOST_FOREACH(const sharedFactor& factor, factors_)
if (factor) {
BOOST_FOREACH(Index j, factor->keys())
allKeys.insert(j);
}
return allKeys;
}
/* ************************************************************************* */
template<class FACTOR>
std::pair<typename FactorGraphOrdered<FACTOR>::sharedConditional, FactorGraphOrdered<FACTOR> >
FactorGraphOrdered<FACTOR>::eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const
{
// Build variable index
VariableIndexOrdered variableIndex(*this);
// Find first variable
Index firstIndex = 0;
while(firstIndex < variableIndex.size() && variableIndex[firstIndex].empty())
++ firstIndex;
// Check that number of variables is in bounds
if(firstIndex + nFrontals > variableIndex.size())
throw std::invalid_argument("Requested to eliminate more frontal variables than exist in the factor graph.");
// Get set of involved factors
FastSet<size_t> involvedFactorIs;
for(Index j = firstIndex; j < firstIndex + nFrontals; ++j) {
BOOST_FOREACH(size_t i, variableIndex[j]) {
involvedFactorIs.insert(i);
}
}
// Separate factors into involved and remaining
FactorGraphOrdered<FactorType> involvedFactors;
FactorGraphOrdered<FactorType> remainingFactors;
FastSet<size_t>::const_iterator involvedFactorIsIt = involvedFactorIs.begin();
for(size_t i = 0; i < this->size(); ++i) {
if(involvedFactorIsIt != involvedFactorIs.end() && *involvedFactorIsIt == i) {
// If the current factor is involved, add it to involved and increment involved iterator
involvedFactors.push_back((*this)[i]);
++ involvedFactorIsIt;
} else {
// If not involved, add to remaining
remainingFactors.push_back((*this)[i]);
}
}
// Do dense elimination on the involved factors
typename FactorGraphOrdered<FactorType>::EliminationResult eliminationResult =
eliminate(involvedFactors, nFrontals);
// Add the remaining factor back into the factor graph
remainingFactors.push_back(eliminationResult.second);
// Return the eliminated factor and remaining factor graph
return std::make_pair(eliminationResult.first, remainingFactors);
}
/* ************************************************************************* */
template<class FACTOR>
std::pair<typename FactorGraphOrdered<FACTOR>::sharedConditional, FactorGraphOrdered<FACTOR> >
FactorGraphOrdered<FACTOR>::eliminate(const std::vector<KeyType>& variables, const Eliminate& eliminateFcn,
boost::optional<const VariableIndexOrdered&> variableIndex_) const
{
const VariableIndexOrdered& variableIndex =
variableIndex_ ? *variableIndex_ : VariableIndexOrdered(*this);
// First find the involved factors
FactorGraphOrdered<FACTOR> 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 sharedFactor factor = this->at(factorIndex);
involvedFactors.push_back(factor); // Add involved factor
highestInvolvedVariable = std::max( // Updated largest index
highestInvolvedVariable,
*std::max_element(factor->begin(), factor->end()));
}
sharedConditional conditional;
sharedFactor remainingFactor;
if(involvedFactors.size() > 0) {
// Now permute the variables to be eliminated to the front of the ordering
Permutation toFront = Permutation::PullToFront(variables, highestInvolvedVariable+1);
Permutation toFrontInverse = *toFront.inverse();
BOOST_FOREACH(const sharedFactor& factor, involvedFactors) {
factor->permuteWithInverse(toFrontInverse); }
// Eliminate into conditional and remaining factor
EliminationResult eliminated = eliminateFcn(involvedFactors, variables.size());
conditional = eliminated.first;
remainingFactor = eliminated.second;
// Undo the permutation
BOOST_FOREACH(const sharedFactor& factor, involvedFactors) {
factor->permuteWithInverse(toFront); }
conditional->permuteWithInverse(toFront);
remainingFactor->permuteWithInverse(toFront);
} else {
// Eliminate 0 variables
EliminationResult eliminated = eliminateFcn(involvedFactors, 0);
conditional = eliminated.first;
remainingFactor = eliminated.second;
}
// Build the remaining graph, without the removed factors
FactorGraphOrdered<FACTOR> remainingGraph;
remainingGraph.reserve(this->size() - involvedFactors.size() + 1);
FastSet<size_t>::const_iterator involvedFactorIndexIt = involvedFactorIndices.begin();
for(size_t i = 0; i < this->size(); ++i) {
if(involvedFactorIndexIt != involvedFactorIndices.end() && *involvedFactorIndexIt == i)
++ involvedFactorIndexIt;
else
remainingGraph.push_back(this->at(i));
}
// Add the remaining factor if it is not empty.
if(remainingFactor->size() != 0)
remainingGraph.push_back(remainingFactor);
return std::make_pair(conditional, remainingGraph);
}
/* ************************************************************************* */
template<class FACTOR>
void FactorGraphOrdered<FACTOR>::replace(size_t index, sharedFactor factor) {
if (index >= factors_.size()) throw std::invalid_argument(boost::str(
boost::format("Factor graph does not contain a factor with index %d.")
% index));
// Replace the factor
factors_[index] = factor;
}
/* ************************************************************************* */
template<class FACTORGRAPH>
FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2) {
// create new linear factor graph equal to the first one
FACTORGRAPH fg = fg1;
// add the second factors_ in the graph
fg.push_back(fg2);
return fg;
}
/* ************************************************************************* */
template<class DERIVEDFACTOR, class KEY>
typename DERIVEDFACTOR::shared_ptr Combine(const FactorGraphOrdered<DERIVEDFACTOR>& factors,
const FastMap<KEY, std::vector<KEY> >& variableSlots) {
typedef const std::pair<const KEY, std::vector<KEY> > KeySlotPair;
// Local functional for getting keys out of key-value pairs
struct Local { static KEY FirstOf(const KeySlotPair& pr) { return pr.first; } };
return typename DERIVEDFACTOR::shared_ptr(new DERIVEDFACTOR(
boost::make_transform_iterator(variableSlots.begin(), &Local::FirstOf),
boost::make_transform_iterator(variableSlots.end(), &Local::FirstOf)));
}
/* ************************************************************************* */
// 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 boost::shared_ptr<FACTOR> >& factors_io,
const typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique& clique) {
if(clique) {
// Add factor from this clique
factors_io.push_back((*clique)->toFactor());
// Traverse children
typedef typename BayesTreeOrdered<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& child, clique->children())
_FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL,CLIQUE>(factors_io, child);
}
}
/* ************************************************************************* */
template<class FACTOR>
template<class CONDITIONAL, class CLIQUE>
FactorGraphOrdered<FACTOR>::FactorGraphOrdered(const BayesTreeOrdered<CONDITIONAL,CLIQUE>& bayesTree) {
factors_.reserve(bayesTree.size());
_FactorGraph_BayesTree_adder<FACTOR,CONDITIONAL,CLIQUE>(factors_, bayesTree.root());
}
/* ************************************************************************* */
} // namespace gtsam

273
gtsam/inference/FactorGraphOrdered.h
View File

@ -1,273 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 FactorGraph.h
* @brief Factor Graph Base Class
* @author Carlos Nieto
* @author Christian Potthast
* @author Michael Kaess
*/
// \callgraph
#pragma once
#include <gtsam/base/Testable.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/inference/BayesNetOrdered.h>
#include <boost/serialization/nvp.hpp>
#include <boost/function.hpp>
#include <set>
namespace gtsam {
// Forward declarations
template<class CONDITIONAL, class CLIQUE> class BayesTreeOrdered;
class VariableIndexOrdered;
/**
* A factor graph is a bipartite graph with factor nodes connected to variable nodes.
* In this class, however, only factor nodes are kept around.
* \nosubgrouping
*/
template<class FACTOR>
class FactorGraphOrdered {
public:
typedef FACTOR FactorType; ///< factor type
typedef typename FACTOR::KeyType KeyType; ///< type of Keys we use to index variables 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 FactorGraphOrdered<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<sharedConditional, sharedFactor> EliminationResult;
/** typedef for an eliminate subroutine */
typedef boost::function<EliminationResult(const This&, size_t)> Eliminate;
protected:
/** concept check, makes sure FACTOR defines print and equals */
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
/** Collection of factors */
std::vector<sharedFactor> factors_;
public:
/// @name Standard Constructor
/// @{
/** Default constructor */
FactorGraphOrdered() {}
/// @}
/// @name Advanced Constructors
/// @{
/**
* @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>
FactorGraphOrdered(const BayesNetOrdered<CONDITIONAL>& bayesNet);
/** convert from Bayes tree */
template<class CONDITIONAL, class CLIQUE>
FactorGraphOrdered(const BayesTreeOrdered<CONDITIONAL, CLIQUE>& bayesTree);
/** convert from a derived type */
template<class DERIVEDFACTOR>
FactorGraphOrdered(const FactorGraphOrdered<DERIVEDFACTOR>& factors) {
factors_.assign(factors.begin(), factors.end());
}
/// @}
/// @name Adding Factors
/// @{
/**
* Reserve space for the specified number of factors if you know in
* advance how many there will be (works like std::vector::reserve).
*/
void reserve(size_t size) { factors_.reserve(size); }
/** Add a factor */
template<class DERIVEDFACTOR>
void push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor) {
factors_.push_back(boost::shared_ptr<FACTOR>(factor));
}
/** push back many factors */
void push_back(const This& factors) {
factors_.insert(end(), factors.begin(), factors.end());
}
/** push back many factors with an iterator */
template<typename ITERATOR>
void push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
factors_.insert(end(), firstFactor, lastFactor);
}
/**
* @brief Add a vector of derived factors
* @param factors to add
*/
template<typename DERIVEDFACTOR>
void push_back(const std::vector<typename boost::shared_ptr<DERIVEDFACTOR> >& factors) {
factors_.insert(end(), factors.begin(), factors.end());
}
/// @}
/// @name Testable
/// @{
/** print out graph */
void print(const std::string& s = "FactorGraph",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** Check equality */
bool equals(const This& fg, double tol = 1e-9) const;
/// @}
/// @name Standard Interface
/// @{
/** return the number of factors and NULLS */
size_t size() const { return factors_.size();}
/** Simple check for an empty graph - faster than comparing size() to zero */
bool empty() const { return factors_.empty(); }
/** const cast to the underlying vector of factors */
operator const std::vector<sharedFactor>&() const { return factors_; }
/** Get a specific factor by index */
const sharedFactor at(size_t i) const { assert(i<factors_.size()); return factors_[i]; }
sharedFactor& at(size_t i) { assert(i<factors_.size()); return factors_[i]; }
const sharedFactor operator[](size_t i) const { return at(i); }
sharedFactor& operator[](size_t i) { return at(i); }
/** Checks whether a valid factor exists at the given index */
inline bool exists(size_t i) const { return i<factors_.size() && factors_[i]; }
/** STL begin, so we can use BOOST_FOREACH */
const_iterator begin() const { return factors_.begin();}
/** STL end, so we can use BOOST_FOREACH */
const_iterator end() const { return factors_.end(); }
/** Get the first factor */
sharedFactor front() const { return factors_.front(); }
/** Get the last factor */
sharedFactor back() const { return factors_.back(); }
/** Eliminate the first \c n frontal variables, returning the resulting
* conditional and remaining factor graph - this is very inefficient for
* eliminating all variables, to do that use EliminationTree or
* JunctionTree.
*/
std::pair<sharedConditional, FactorGraphOrdered<FactorType> > eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const;
/** 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.
*/
std::pair<sharedConditional, FactorGraphOrdered<FactorType> > eliminate(
const std::vector<KeyType>& variables, const Eliminate& eliminateFcn,
boost::optional<const VariableIndexOrdered&> variableIndex = boost::none) const;
/** Eliminate a single variable, by calling FactorGraph::eliminate. */
std::pair<sharedConditional, FactorGraphOrdered<FactorType> > eliminateOne(
KeyType variable, const Eliminate& eliminateFcn,
boost::optional<const VariableIndexOrdered&> variableIndex = boost::none) const {
std::vector<size_t> variables(1, variable);
return eliminate(variables, eliminateFcn, variableIndex);
}
/// @}
/// @name Modifying Factor Graphs (imperative, discouraged)
/// @{
/** non-const STL-style begin() */
iterator begin() { return factors_.begin();}
/** non-const STL-style end() */
iterator end() { return factors_.end(); }
/** resize the factor graph. TODO: effects? */
void resize(size_t size) { factors_.resize(size); }
/** delete factor without re-arranging indexes by inserting a NULL pointer */
inline void remove(size_t i) { factors_[i].reset();}
/** replace a factor by index */
void replace(size_t index, sharedFactor factor);
/// @}
/// @name Advanced Interface
/// @{
/** return the number valid factors */
size_t nrFactors() const;
/** Potentially very slow function to return all keys involved */
std::set<KeyType> keys() const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(factors_);
}
/// @}
}; // FactorGraph
/** Create a combined joint factor (new style for EliminationTree). */
template<class DERIVEDFACTOR, class KEY>
typename DERIVEDFACTOR::shared_ptr Combine(const FactorGraphOrdered<DERIVEDFACTOR>& factors,
const FastMap<KEY, std::vector<KEY> >& variableSlots);
/**
* static function that combines two factor graphs
* @param fg1 Linear factor graph
* @param fg2 Linear factor graph
* @return a new combined factor graph
*/
template<class FACTORGRAPH>
FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2);
} // namespace gtsam
#include <gtsam/inference/FactorGraphOrdered-inl.h>

108
gtsam/inference/FactorOrdered-inl.h
View File

@ -1,108 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 Factor-inl.h
* @author Richard Roberts
* @date Sep 1, 2010
*/
#pragma once
#include <gtsam/inference/FactorOrdered.h>
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <iostream>
#include <stdexcept>
namespace gtsam {
/* ************************************************************************* */
template<typename KEY>
FactorOrdered<KEY>::FactorOrdered(const FactorOrdered<KEY>& f) :
keys_(f.keys_) {
assertInvariants();
}
/* ************************************************************************* */
template<typename KEY>
FactorOrdered<KEY>::FactorOrdered(const ConditionalType& c) :
keys_(c.keys()) {
// assert(c.nrFrontals() == 1);
assertInvariants();
}
/* ************************************************************************* */
template<typename KEY>
void FactorOrdered<KEY>::assertInvariants() const {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
// Check that keys are all unique
std::multiset<KeyType> nonunique(keys_.begin(), keys_.end());
std::set<KeyType> unique(keys_.begin(), keys_.end());
bool correct = (nonunique.size() == unique.size())
&& std::equal(nonunique.begin(), nonunique.end(), unique.begin());
if (!correct) throw std::logic_error(
"Factor::assertInvariants: Factor contains duplicate keys");
#endif
}
/* ************************************************************************* */
template<typename KEY>
void FactorOrdered<KEY>::print(const std::string& s,
const IndexFormatter& formatter) const {
printKeys(s,formatter);
}
/* ************************************************************************* */
template<typename KEY>
void FactorOrdered<KEY>::printKeys(const std::string& s, const IndexFormatter& formatter) const {
std::cout << s << " ";
BOOST_FOREACH(KEY key, keys_) std::cout << " " << formatter(key);
std::cout << std::endl;
}
/* ************************************************************************* */
template<typename KEY>
bool FactorOrdered<KEY>::equals(const This& other, double tol) const {
return keys_ == other.keys_;
}
/* ************************************************************************* */
#ifdef TRACK_ELIMINATE
template<typename KEY>
template<class COND>
typename COND::shared_ptr FactorOrdered<KEY>::eliminateFirst() {
assert(!keys_.empty());
assertInvariants();
KEY eliminated = keys_.front();
keys_.erase(keys_.begin());
assertInvariants();
return typename COND::shared_ptr(new COND(eliminated, keys_));
}
/* ************************************************************************* */
template<typename KEY>
template<class COND>
typename BayesNetOrdered<COND>::shared_ptr FactorOrdered<KEY>::eliminate(size_t nrFrontals) {
assert(keys_.size() >= nrFrontals);
assertInvariants();
typename BayesNetOrdered<COND>::shared_ptr fragment(new BayesNetOrdered<COND> ());
const_iterator it = this->begin();
for (KEY n = 0; n < nrFrontals; ++n, ++it)
fragment->push_back(COND::FromRange(it, const_iterator(this->end()), 1));
if (nrFrontals > 0) keys_.assign(fragment->back()->beginParents(),
fragment->back()->endParents());
assertInvariants();
return fragment;
}
#endif
}

232
gtsam/inference/FactorOrdered.h
View File

@ -1,232 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 Factor.h
* @brief The base class for all factors
* @author Kai Ni
* @author Frank Dellaert
* @author Richard Roberts
*/
// \callgraph
#pragma once
#include <set>
#include <vector>
#include <boost/serialization/nvp.hpp>
#include <boost/foreach.hpp>
#include <boost/function/function1.hpp>
#include <boost/lexical_cast.hpp>
#include <gtsam/global_includes.h>
#include <gtsam/base/FastMap.h>
namespace gtsam {
template<class KEY> class ConditionalOrdered;
/**
* This is the base class for all factor types. It is templated on a KEY type,
* which will be the type used to label variables. Key types currently in use
* in gtsam are Index with symbolic (IndexFactor, SymbolicFactorGraph) and
* Gaussian factors (GaussianFactor, JacobianFactor, HessianFactor, GaussianFactorGraph),
* and Key with nonlinear factors (NonlinearFactor, NonlinearFactorGraph).
* though currently only IndexFactor and IndexConditional derive from this
* class, using Index keys. This class does not store any data other than its
* keys. Derived classes store data such as matrices and probability tables.
*
* Note that derived classes *must* redefine the ConditionalType and shared_ptr
* typedefs to refer to the associated conditional and shared_ptr types of the
* derived class. See IndexFactor, JacobianFactor, etc. for examples.
*
* This class is \b not virtual for performance reasons - derived symbolic classes,
* IndexFactor and IndexConditional, need to be created and destroyed quickly
* during symbolic elimination. GaussianFactor and NonlinearFactor are virtual.
* \nosubgrouping
*/
template<typename KEY>
class FactorOrdered {
public:
typedef KEY KeyType; ///< The KEY template parameter
typedef FactorOrdered<KeyType> This; ///< This class
/**
* Typedef to the conditional type obtained by eliminating this factor,
* derived classes must redefine this.
*/
typedef ConditionalOrdered<KeyType> ConditionalType;
/// A shared_ptr to this class, derived classes must redefine this.
typedef boost::shared_ptr<FactorOrdered> shared_ptr;
/// Iterator over keys
typedef typename std::vector<KeyType>::iterator iterator;
/// Const iterator over keys
typedef typename std::vector<KeyType>::const_iterator const_iterator;
protected:
/// The keys involved in this factor
std::vector<KeyType> keys_;
public:
/// @name Standard Constructors
/// @{
/** Copy constructor */
FactorOrdered(const This& f);
/** Construct from conditional, calls ConditionalType::toFactor() */
FactorOrdered(const ConditionalType& c);
/** Default constructor for I/O */
FactorOrdered() {}
/** Construct unary factor */
FactorOrdered(KeyType key) : keys_(1) {
keys_[0] = key; assertInvariants(); }
/** Construct binary factor */
FactorOrdered(KeyType key1, KeyType key2) : keys_(2) {
keys_[0] = key1; keys_[1] = key2; assertInvariants(); }
/** Construct ternary factor */
FactorOrdered(KeyType key1, KeyType key2, KeyType key3) : keys_(3) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; assertInvariants(); }
/** Construct 4-way factor */
FactorOrdered(KeyType key1, KeyType key2, KeyType key3, KeyType key4) : keys_(4) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); }
/** Construct 5-way factor */
FactorOrdered(KeyType key1, KeyType key2, KeyType key3, KeyType key4, KeyType key5) : keys_(5) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; keys_[4] = key5; assertInvariants(); }
/** Construct 6-way factor */
FactorOrdered(KeyType key1, KeyType key2, KeyType key3, KeyType key4, KeyType key5, KeyType key6) : keys_(6) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; keys_[4] = key5; keys_[5] = key6; assertInvariants(); }
/// @}
/// @name Advanced Constructors
/// @{
/** Construct n-way factor */
FactorOrdered(const std::set<KeyType>& keys) {
BOOST_FOREACH(const KeyType& key, keys) keys_.push_back(key);
assertInvariants();
}
/** Construct n-way factor */
FactorOrdered(const std::vector<KeyType>& keys) : keys_(keys) {
assertInvariants();
}
/** Constructor from a collection of keys */
template<class KEYITERATOR> FactorOrdered(KEYITERATOR beginKey, KEYITERATOR endKey) :
keys_(beginKey, endKey) { assertInvariants(); }
/// @}
#ifdef TRACK_ELIMINATE
/**
* eliminate the first variable involved in this factor
* @return a conditional on the eliminated variable
*/
template<class CONDITIONAL>
typename CONDITIONAL::shared_ptr eliminateFirst();
/**
* eliminate the first nrFrontals frontal variables.
*/
template<class CONDITIONAL>
typename BayesNetOrdered<CONDITIONAL>::shared_ptr eliminate(size_t nrFrontals = 1);
#endif
/// @name Standard Interface
/// @{
/// First key
KeyType front() const { return keys_.front(); }
/// Last key
KeyType back() const { return keys_.back(); }
/// find
const_iterator find(KeyType key) const { return std::find(begin(), end(), key); }
/// Access the factor's involved variable keys
const std::vector<KeyType>& keys() const { return keys_; }
/** iterators */
const_iterator begin() const { return keys_.begin(); } ///TODO: comment
const_iterator end() const { return keys_.end(); } ///TODO: comment
/**
* @return the number of variables involved in this factor
*/
size_t size() const { return keys_.size(); }
/// @}
/// @name Testable
/// @{
/// print
void print(const std::string& s = "Factor",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/// print only keys
void printKeys(const std::string& s = "Factor",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/// check equality
bool equals(const This& other, double tol = 1e-9) const;
/// @}
/// @name Advanced Interface
/// @{
/**
* @return keys involved in this factor
*/
std::vector<KeyType>& keys() { return keys_; }
/** mutable iterators */
iterator begin() { return keys_.begin(); } ///TODO: comment
iterator end() { return keys_.end(); } ///TODO: comment
protected:
friend class JacobianFactorOrdered;
friend class HessianFactorOrdered;
/// Internal consistency check that is run frequently when in debug mode.
/// If NDEBUG is defined, this is empty and optimized out.
void assertInvariants() const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(keys_);
}
/// @}
};
}
#include <gtsam/inference/FactorOrdered-inl.h>

107
gtsam/inference/GenericMultifrontalSolver-inl.h
View File

@ -1,107 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GenericMultifrontalSolver-inl.h
* @author Richard Roberts
* @date Oct 21, 2010
*/
#pragma once
#include <gtsam/inference/FactorOrdered-inl.h>
#include <gtsam/inference/JunctionTreeOrdered.h>
#include <gtsam/inference/BayesNetOrdered-inl.h>
namespace gtsam {
/* ************************************************************************* */
template<class F, class JT>
GenericMultifrontalSolver<F, JT>::GenericMultifrontalSolver(
const FactorGraphOrdered<F>& graph) :
structure_(new VariableIndexOrdered(graph)), junctionTree_(
new JT(graph, *structure_)) {
}
/* ************************************************************************* */
template<class F, class JT>
GenericMultifrontalSolver<F, JT>::GenericMultifrontalSolver(
const sharedGraph& graph,
const VariableIndexOrdered::shared_ptr& variableIndex) :
structure_(variableIndex), junctionTree_(new JT(*graph, *structure_)) {
}
/* ************************************************************************* */
template<class F, class JT>
void GenericMultifrontalSolver<F, JT>::print(const std::string& s) const {
this->structure_->print(s + " structure:\n");
this->junctionTree_->print(s + " jtree:");
}
/* ************************************************************************* */
template<class F, class JT>
bool GenericMultifrontalSolver<F, JT>::equals(
const GenericMultifrontalSolver& expected, double tol) const {
if (!this->structure_->equals(*expected.structure_, tol)) return false;
if (!this->junctionTree_->equals(*expected.junctionTree_, tol)) return false;
return true;
}
/* ************************************************************************* */
template<class F, class JT>
void GenericMultifrontalSolver<F, JT>::replaceFactors(const sharedGraph& graph) {
junctionTree_.reset(new JT(*graph, *structure_));
}
/* ************************************************************************* */
template<class FACTOR, class JUNCTIONTREE>
typename BayesTreeOrdered<typename FACTOR::ConditionalType>::shared_ptr
GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::eliminate(Eliminate function) const {
// eliminate junction tree, returns pointer to root
typename BayesTreeOrdered<typename FACTOR::ConditionalType>::sharedClique
root = junctionTree_->eliminate(function);
// create an empty Bayes tree and insert root clique
typename BayesTreeOrdered<typename FACTOR::ConditionalType>::shared_ptr
bayesTree(new BayesTreeOrdered<typename FACTOR::ConditionalType>);
bayesTree->insert(root);
// return the Bayes tree
return bayesTree;
}
/* ************************************************************************* */
template<class F, class JT>
typename FactorGraphOrdered<F>::shared_ptr GenericMultifrontalSolver<F, JT>::jointFactorGraph(
const std::vector<Index>& js, Eliminate function) const {
// FIXME: joint for arbitrary sets of variables not present
// TODO: develop and implement theory for shortcuts of more than two variables
if (js.size() != 2) throw std::domain_error(
"*MultifrontalSolver::joint(js) currently can only compute joint marginals\n"
"for exactly two variables. You can call marginal to compute the\n"
"marginal for one variable. *SequentialSolver::joint(js) can compute the\n"
"joint marginal over any number of variables, so use that if necessary.\n");
return eliminate(function)->joint(js[0], js[1], function);
}
/* ************************************************************************* */
template<class F, class JT>
typename boost::shared_ptr<F> GenericMultifrontalSolver<F, JT>::marginalFactor(
Index j, Eliminate function) const {
return eliminate(function)->marginalFactor(j, function);
}
}

124
gtsam/inference/GenericMultifrontalSolver.h
View File

@ -1,124 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GenericMultifrontalSolver.h
* @author Richard Roberts
* @date Oct 21, 2010
*/
#pragma once
#include <utility>
#include <gtsam/inference/JunctionTreeOrdered.h>
#include <gtsam/inference/FactorGraphOrdered.h>
namespace gtsam {
/**
* A Generic Multifrontal Solver class
*
* A solver is given a factor graph at construction, and implements
* a strategy to solve it (in this case, eliminate into a Bayes tree).
* This generic one will create a Bayes tree when eliminate() is called.
*
* Takes two template arguments:
* FACTOR the factor type, e.g., GaussianFactor, DiscreteFactor
* JUNCTIONTREE annoyingly, you also have to supply a compatible JT type
* i.e., one templated on a factor graph with the same factors
* TODO: figure why this is so and possibly fix it
* \nosubgrouping
*/
template<class FACTOR, class JUNCTIONTREE>
class GenericMultifrontalSolver {
protected:
/// Column structure of the factor graph
VariableIndexOrdered::shared_ptr structure_;
/// Junction tree that performs elimination.
typename JUNCTIONTREE::shared_ptr junctionTree_;
public:
typedef typename FactorGraphOrdered<FACTOR>::shared_ptr sharedGraph;
typedef typename FactorGraphOrdered<FACTOR>::Eliminate Eliminate;
/// @name Standard Constructors
/// @{
/**
* Construct the solver for a factor graph. This builds the junction
* tree, which does the symbolic elimination, identifies the cliques,
* and distributes all the factors to the right cliques.
*/
GenericMultifrontalSolver(const FactorGraphOrdered<FACTOR>& factorGraph);
/**
* Construct the solver with a shared pointer to a factor graph and to a
* VariableIndex. The solver will store these pointers, so this constructor
* is the fastest.
*/
GenericMultifrontalSolver(const sharedGraph& factorGraph,
const VariableIndexOrdered::shared_ptr& variableIndex);
/// @}
/// @name Testable
/// @{
/** Print to cout */
void print(const std::string& name = "GenericMultifrontalSolver: ") const;
/** Test whether is equal to another */
bool equals(const GenericMultifrontalSolver& other, double tol = 1e-9) const;
/// @}
/// @name Standard Interface
/// @{
/**
* Replace the factor graph with a new one having the same structure. The
* This function can be used if the numerical part of the factors changes,
* such as during relinearization or adjusting of noise models.
*/
void replaceFactors(const sharedGraph& factorGraph);
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
typename BayesTreeOrdered<typename FACTOR::ConditionalType>::shared_ptr
eliminate(Eliminate function) const;
/**
* Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. This function returns the result as a factor
* graph.
*/
typename FactorGraphOrdered<FACTOR>::shared_ptr jointFactorGraph(
const std::vector<Index>& js, Eliminate function) const;
/**
* Compute the marginal density over a variable, by integrating out
* all of the other variables. This function returns the result as a factor.
*/
typename boost::shared_ptr<FACTOR> marginalFactor(Index j,
Eliminate function) const;
/// @}
};
} // gtsam
#include <gtsam/inference/GenericMultifrontalSolver-inl.h>

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

@ -1,203 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GenericSequentialSolver-inl.h
* @brief Implementation for generic sequential solver
* @author Richard Roberts
* @date Oct 21, 2010
*/
#pragma once
#include <gtsam/inference/FactorOrdered.h>
#include <gtsam/inference/FactorGraphOrdered.h>
#include <gtsam/inference/EliminationTreeOrdered.h>
#include <gtsam/inference/BayesNetOrdered.h>
#include <gtsam/inference/inferenceOrdered.h>
#include <boost/foreach.hpp>
namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const FactorGraphOrdered<FACTOR>& factorGraph) {
gttic(GenericSequentialSolver_constructor1);
assert(factorGraph.size());
factors_.reset(new FactorGraphOrdered<FACTOR>(factorGraph));
structure_.reset(new VariableIndexOrdered(factorGraph));
eliminationTree_ = EliminationTreeOrdered<FACTOR>::Create(*factors_, *structure_);
}
/* ************************************************************************* */
template<class FACTOR>
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(
const sharedFactorGraph& factorGraph,
const boost::shared_ptr<VariableIndexOrdered>& variableIndex)
{
gttic(GenericSequentialSolver_constructor2);
factors_ = factorGraph;
structure_ = variableIndex;
eliminationTree_ = EliminationTreeOrdered<FACTOR>::Create(*factors_, *structure_);
}
/* ************************************************************************* */
template<class FACTOR>
void GenericSequentialSolver<FACTOR>::print(const std::string& s) const {
this->factors_->print(s + " factors:");
this->structure_->print(s + " structure:\n");
this->eliminationTree_->print(s + " etree:");
}
/* ************************************************************************* */
template<class FACTOR>
bool GenericSequentialSolver<FACTOR>::equals(
const GenericSequentialSolver& expected, double tol) const {
if (!this->factors_->equals(*expected.factors_, tol))
return false;
if (!this->structure_->equals(*expected.structure_, tol))
return false;
if (!this->eliminationTree_->equals(*expected.eliminationTree_, tol))
return false;
return true;
}
/* ************************************************************************* */
template<class FACTOR>
void GenericSequentialSolver<FACTOR>::replaceFactors(
const sharedFactorGraph& factorGraph) {
// Reset this shared pointer first to deallocate if possible - for big
// problems there may not be enough memory to store two copies.
eliminationTree_.reset();
factors_ = factorGraph;
eliminationTree_ = EliminationTreeOrdered<FACTOR>::Create(*factors_, *structure_);
}
/* ************************************************************************* */
template<class FACTOR>
typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
GenericSequentialSolver<FACTOR>::eliminate(Eliminate function) const {
return eliminationTree_->eliminate(function);
}
/* ************************************************************************* */
template<class FACTOR>
typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
GenericSequentialSolver<FACTOR>::eliminate(const Permutation& permutation,
Eliminate function, boost::optional<size_t> nrToEliminate) const
{
gttic(GenericSequentialSolver_eliminate);
// Create inverse permutation
Permutation::shared_ptr permutationInverse(permutation.inverse());
// 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 boost::shared_ptr<FACTOR>& factor, *factors_)
if (factor)
factor->permuteWithInverse(*permutationInverse);
// Eliminate using elimination tree provided
typename EliminationTreeOrdered<FACTOR>::shared_ptr etree = EliminationTreeOrdered<FACTOR>::Create(*factors_);
sharedBayesNet bayesNet;
if(nrToEliminate)
bayesNet = etree->eliminatePartial(function, *nrToEliminate);
else
bayesNet = etree->eliminate(function);
// Undo the permutation on the original factors and on the structure.
BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *factors_)
if (factor)
factor->permuteWithInverse(permutation);
// Undo the permutation on the conditionals
BOOST_FOREACH(const boost::shared_ptr<Conditional>& c, *bayesNet)
c->permuteWithInverse(permutation);
return bayesNet;
}
/* ************************************************************************* */
template<class FACTOR>
typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
GenericSequentialSolver<FACTOR>::conditionalBayesNet(
const std::vector<Index>& js, size_t nrFrontals,
Eliminate function) const
{
gttic(GenericSequentialSolver_conditionalBayesNet);
// Compute a COLAMD permutation with the marginal variables constrained to the end.
// TODO in case of nrFrontals, the order of js has to be respected here !
Permutation::shared_ptr permutation(
inference::PermutationCOLAMD(*structure_, js, true));
// Eliminate only variables J \cup F from P(J,F,S) to get P(F|S)
size_t nrVariables = structure_->size();
size_t nrMarginalized = nrVariables - js.size();
size_t nrToEliminate = nrMarginalized + nrFrontals;
sharedBayesNet bayesNet = eliminate(*permutation, function, nrToEliminate);
// Get rid of conditionals on variables that we want to marginalize out
for (size_t i = 0; i < nrMarginalized; i++)
bayesNet->pop_front();
return bayesNet;
}
/* ************************************************************************* */
template<class FACTOR>
typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
GenericSequentialSolver<FACTOR>::jointBayesNet(const std::vector<Index>& js,
Eliminate function) const
{
gttic(GenericSequentialSolver_jointBayesNet);
// Compute a COLAMD permutation with the marginal variables constrained to the end.
Permutation::shared_ptr permutation(
inference::PermutationCOLAMD(*structure_, js));
// Eliminate all variables
sharedBayesNet bayesNet = eliminate(*permutation, function);
// Get rid of conditionals on variables that we want to marginalize out
size_t nrMarginalized = bayesNet->size() - js.size();
for (size_t i = 0; i < nrMarginalized; i++)
bayesNet->pop_front();
return bayesNet;
}
/* ************************************************************************* */
template<class FACTOR>
typename FactorGraphOrdered<FACTOR>::shared_ptr //
GenericSequentialSolver<FACTOR>::jointFactorGraph(
const std::vector<Index>& js, Eliminate function) const
{
gttic(GenericSequentialSolver_jointFactorGraph);
// Eliminate all variables
typename BayesNetOrdered<Conditional>::shared_ptr bayesNet = jointBayesNet(js, function);
return boost::make_shared<FactorGraphOrdered<FACTOR> >(*bayesNet);
}
/* ************************************************************************* */
template<class FACTOR>
typename boost::shared_ptr<FACTOR> //
GenericSequentialSolver<FACTOR>::marginalFactor(Index j, Eliminate function) const {
gttic(GenericSequentialSolver_marginalFactor);
// Create a container for the one variable index
std::vector<Index> js(1);
js[0] = j;
// Call joint and return the only factor in the factor graph it returns
// TODO: just call jointBayesNet and grab last conditional, then toFactor....
return (*this->jointFactorGraph(js, function))[0];
}
} // namespace gtsam

179
gtsam/inference/GenericSequentialSolver.h
View File

@ -1,179 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GenericSequentialSolver.h
* @brief generic sequential elimination
* @author Richard Roberts
* @date Oct 21, 2010
*/
#pragma once
#include <gtsam/global_includes.h>
#include <gtsam/base/Testable.h>
#include <boost/function.hpp>
#include <boost/optional.hpp>
#include <utility>
#include <vector>
namespace gtsam {
class VariableIndexOrdered;
class Permutation;
}
namespace gtsam {
template<class FACTOR> class EliminationTreeOrdered;
}
namespace gtsam {
template<class FACTOR> class FactorGraphOrdered;
}
namespace gtsam {
template<class CONDITIONAL> class BayesNetOrdered;
}
namespace gtsam {
/**
* This solver implements sequential variable elimination for factor graphs.
* Underlying this is a column elimination tree, see Gilbert 2001 BIT.
*
* The elimination ordering is "baked in" to the variable indices at this
* stage, i.e. elimination proceeds in order from '0'.
*
* This is not the most efficient algorithm we provide, most efficient is the
* MultifrontalSolver, which examines and uses the clique structure.
* However, sequential variable elimination is easier to understand so this is a good
* starting point to learn about these algorithms and our implementation.
* Additionally, the first step of MFQR is symbolic sequential elimination.
* \nosubgrouping
*/
template<class FACTOR>
class GenericSequentialSolver {
protected:
typedef boost::shared_ptr<FactorGraphOrdered<FACTOR> > sharedFactorGraph;
typedef typename FACTOR::ConditionalType Conditional;
typedef typename boost::shared_ptr<Conditional> sharedConditional;
typedef typename boost::shared_ptr<BayesNetOrdered<Conditional> > sharedBayesNet;
typedef std::pair<boost::shared_ptr<Conditional>, boost::shared_ptr<FACTOR> > EliminationResult;
typedef boost::function<
EliminationResult(const FactorGraphOrdered<FACTOR>&, size_t)> Eliminate;
/** Store the original factors for computing marginals
* TODO Frank says: really? Marginals should be computed from result.
*/
sharedFactorGraph factors_;
/** Store column structure of the factor graph. Why? */
boost::shared_ptr<VariableIndexOrdered> structure_;
/** Elimination tree that performs elimination */
boost::shared_ptr<EliminationTreeOrdered<FACTOR> > eliminationTree_;
/** concept checks */
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
// GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTreeOrdered)
/**
* Eliminate in a different order, given a permutation
*/
sharedBayesNet eliminate(const Permutation& permutation, Eliminate function,
boost::optional<size_t> nrToEliminate = boost::none ///< If given a number of variables to eliminate, will only eliminate that many
) const;
public:
/// @name Standard Constructors
/// @{
/**
* Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the work of elimination.
*/
GenericSequentialSolver(const FactorGraphOrdered<FACTOR>& factorGraph);
/**
* Construct the solver with a shared pointer to a factor graph and to a
* VariableIndex. The solver will store these pointers, so this constructor
* is the fastest.
*/
GenericSequentialSolver(const sharedFactorGraph& factorGraph,
const boost::shared_ptr<VariableIndexOrdered>& variableIndex);
/// @}
/// @name Testable
/// @{
/** Print to cout */
void print(const std::string& name = "GenericSequentialSolver: ") const;
/** Test whether is equal to another */
bool equals(const GenericSequentialSolver& other, double tol = 1e-9) const;
/// @}
/// @name Standard Interface
/// @{
/**
* Replace the factor graph with a new one having the same structure. The
* This function can be used if the numerical part of the factors changes,
* such as during relinearization or adjusting of noise models.
*/
void replaceFactors(const sharedFactorGraph& factorGraph);
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
sharedBayesNet eliminate(Eliminate function) const;
/**
* Compute a conditional density P(F|S) while marginalizing out variables J
* P(F|S) is obtained by P(J,F,S)=P(J|F,S)P(F|S)P(S) and dropping P(S)
* Returns the result as a Bayes net.
*/
sharedBayesNet
conditionalBayesNet(const std::vector<Index>& js, size_t nrFrontals,
Eliminate function) const;
/**
* Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. Returns the result as a Bayes net
*/
sharedBayesNet
jointBayesNet(const std::vector<Index>& js, Eliminate function) const;
/**
* Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. Returns the result as a factor graph.
*/
typename FactorGraphOrdered<FACTOR>::shared_ptr
jointFactorGraph(const std::vector<Index>& js, Eliminate function) const;
/**
* 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 boost::shared_ptr<FACTOR>
marginalFactor(Index j, Eliminate function) const;
/// @}
}
;
// GenericSequentialSolver
}// namespace gtsam
#include <gtsam/inference/GenericSequentialSolver-inl.h>

74
gtsam/inference/ISAMOrdered-inl.h
View File

@ -1,74 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 ISAM-inl.h
* @brief Incremental update functionality (iSAM) for BayesTree.
* @author Michael Kaess
*/
#pragma once
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <gtsam/inference/ConditionalOrdered.h>
#include <gtsam/inference/BayesTreeOrdered.h>
#include <gtsam/inference/GenericMultifrontalSolver.h>
namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
ISAMOrdered<CONDITIONAL>::ISAMOrdered() : BayesTreeOrdered<CONDITIONAL>() {}
/* ************************************************************************* */
template<class CONDITIONAL>
template<class FG> void ISAMOrdered<CONDITIONAL>::update_internal(
const FG& newFactors, Cliques& orphans, typename FG::Eliminate function) {
// Remove the contaminated part of the Bayes tree
// Throw exception if disconnected
BayesNetOrdered<CONDITIONAL> bn;
if (!this->empty()) {
this->removeTop(newFactors.keys(), bn, orphans);
if (bn.empty())
throw std::runtime_error(
"ISAM::update_internal(): no variables in common between existing Bayes tree and incoming factors!");
}
FG factors(bn);
// add the factors themselves
factors.push_back(newFactors);
// eliminate into a Bayes net
GenericMultifrontalSolver<typename CONDITIONAL::FactorType, JunctionTreeOrdered<FG> > solver(factors);
boost::shared_ptr<BayesTreeOrdered<CONDITIONAL> > bayesTree;
bayesTree = solver.eliminate(function);
this->root_ = bayesTree->root();
this->nodes_ = bayesTree->nodes();
// add orphans to the bottom of the new tree
BOOST_FOREACH(sharedClique orphan, orphans)
this->insert(orphan);
}
/* ************************************************************************* */
template<class CONDITIONAL>
template<class FG>
void ISAMOrdered<CONDITIONAL>::update(const FG& newFactors,
typename FG::Eliminate function) {
Cliques orphans;
this->update_internal(newFactors, orphans, function);
}
}
/// namespace gtsam

76
gtsam/inference/ISAMOrdered.h
View File

@ -1,76 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 ISAM.h
* @brief Incremental update functionality (iSAM) for BayesTree.
* @author Michael Kaess
*/
// \callgraph
#pragma once
#include <gtsam/inference/BayesTreeOrdered.h>
namespace gtsam {
/**
* A Bayes tree with an update methods that implements the iSAM algorithm.
* Given a set of new factors, it re-eliminates the invalidated part of the tree.
* \nosubgrouping
*/
template<class CONDITIONAL>
class ISAMOrdered: public BayesTreeOrdered<CONDITIONAL> {
private:
typedef BayesTreeOrdered<CONDITIONAL> Base;
public:
/// @name Standard Constructors
/// @{
/** Create an empty Bayes Tree */
ISAMOrdered();
/** Copy constructor */
ISAMOrdered(const Base& bayesTree) :
Base(bayesTree) {
}
/// @}
/// @name Advanced Interface Interface
/// @{
/**
* update the Bayes tree with a set of new factors, typically derived from measurements
* @param newFactors is a factor graph that contains the new factors
* @param function an elimination routine
*/
template<class FG>
void update(const FG& newFactors, typename FG::Eliminate function);
typedef typename BayesTreeOrdered<CONDITIONAL>::sharedClique sharedClique; ///<TODO: comment
typedef typename BayesTreeOrdered<CONDITIONAL>::Cliques Cliques; ///<TODO: comment
/** update_internal provides access to list of orphans for drawing purposes */
template<class FG>
void update_internal(const FG& newFactors, Cliques& orphans,
typename FG::Eliminate function);
/// @}
};
}/// namespace gtsam
#include <gtsam/inference/ISAMOrdered-inl.h>

65
gtsam/inference/IndexConditionalOrdered.cpp
View File

@ -1,65 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 IndexConditional.cpp
* @author Richard Roberts
* @date Oct 17, 2010
*/
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/lambda/lambda.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
namespace gtsam {
using namespace std;
using namespace boost::lambda;
/* ************************************************************************* */
void IndexConditionalOrdered::assertInvariants() const {
// Checks for uniqueness of keys
Base::assertInvariants();
}
/* ************************************************************************* */
bool IndexConditionalOrdered::reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
BOOST_FOREACH(KeyType key, frontals()) { assert(inverseReduction.find(key) == inverseReduction.end()); }
#endif
bool parentChanged = false;
BOOST_FOREACH(KeyType& parent, parents()) {
internal::Reduction::const_iterator it = inverseReduction.find(parent);
if(it != inverseReduction.end()) {
parentChanged = true;
parent = it->second;
}
}
assertInvariants();
return parentChanged;
}
/* ************************************************************************* */
void IndexConditionalOrdered::permuteWithInverse(const Permutation& inversePermutation) {
BOOST_FOREACH(Index& key, keys())
key = inversePermutation[key];
assertInvariants();
}
/* ************************************************************************* */
}

135
gtsam/inference/IndexConditionalOrdered.h
View File

@ -1,135 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 IndexConditional.h
* @author Richard Roberts
* @date Oct 17, 2010
*/
#pragma once
#include <gtsam/global_includes.h>
#include <gtsam/inference/ConditionalOrdered.h>
#include <gtsam/inference/IndexFactorOrdered.h>
#include <gtsam/inference/PermutationOrdered.h>
namespace gtsam {
/**
* IndexConditional serves two purposes. It is the base class for
* GaussianConditional, and also functions as a symbolic conditional with
* Index keys, produced by symbolic elimination of IndexFactor.
*
* It derives from Conditional with a key type of Index, which is an
* unsigned integer.
* \nosubgrouping
*/
class IndexConditionalOrdered : public ConditionalOrdered<Index> {
protected:
// Checks that frontal indices are sorted and lower than parent indices
GTSAM_EXPORT void assertInvariants() const;
public:
typedef IndexConditionalOrdered This;
typedef ConditionalOrdered<Index> Base;
typedef IndexFactorOrdered FactorType;
typedef boost::shared_ptr<IndexConditionalOrdered> shared_ptr;
/// @name Standard Constructors
/// @{
/** Empty Constructor to make serialization possible */
IndexConditionalOrdered() { assertInvariants(); }
/** No parents */
IndexConditionalOrdered(Index j) : Base(j) { assertInvariants(); }
/** Single parent */
IndexConditionalOrdered(Index j, Index parent) : Base(j, parent) { assertInvariants(); }
/** Two parents */
IndexConditionalOrdered(Index j, Index parent1, Index parent2) : Base(j, parent1, parent2) { assertInvariants(); }
/** Three parents */
IndexConditionalOrdered(Index j, Index parent1, Index parent2, Index parent3) : Base(j, parent1, parent2, parent3) { assertInvariants(); }
/// @}
/// @name Advanced Constructors
/// @{
/** Constructor from a frontal variable and a vector of parents */
IndexConditionalOrdered(Index j, const std::vector<Index>& parents) : Base(j, parents) {
assertInvariants();
}
/** Constructor from keys and nr of frontal variables */
IndexConditionalOrdered(const std::vector<Index>& keys, size_t nrFrontals) :
Base(keys, nrFrontals) {
assertInvariants();
}
/** Constructor from keys and nr of frontal variables */
IndexConditionalOrdered(const std::list<Index>& keys, size_t nrFrontals) :
Base(keys, nrFrontals) {
assertInvariants();
}
/// @}
/// @name Standard Interface
/// @{
/** Named constructor directly returning a shared pointer */
template<class KEYS>
static shared_ptr FromKeys(const KEYS& keys, size_t nrFrontals) {
shared_ptr conditional(new IndexConditionalOrdered());
conditional->keys_.assign(keys.begin(), keys.end());
conditional->nrFrontals_ = nrFrontals;
return conditional;
}
/** Convert to a factor */
IndexFactorOrdered::shared_ptr toFactor() const {
return IndexFactorOrdered::shared_ptr(new IndexFactorOrdered(*this));
}
/// @}
/// @name Advanced Interface
/// @{
/** Permute the variables when only separator variables need to be permuted.
* Returns true if any reordered variables appeared in the separator and
* false if not.
*/
GTSAM_EXPORT bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction);
/**
* Permutes the Conditional, but for efficiency requires the permutation
* to already be inverted.
*/
GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
/// @}
};
}

71
gtsam/inference/IndexFactorOrdered.cpp
View File

@ -1,71 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 IndexFactor.cpp
* @author Richard Roberts
* @date Oct 17, 2010
*/
#include <gtsam/inference/IndexFactorOrdered.h>
#include <gtsam/inference/FactorOrdered.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
void IndexFactorOrdered::assertInvariants() const {
Base::assertInvariants();
}
/* ************************************************************************* */
IndexFactorOrdered::IndexFactorOrdered(const IndexConditionalOrdered& c) :
Base(c) {
assertInvariants();
}
/* ************************************************************************* */
#ifdef TRACK_ELIMINATE
boost::shared_ptr<IndexConditionalOrdered> IndexFactorOrdered::eliminateFirst() {
boost::shared_ptr<IndexConditionalOrdered> result(Base::eliminateFirst<
IndexConditionalOrdered>());
assertInvariants();
return result;
}
/* ************************************************************************* */
boost::shared_ptr<BayesNetOrdered<IndexConditionalOrdered> > IndexFactorOrdered::eliminate(
size_t nrFrontals) {
boost::shared_ptr<BayesNetOrdered<IndexConditionalOrdered> > result(Base::eliminate<
IndexConditionalOrdered>(nrFrontals));
assertInvariants();
return result;
}
#endif
/* ************************************************************************* */
void IndexFactorOrdered::permuteWithInverse(const Permutation& inversePermutation) {
BOOST_FOREACH(Index& key, keys())
key = inversePermutation[key];
assertInvariants();
}
/* ************************************************************************* */
void IndexFactorOrdered::reduceWithInverse(const internal::Reduction& inverseReduction) {
BOOST_FOREACH(Index& key, keys())
key = inverseReduction[key];
assertInvariants();
}
/* ************************************************************************* */
} // gtsam

179
gtsam/inference/IndexFactorOrdered.h
View File

@ -1,179 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 IndexFactor.h
* @author Richard Roberts
* @date Oct 17, 2010
*/
#pragma once
#include <gtsam/inference/FactorOrdered.h>
#include <gtsam/inference/PermutationOrdered.h>
namespace gtsam {
// Forward declaration of IndexConditional
class IndexConditionalOrdered;
/**
* IndexFactor serves two purposes. It is the base class for all linear
* factors (GaussianFactor, JacobianFactor, HessianFactor), and also
* functions as a symbolic factor with Index keys, used to do symbolic
* elimination by JunctionTree.
*
* It derives from Factor with a key type of Index, an unsigned integer.
* \nosubgrouping
*/
class IndexFactorOrdered: public FactorOrdered<Index> {
protected:
/// @name Advanced Interface
/// @{
/// Internal function for checking class invariants (unique keys for this factor)
GTSAM_EXPORT void assertInvariants() const;
/// @}
public:
typedef IndexFactorOrdered This;
typedef FactorOrdered<Index> Base;
/** Elimination produces an IndexConditional */
typedef IndexConditionalOrdered ConditionalType;
/** Overriding the shared_ptr typedef */
typedef boost::shared_ptr<IndexFactorOrdered> shared_ptr;
/// @name Standard Interface
/// @{
/** Copy constructor */
IndexFactorOrdered(const This& f) :
Base(f) {
assertInvariants();
}
/** Construct from derived type */
GTSAM_EXPORT IndexFactorOrdered(const IndexConditionalOrdered& c);
/** Default constructor for I/O */
IndexFactorOrdered() {
assertInvariants();
}
/** Construct unary factor */
IndexFactorOrdered(Index j) :
Base(j) {
assertInvariants();
}
/** Construct binary factor */
IndexFactorOrdered(Index j1, Index j2) :
Base(j1, j2) {
assertInvariants();
}
/** Construct ternary factor */
IndexFactorOrdered(Index j1, Index j2, Index j3) :
Base(j1, j2, j3) {
assertInvariants();
}
/** Construct 4-way factor */
IndexFactorOrdered(Index j1, Index j2, Index j3, Index j4) :
Base(j1, j2, j3, j4) {
assertInvariants();
}
/** Construct 5-way factor */
IndexFactorOrdered(Index j1, Index j2, Index j3, Index j4, Index j5) :
Base(j1, j2, j3, j4, j5) {
assertInvariants();
}
/** Construct 6-way factor */
IndexFactorOrdered(Index j1, Index j2, Index j3, Index j4, Index j5, Index j6) :
Base(j1, j2, j3, j4, j5, j6) {
assertInvariants();
}
/// @}
/// @name Advanced Constructors
/// @{
/** Construct n-way factor */
IndexFactorOrdered(const std::set<Index>& js) :
Base(js) {
assertInvariants();
}
/** Construct n-way factor */
IndexFactorOrdered(const std::vector<Index>& js) :
Base(js) {
assertInvariants();
}
/** Constructor from a collection of keys */
template<class KeyIterator> IndexFactorOrdered(KeyIterator beginKey,
KeyIterator endKey) :
Base(beginKey, endKey) {
assertInvariants();
}
/// @}
#ifdef TRACK_ELIMINATE
/**
* eliminate the first variable involved in this factor
* @return a conditional on the eliminated variable
*/
GTSAM_EXPORT boost::shared_ptr<ConditionalType> eliminateFirst();
/** eliminate the first nrFrontals frontal variables.*/
GTSAM_EXPORT boost::shared_ptr<BayesNetOrdered<ConditionalType> > eliminate(size_t nrFrontals =
1);
#endif
/// @name Advanced Interface
/// @{
/**
* Permutes the factor, but for efficiency requires the permutation
* to already be inverted.
*/
GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation);
/**
* Apply a reduction, which is a remapping of variable indices.
*/
GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction);
virtual ~IndexFactorOrdered() {
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
/// @}
}; // IndexFactor
}

207
gtsam/inference/JunctionTreeOrdered-inl.h
View File

@ -1,207 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 JunctionTree-inl.h
* @date Feb 4, 2010
* @author Kai Ni
* @author Frank Dellaert
* @brief The junction tree, template bodies
*/
#pragma once
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/EliminationTreeOrdered.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
namespace gtsam {
/* ************************************************************************* */
template <class FG, class BTCLIQUE>
void JunctionTreeOrdered<FG,BTCLIQUE>::construct(const FG& fg, const VariableIndexOrdered& variableIndex) {
gttic(JT_construct);
gttic(JT_symbolic_ET);
const typename EliminationTreeOrdered<IndexFactorOrdered>::shared_ptr symETree =
EliminationTreeOrdered<IndexFactorOrdered>::Create(fg, variableIndex);
assert(symETree.get());
gttoc(JT_symbolic_ET);
gttic(JT_symbolic_eliminate);
SymbolicBayesNetOrdered::shared_ptr sbn = symETree->eliminate(&EliminateSymbolic);
assert(sbn.get());
gttoc(JT_symbolic_eliminate);
gttic(symbolic_BayesTree);
SymbolicBayesTree sbt(*sbn);
gttoc(symbolic_BayesTree);
// distribute factors
gttic(distributeFactors);
this->root_ = distributeFactors(fg, sbt.root());
gttoc(distributeFactors);
}
/* ************************************************************************* */
template <class FG, class BTCLIQUE>
JunctionTreeOrdered<FG,BTCLIQUE>::JunctionTreeOrdered(const FG& fg) {
gttic(VariableIndexOrdered);
VariableIndexOrdered varIndex(fg);
gttoc(VariableIndexOrdered);
construct(fg, varIndex);
}
/* ************************************************************************* */
template <class FG, class BTCLIQUE>
JunctionTreeOrdered<FG,BTCLIQUE>::JunctionTreeOrdered(const FG& fg, const VariableIndexOrdered& variableIndex) {
construct(fg, variableIndex);
}
/* ************************************************************************* */
template<class FG, class BTCLIQUE>
typename JunctionTreeOrdered<FG,BTCLIQUE>::sharedClique JunctionTreeOrdered<FG,BTCLIQUE>::distributeFactors(
const FG& fg, const SymbolicBayesTree::sharedClique& bayesClique) {
// Build "target" index. This is an index for each variable of the factors
// that involve this variable as their *lowest-ordered* variable. For each
// factor, it is the lowest-ordered variable of that factor that pulls the
// factor into elimination, after which all of the information in the
// factor is contained in the eliminated factors that are passed up the
// tree as elimination continues.
// Two stages - first build an array of the lowest-ordered variable in each
// factor and find the last variable to be eliminated.
std::vector<Index> lowestOrdered(fg.size(), std::numeric_limits<Index>::max());
Index maxVar = 0;
for(size_t i=0; i<fg.size(); ++i)
if(fg[i]) {
typename FG::FactorType::const_iterator min = std::min_element(fg[i]->begin(), fg[i]->end());
if(min != fg[i]->end()) {
lowestOrdered[i] = *min;
maxVar = std::max(maxVar, *min);
}
}
// Now add each factor to the list corresponding to its lowest-ordered
// variable.
std::vector<FastList<size_t> > targets(maxVar+1);
for(size_t i=0; i<lowestOrdered.size(); ++i)
if(lowestOrdered[i] != std::numeric_limits<Index>::max())
targets[lowestOrdered[i]].push_back(i);
// Now call the recursive distributeFactors
return distributeFactors(fg, targets, bayesClique);
}
/* ************************************************************************* */
template<class FG, class BTCLIQUE>
typename JunctionTreeOrdered<FG,BTCLIQUE>::sharedClique JunctionTreeOrdered<FG,BTCLIQUE>::distributeFactors(const FG& fg,
const std::vector<FastList<size_t> >& targets,
const SymbolicBayesTree::sharedClique& bayesClique) {
if(bayesClique) {
// create a new clique in the junction tree
sharedClique clique(new Clique((*bayesClique)->beginFrontals(), (*bayesClique)->endFrontals(),
(*bayesClique)->beginParents(), (*bayesClique)->endParents()));
// count the factors for this cluster to pre-allocate space
{
size_t nFactors = 0;
BOOST_FOREACH(const Index frontal, clique->frontal) {
// There may be less variables in "targets" than there really are if
// some of the highest-numbered variables do not pull in any factors.
if(frontal < targets.size())
nFactors += targets[frontal].size(); }
clique->reserve(nFactors);
}
// add the factors to this cluster
BOOST_FOREACH(const Index frontal, clique->frontal) {
if(frontal < targets.size()) {
BOOST_FOREACH(const size_t factorI, targets[frontal]) {
clique->push_back(fg[factorI]); } } }
// recursively call the children
BOOST_FOREACH(const typename SymbolicBayesTree::sharedClique bayesChild, bayesClique->children()) {
sharedClique child = distributeFactors(fg, targets, bayesChild);
clique->addChild(child);
child->parent() = clique;
}
return clique;
} else
return sharedClique();
}
/* ************************************************************************* */
template<class FG, class BTCLIQUE>
std::pair<typename JunctionTreeOrdered<FG,BTCLIQUE>::BTClique::shared_ptr,
typename FG::sharedFactor> JunctionTreeOrdered<FG,BTCLIQUE>::eliminateOneClique(
typename FG::Eliminate function,
const boost::shared_ptr<const Clique>& current) const {
FG fg; // factor graph will be assembled from local factors and marginalized children
fg.reserve(current->size() + current->children().size());
fg.push_back(*current); // add the local factors
// receive the factors from the child and its clique point
std::list<typename BTClique::shared_ptr> children;
BOOST_FOREACH(const boost::shared_ptr<const Clique>& child, current->children()) {
std::pair<typename BTClique::shared_ptr, typename FG::sharedFactor> tree_factor(
eliminateOneClique(function, child));
children.push_back(tree_factor.first);
fg.push_back(tree_factor.second);
}
// eliminate the combined factors
// warning: fg is being eliminated in-place and will contain marginal afterwards
// Now that we know which factors and variables, and where variables
// come from and go to, create and eliminate the new joint factor.
gttic(CombineAndEliminate);
typename FG::EliminationResult eliminated(function(fg,
current->frontal.size()));
gttoc(CombineAndEliminate);
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
assert(std::equal(eliminated.second->begin(), eliminated.second->end(), current->separator.begin()));
#endif
gttic(Update_tree);
// create a new clique corresponding the combined factors
typename BTClique::shared_ptr new_clique(BTClique::Create(eliminated));
new_clique->children_ = children;
BOOST_FOREACH(typename BTClique::shared_ptr& childRoot, children) {
childRoot->parent_ = new_clique;
}
gttoc(Update_tree);
return std::make_pair(new_clique, eliminated.second);
}
/* ************************************************************************* */
template<class FG, class BTCLIQUE>
typename BTCLIQUE::shared_ptr JunctionTreeOrdered<FG,BTCLIQUE>::eliminate(
typename FG::Eliminate function) const
{
if (this->root()) {
gttic(JT_eliminate);
std::pair<typename BTClique::shared_ptr, typename FG::sharedFactor> ret =
this->eliminateOneClique(function, this->root());
if (ret.second->size() != 0) throw std::runtime_error(
"JuntionTree::eliminate: elimination failed because of factors left over!");
gttoc(JT_eliminate);
return ret.first;
} else
return typename BTClique::shared_ptr();
}
} //namespace gtsam

135
gtsam/inference/JunctionTreeOrdered.h
View File

@ -1,135 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 JunctionTree.h
* @date Feb 4, 2010
* @author Kai Ni
* @author Frank Dellaert
* @brief: The junction tree
*/
#pragma once
#include <set>
#include <vector>
#include <gtsam/base/FastList.h>
#include <gtsam/inference/BayesTreeOrdered.h>
#include <gtsam/inference/ClusterTreeOrdered.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
#include <gtsam/inference/VariableIndexOrdered.h>
namespace gtsam {
/**
* A ClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with
* the additional property that it represents the clique tree associated with a Bayes net.
*
* In GTSAM a junction tree is an intermediate data structure in multifrontal
* variable elimination. Each node is a cluster of factors, along with a
* clique of variables that are eliminated all at once. In detail, every node k represents
* a clique (maximal fully connected subset) of an associated chordal graph, such as a
* chordal Bayes net resulting from elimination.
*
* The difference with the BayesTree is that a JunctionTree stores factors, whereas a
* BayesTree stores conditionals, that are the product of eliminating the factors in the
* corresponding JunctionTree cliques.
*
* The tree structure and elimination method are exactly analagous to the EliminationTree,
* except that in the JunctionTree, at each node multiple variables are eliminated at a time.
*
*
* \addtogroup Multifrontal
* \nosubgrouping
*/
template<class FG, class BTCLIQUE=typename BayesTreeOrdered<typename FG::FactorType::ConditionalType>::Clique>
class JunctionTreeOrdered: public ClusterTreeOrdered<FG> {
public:
/// In a junction tree each cluster is associated with a clique
typedef typename ClusterTreeOrdered<FG>::Cluster Clique;
typedef typename Clique::shared_ptr sharedClique; ///< Shared pointer to a clique
/// The BayesTree type produced by elimination
typedef BTCLIQUE BTClique;
/// Shared pointer to this class
typedef boost::shared_ptr<JunctionTreeOrdered<FG> > shared_ptr;
/// We will frequently refer to a symbolic Bayes tree, used to find the clique structure
typedef gtsam::BayesTreeOrdered<IndexConditionalOrdered> SymbolicBayesTree;
private:
/// @name Advanced Interface
/// @{
/// distribute the factors along the cluster tree
sharedClique distributeFactors(const FG& fg,
const SymbolicBayesTree::sharedClique& clique);
/// distribute the factors along the cluster tree
sharedClique distributeFactors(const FG& fg, const std::vector<FastList<size_t> >& targets,
const SymbolicBayesTree::sharedClique& clique);
/// recursive elimination function
std::pair<typename BTClique::shared_ptr, typename FG::sharedFactor>
eliminateOneClique(typename FG::Eliminate function,
const boost::shared_ptr<const Clique>& clique) const;
/// internal constructor
void construct(const FG& fg, const VariableIndexOrdered& variableIndex);
/// @}
public:
/// @name Standard Constructors
/// @{
/** Default constructor */
JunctionTreeOrdered() {}
/** Named constructor to build the junction tree of a factor graph. Note
* that this has to compute the column structure as a VariableIndex, so if you
* already have this precomputed, use the JunctionTree(const FG&, const VariableIndex&)
* constructor instead.
* @param factorGraph The factor graph for which to build the elimination tree
*/
JunctionTreeOrdered(const FG& factorGraph);
/** Construct from a factor graph and pre-computed variable index.
* @param fg The factor graph for which to build the junction tree
* @param structure The set of factors involving each variable. If this is not
* precomputed, you can call the JunctionTree(const FG&)
* constructor instead.
*/
JunctionTreeOrdered(const FG& fg, const VariableIndexOrdered& variableIndex);
/// @}
/// @name Standard Interface
/// @{
/** Eliminate the factors in the subgraphs to produce a BayesTree.
* @param function The function used to eliminate, see the namespace functions
* in GaussianFactorGraph.h
* @return The BayesTree resulting from elimination
*/
typename BTClique::shared_ptr eliminate(typename FG::Eliminate function) const;
/// @}
}; // JunctionTree
} // namespace gtsam
#include <gtsam/inference/JunctionTreeOrdered-inl.h>

226
gtsam/inference/PermutationOrdered.cpp
View File

@ -1,226 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 Permutation.cpp
* @author Richard Roberts
* @date Oct 9, 2010
*/
#include <gtsam/inference/PermutationOrdered.h>
#include <iostream>
#include <sstream>
#include <stdexcept>
#include <boost/foreach.hpp>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
Permutation Permutation::Identity(Index nVars) {
Permutation ret(nVars);
for(Index i=0; i<nVars; ++i)
ret[i] = i;
return ret;
}
/* ************************************************************************* */
Permutation Permutation::PullToFront(const vector<Index>& toFront, size_t size, bool filterDuplicates) {
Permutation ret(size);
// Mask of which variables have been pulled, used to reorder
vector<bool> pulled(size, false);
// Put the pulled variables at the front of the permutation and set up the
// pulled flags.
size_t toFrontUniqueSize = 0;
for(Index j=0; j<toFront.size(); ++j) {
if(!pulled[toFront[j]]) {
ret[j] = toFront[j];
pulled[toFront[j]] = true;
++ toFrontUniqueSize;
} else if(!filterDuplicates) {
stringstream ss;
ss << "Duplicate variable given as input to Permutation::PullToFront:\n";
ss << " toFront:";
BOOST_FOREACH(Index i, toFront) { ss << " " << i; }
ss << ", size = " << size << endl;
throw invalid_argument(ss.str());
}
}
// Fill in the rest of the variables
Index nextVar = toFrontUniqueSize;
for(Index j=0; j<size; ++j)
if(!pulled[j])
ret[nextVar++] = j;
assert(nextVar == size);
return ret;
}
/* ************************************************************************* */
Permutation Permutation::PushToBack(const std::vector<Index>& toBack, size_t size, bool filterDuplicates) {
Permutation ret(size);
// Mask of which variables have been pushed, used to reorder
vector<bool> pushed(size, false);
// Put the pushed variables at the back of the permutation and set up the
// pushed flags;
Index nextVar = size;
size_t toBackUniqueSize = 0;
if(toBack.size() > 0) {
Index j = toBack.size();
do {
-- j;
if(!pushed[toBack[j]]) {
ret[--nextVar] = toBack[j];
pushed[toBack[j]] = true;
++ toBackUniqueSize;
} else if(!filterDuplicates) {
stringstream ss;
ss << "Duplicate variable given as input to Permutation::PushToBack:\n";
ss << " toBack:";
BOOST_FOREACH(Index i, toBack) { ss << " " << i; }
ss << ", size = " << size << endl;
throw invalid_argument(ss.str());
}
} while(j > 0);
}
assert(nextVar == size - toBackUniqueSize);
// Fill in the rest of the variables
nextVar = 0;
for(Index j=0; j<size; ++j)
if(!pushed[j])
ret[nextVar++] = j;
assert(nextVar == size - toBackUniqueSize);
return ret;
}
/* ************************************************************************* */
Permutation::shared_ptr Permutation::permute(const Permutation& permutation) const {
const size_t nVars = permutation.size();
Permutation::shared_ptr result(new Permutation(nVars));
for(size_t j=0; j<nVars; ++j) {
assert(permutation[j] < rangeIndices_.size());
(*result)[j] = operator[](permutation[j]);
}
return result;
}
/* ************************************************************************* */
Permutation::shared_ptr Permutation::inverse() const {
Permutation::shared_ptr result(new Permutation(this->size()));
for(Index i=0; i<this->size(); ++i) {
assert((*this)[i] < this->size());
(*result)[(*this)[i]] = i;
}
return result;
}
/* ************************************************************************* */
void Permutation::print(const std::string& str) const {
std::cout << str << " ";
BOOST_FOREACH(Index s, rangeIndices_) { std::cout << s << " "; }
std::cout << std::endl;
}
namespace internal {
/* ************************************************************************* */
Reduction Reduction::CreateAsInverse(const Permutation& p) {
Reduction result;
for(Index j = 0; j < p.size(); ++j)
result.insert(std::make_pair(p[j], j));
return result;
}
/* ************************************************************************* */
Reduction Reduction::CreateFromPartialPermutation(const Permutation& selector, const Permutation& p) {
if(selector.size() != p.size())
throw invalid_argument("internal::Reduction::CreateFromPartialPermutation called with selector and permutation of different sizes");
Reduction result;
for(size_t dstSlot = 0; dstSlot < p.size(); ++dstSlot)
result.insert(make_pair(selector[dstSlot], selector[p[dstSlot]]));
return result;
}
/* ************************************************************************* */
void Reduction::applyInverse(std::vector<Index>& js) const {
BOOST_FOREACH(Index& j, js) {
j = this->find(j)->second;
}
}
/* ************************************************************************* */
Permutation Reduction::inverse() const {
Index maxIndex = 0;
BOOST_FOREACH(const value_type& target_source, *this) {
if(target_source.second > maxIndex)
maxIndex = target_source.second;
}
Permutation result(maxIndex + 1);
BOOST_FOREACH(const value_type& target_source, *this) {
result[target_source.second] = target_source.first;
}
return result;
}
/* ************************************************************************* */
const Index& Reduction::operator[](const Index& j) {
iterator it = this->find(j);
if(it == this->end())
return j;
else
return it->second;
}
/* ************************************************************************* */
const Index& Reduction::operator[](const Index& j) const {
const_iterator it = this->find(j);
if(it == this->end())
return j;
else
return it->second;
}
/* ************************************************************************* */
void Reduction::print(const std::string& s) const {
cout << s << " reduction:" << endl;
BOOST_FOREACH(const value_type& p, *this)
cout << " " << p.first << " : " << p.second << endl;
}
/* ************************************************************************* */
bool Reduction::equals(const Reduction& other, double tol) const {
return (const Base&)(*this) == (const Base&)other;
}
/* ************************************************************************* */
Permutation createReducingPermutation(const std::set<Index>& indices) {
Permutation p(indices.size());
Index newJ = 0;
BOOST_FOREACH(Index j, indices) {
p[newJ] = j;
++ newJ;
}
return p;
}
} // \namespace internal
/* ************************************************************************* */
} // \namespace gtsam

215
gtsam/inference/PermutationOrdered.h
View File

@ -1,215 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 Permutation.h
* @author Richard Roberts
* @date Sep 12, 2010
*/
#pragma once
#include <vector>
#include <string>
#include <set>
#include <iostream>
#include <boost/shared_ptr.hpp>
#include <gtsam/global_includes.h>
#include <gtsam/base/FastMap.h>
namespace gtsam {
/**
* A permutation reorders variables, for example to reduce fill-in during
* elimination. To save computation, the permutation can be applied to
* the necessary data structures only once, then multiple computations
* performed on the permuted problem. For example, in an iterative
* non-linear setting, a permutation can be created from the symbolic graph
* structure and applied to the ordering of nonlinear variables once, so
* that linearized factor graphs are already correctly ordered and need
* not be permuted.
*
* Consistent with their mathematical definition, permutations are functions
* that accept the destination index and return the source index. For example,
* to permute data structure A into a new data structure B using permutation P,
* the mapping is B[i] = A[P[i]]. This is the behavior implemented by
* B = A.permute(P).
*
* For some data structures in GTSAM, for efficiency it is necessary to have
* the inverse of the desired permutation. In this case, the data structure
* will implement permuteWithInverse(P) instead of permute(P). You may already
* have the inverse permutation by construction, or may instead compute it with
* Pinv = P.inverse().
*
* Permutations can be composed and inverted
* in order to create new indexing for a structure.
* \nosubgrouping
*/
class GTSAM_EXPORT Permutation {
protected:
std::vector<Index> rangeIndices_;
public:
typedef boost::shared_ptr<Permutation> shared_ptr;
typedef std::vector<Index>::const_iterator const_iterator;
typedef std::vector<Index>::iterator iterator;
/// @name Standard Constructors
/// @{
/**
* Create an empty permutation. This cannot do anything, but you can later
* assign to it.
*/
Permutation() {}
/**
* Create an uninitialized permutation. You must assign all values using the
* square bracket [] operator or they will be undefined!
*/
Permutation(Index nVars) : rangeIndices_(nVars) {}
/// @}
/// @name Testable
/// @{
/** Print */
void print(const std::string& str = "Permutation: ") const;
/** Check equality */
bool equals(const Permutation& rhs, double tol=0.0) const { return rangeIndices_ == rhs.rangeIndices_; }
/// @}
/// @name Standard Interface
/// @{
/**
* Return the source index of the supplied destination index.
*/
Index operator[](Index variable) const { check(variable); return rangeIndices_[variable]; }
/**
* Return the source index of the supplied destination index. This version allows modification.
*/
Index& operator[](Index variable) { check(variable); return rangeIndices_[variable]; }
/**
* Return the source index of the supplied destination index. Synonym for operator[](Index).
*/
Index at(Index variable) const { return operator[](variable); }
/**
* Return the source index of the supplied destination index. This version allows modification. Synonym for operator[](Index).
*/
Index& at(Index variable) { return operator[](variable); }
/**
* The number of variables in the range of this permutation, i.e. the
* destination space.
*/
Index size() const { return rangeIndices_.size(); }
/** Whether the permutation contains any entries */
bool empty() const { return rangeIndices_.empty(); }
/**
* Resize the permutation. You must fill in the new entries if new new size
* is larger than the old one. If the new size is smaller, entries from the
* end are discarded.
*/
void resize(size_t newSize) { rangeIndices_.resize(newSize); }
/**
* Return an identity permutation.
*/
static Permutation Identity(Index nVars);
/**
* Create a permutation that pulls the given variables to the front while
* pushing the rest to the back.
*/
static Permutation PullToFront(const std::vector<Index>& toFront, size_t size, bool filterDuplicates = false);
/**
* Create a permutation that pushes the given variables to the back while
* pulling the rest to the front.
*/
static Permutation PushToBack(const std::vector<Index>& toBack, size_t size, bool filterDuplicates = false);
/**
* Permute the permutation, p1.permute(p2)[i] is equivalent to p1[p2[i]].
*/
Permutation::shared_ptr permute(const Permutation& permutation) const;
/**
* Return the inverse permutation. This is only possible if this is a non-
* reducing permutation, that is, (*this)[i] < this->size() for all
* i < size(). If NDEBUG is not defined, this conditional will be checked.
*/
Permutation::shared_ptr inverse() const;
const_iterator begin() const { return rangeIndices_.begin(); } ///< Iterate through the indices
const_iterator end() const { return rangeIndices_.end(); } ///< Iterate through the indices
/** Apply the permutation to a collection, which must have operator[] defined.
* Note that permutable gtsam data structures typically have their own
* permute function to apply a permutation. Permutation::applyToCollection is
* a generic function, e.g. for STL classes.
* @param input The input collection.
* @param output The preallocated output collection, which is assigned output[i] = input[permutation[i]]
*/
template<typename INPUT_COLLECTION, typename OUTPUT_COLLECTION>
void applyToCollection(OUTPUT_COLLECTION& output, const INPUT_COLLECTION& input) const {
for(size_t i = 0; i < output.size(); ++i) output[i] = input[(*this)[i]]; }
/// @}
/// @name Advanced Interface
/// @{
iterator begin() { return rangeIndices_.begin(); } ///< Iterate through the indices
iterator end() { return rangeIndices_.end(); } ///< Iterate through the indices
protected:
void check(Index variable) const { assert(variable < rangeIndices_.size()); }
/// @}
};
namespace internal {
/**
* An internal class used for storing and applying a permutation from a map
*/
class Reduction : public gtsam::FastMap<Index,Index> {
public:
typedef gtsam::FastMap<Index,Index> Base;
GTSAM_EXPORT static Reduction CreateAsInverse(const Permutation& p);
GTSAM_EXPORT static Reduction CreateFromPartialPermutation(const Permutation& selector, const Permutation& p);
GTSAM_EXPORT void applyInverse(std::vector<Index>& js) const;
GTSAM_EXPORT Permutation inverse() const;
GTSAM_EXPORT const Index& operator[](const Index& j);
GTSAM_EXPORT const Index& operator[](const Index& j) const;
GTSAM_EXPORT void print(const std::string& s="") const;
GTSAM_EXPORT bool equals(const Reduction& other, double tol = 1e-9) const;
};
/**
* Reduce the variable indices so that those in the set are mapped to start at zero
*/
GTSAM_EXPORT Permutation createReducingPermutation(const std::set<Index>& indices);
} // \namespace internal
} // \namespace gtsam

132
gtsam/inference/SymbolicFactorGraphOrdered.cpp
View File

@ -1,132 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 SymbolicFactorGraph.cpp
* @date Oct 29, 2009
* @author Frank Dellaert
*/
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/BayesNetOrdered.h>
#include <gtsam/inference/EliminationTreeOrdered.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
#include <boost/make_shared.hpp>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
SymbolicFactorGraphOrdered::SymbolicFactorGraphOrdered(const SymbolicBayesNetOrdered& bayesNet) :
FactorGraphOrdered<IndexFactorOrdered>(bayesNet) {}
/* ************************************************************************* */
SymbolicFactorGraphOrdered::SymbolicFactorGraphOrdered(const SymbolicBayesTreeOrdered& bayesTree) :
FactorGraphOrdered<IndexFactorOrdered>(bayesTree) {}
/* ************************************************************************* */
void SymbolicFactorGraphOrdered::push_factor(Index key) {
push_back(boost::make_shared<IndexFactorOrdered>(key));
}
/** Push back binary factor */
void SymbolicFactorGraphOrdered::push_factor(Index key1, Index key2) {
push_back(boost::make_shared<IndexFactorOrdered>(key1,key2));
}
/** Push back ternary factor */
void SymbolicFactorGraphOrdered::push_factor(Index key1, Index key2, Index key3) {
push_back(boost::make_shared<IndexFactorOrdered>(key1,key2,key3));
}
/** Push back 4-way factor */
void SymbolicFactorGraphOrdered::push_factor(Index key1, Index key2, Index key3, Index key4) {
push_back(boost::make_shared<IndexFactorOrdered>(key1,key2,key3,key4));
}
/* ************************************************************************* */
FastSet<Index>
SymbolicFactorGraphOrdered::keys() const {
FastSet<Index> keys;
BOOST_FOREACH(const sharedFactor& factor, *this) {
if(factor) keys.insert(factor->begin(), factor->end()); }
return keys;
}
/* ************************************************************************* */
std::pair<SymbolicFactorGraphOrdered::sharedConditional, SymbolicFactorGraphOrdered>
SymbolicFactorGraphOrdered::eliminateFrontals(size_t nFrontals) const
{
return FactorGraphOrdered<IndexFactorOrdered>::eliminateFrontals(nFrontals, EliminateSymbolic);
}
/* ************************************************************************* */
std::pair<SymbolicFactorGraphOrdered::sharedConditional, SymbolicFactorGraphOrdered>
SymbolicFactorGraphOrdered::eliminate(const std::vector<Index>& variables) const
{
return FactorGraphOrdered<IndexFactorOrdered>::eliminate(variables, EliminateSymbolic);
}
/* ************************************************************************* */
std::pair<SymbolicFactorGraphOrdered::sharedConditional, SymbolicFactorGraphOrdered>
SymbolicFactorGraphOrdered::eliminateOne(Index variable) const
{
return FactorGraphOrdered<IndexFactorOrdered>::eliminateOne(variable, EliminateSymbolic);
}
/* ************************************************************************* */
void SymbolicFactorGraphOrdered::permuteWithInverse(
const Permutation& inversePermutation) {
BOOST_FOREACH(const sharedFactor& factor, factors_) {
if(factor)
factor->permuteWithInverse(inversePermutation);
}
}
/* ************************************************************************* */
void SymbolicFactorGraphOrdered::reduceWithInverse(
const internal::Reduction& inverseReduction) {
BOOST_FOREACH(const sharedFactor& factor, factors_) {
if(factor)
factor->reduceWithInverse(inverseReduction);
}
}
/* ************************************************************************* */
IndexFactorOrdered::shared_ptr CombineSymbolic(
const FactorGraphOrdered<IndexFactorOrdered>& factors, const FastMap<Index,
vector<Index> >& variableSlots) {
IndexFactorOrdered::shared_ptr combined(Combine<IndexFactorOrdered, Index> (factors, variableSlots));
// combined->assertInvariants();
return combined;
}
/* ************************************************************************* */
pair<IndexConditionalOrdered::shared_ptr, IndexFactorOrdered::shared_ptr> //
EliminateSymbolic(const FactorGraphOrdered<IndexFactorOrdered>& factors, size_t nrFrontals) {
FastSet<Index> keys;
BOOST_FOREACH(const IndexFactorOrdered::shared_ptr& factor, factors)
BOOST_FOREACH(Index var, *factor)
keys.insert(var);
if (keys.size() < nrFrontals) throw invalid_argument(
"EliminateSymbolic requested to eliminate more variables than exist in graph.");
vector<Index> newKeys(keys.begin(), keys.end());
return make_pair(boost::make_shared<IndexConditionalOrdered>(newKeys, nrFrontals),
boost::make_shared<IndexFactorOrdered>(newKeys.begin() + nrFrontals, newKeys.end()));
}
/* ************************************************************************* */
}

151
gtsam/inference/SymbolicFactorGraphOrdered.h
View File

@ -1,151 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 SymbolicFactorGraph.h
* @date Oct 29, 2009
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/global_includes.h>
#include <gtsam/inference/FactorGraphOrdered.h>
#include <gtsam/inference/IndexFactorOrdered.h>
namespace gtsam { template<class FACTOR> class EliminationTreeOrdered; }
namespace gtsam { template<class CONDITIONAL> class BayesNetOrdered; }
namespace gtsam { template<class CONDITIONAL, class CLIQUE> class BayesTreeOrdered; }
namespace gtsam { class IndexConditionalOrdered; }
namespace gtsam {
typedef EliminationTreeOrdered<IndexFactorOrdered> SymbolicEliminationTreeOrdered;
typedef BayesNetOrdered<IndexConditionalOrdered> SymbolicBayesNetOrdered;
typedef BayesTreeOrdered<IndexConditionalOrdered> SymbolicBayesTreeOrdered;
/** Symbolic IndexFactor Graph
* \nosubgrouping
*/
class SymbolicFactorGraphOrdered: public FactorGraphOrdered<IndexFactorOrdered> {
public:
/// @name Standard Constructors
/// @{
/** Construct empty factor graph */
SymbolicFactorGraphOrdered() {
}
/** Construct from a BayesNet */
GTSAM_EXPORT SymbolicFactorGraphOrdered(const SymbolicBayesNetOrdered& bayesNet);
/** Construct from a BayesTree */
GTSAM_EXPORT SymbolicFactorGraphOrdered(const SymbolicBayesTreeOrdered& bayesTree);
/**
* Construct from a factor graph of any type
*/
template<class FACTOR>
SymbolicFactorGraphOrdered(const FactorGraphOrdered<FACTOR>& fg);
/** Eliminate the first \c n frontal variables, returning the resulting
* conditional and remaining factor graph - this is very inefficient for
* eliminating all variables, to do that use EliminationTree or
* JunctionTree. Note that this version simply calls
* FactorGraph<IndexFactor>::eliminateFrontals with EliminateSymbolic
* as the eliminate function argument.
*/
GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraphOrdered> eliminateFrontals(size_t nFrontals) const;
/** 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.
* Note that this version simply calls
* FactorGraph<GaussianFactor>::eliminate with EliminateSymbolic as the eliminate
* function argument.
*/
GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraphOrdered> eliminate(const std::vector<Index>& variables) const;
/** Eliminate a single variable, by calling SymbolicFactorGraph::eliminate. */
GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraphOrdered> eliminateOne(Index variable) const;
/// @}
/// @name Standard Interface
/// @{
/**
* Return the set of variables involved in the factors (computes a set
* union).
*/
GTSAM_EXPORT FastSet<Index> keys() const;
/// @}
/// @name Advanced Interface
/// @{
/** Push back unary factor */
GTSAM_EXPORT void push_factor(Index key);
/** Push back binary factor */
GTSAM_EXPORT void push_factor(Index key1, Index key2);
/** Push back ternary factor */
GTSAM_EXPORT void push_factor(Index key1, Index key2, Index key3);
/** Push back 4-way factor */
GTSAM_EXPORT void push_factor(Index key1, Index key2, Index key3, Index key4);
/** Permute the variables in the factors */
GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation);
/** Apply a reduction, which is a remapping of variable indices. */
GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction);
};
/** Create a combined joint factor (new style for EliminationTree). */
GTSAM_EXPORT IndexFactorOrdered::shared_ptr CombineSymbolic(const FactorGraphOrdered<IndexFactorOrdered>& factors,
const FastMap<Index, std::vector<Index> >& variableSlots);
/**
* CombineAndEliminate provides symbolic elimination.
* Combine and eliminate can also be called separately, but for this and
* derived classes calling them separately generally does extra work.
*/
GTSAM_EXPORT std::pair<boost::shared_ptr<IndexConditionalOrdered>, boost::shared_ptr<IndexFactorOrdered> >
EliminateSymbolic(const FactorGraphOrdered<IndexFactorOrdered>&, size_t nrFrontals = 1);
/// @}
/** Template function implementation */
template<class FACTOR>
SymbolicFactorGraphOrdered::SymbolicFactorGraphOrdered(const FactorGraphOrdered<FACTOR>& fg) {
for (size_t i = 0; i < fg.size(); i++) {
if (fg[i]) {
IndexFactorOrdered::shared_ptr factor(new IndexFactorOrdered(*fg[i]));
push_back(factor);
} else
push_back(IndexFactorOrdered::shared_ptr());
}
}
} // namespace gtsam

25
gtsam/inference/SymbolicMultifrontalSolverOrdered.cpp
View File

@ -1,25 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 SymbolicMultifrontalSolver.cpp
* @author Richard Roberts
* @date Oct 22, 2010
*/
#include <gtsam/inference/SymbolicMultifrontalSolverOrdered.h>
#include <gtsam/inference/JunctionTreeOrdered.h>
namespace gtsam {
//template class GenericMultifrontalSolver<IndexFactor, JunctionTree<FactorGraph<IndexFactor> > >;
}

71
gtsam/inference/SymbolicMultifrontalSolverOrdered.h
View File

@ -1,71 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 SymbolicMultifrontalSolver.h
* @author Richard Roberts
* @date Oct 22, 2010
*/
#pragma once
#include <gtsam/inference/GenericMultifrontalSolver.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
namespace gtsam {
class SymbolicMultifrontalSolver : GenericMultifrontalSolver<IndexFactorOrdered, JunctionTreeOrdered<FactorGraphOrdered<IndexFactorOrdered> > > {
protected:
typedef GenericMultifrontalSolver<IndexFactorOrdered, JunctionTreeOrdered<FactorGraphOrdered<IndexFactorOrdered> > > Base;
public:
/**
* Construct the solver for a factor graph. This builds the junction
* tree, which does the symbolic elimination, identifies the cliques,
* and distributes all the factors to the right cliques.
*/
SymbolicMultifrontalSolver(const SymbolicFactorGraphOrdered& factorGraph) : Base(factorGraph) {};
/**
* Construct the solver with a shared pointer to a factor graph and to a
* VariableIndex. The solver will store these pointers, so this constructor
* is the fastest.
*/
SymbolicMultifrontalSolver(const SymbolicFactorGraphOrdered::shared_ptr& factorGraph,
const VariableIndexOrdered::shared_ptr& variableIndex) : Base(factorGraph, variableIndex) {};
/** Print to cout */
void print(const std::string& name = "SymbolicMultifrontalSolver: ") const { Base::print(name); };
/** Test whether is equal to another */
bool equals(const SymbolicMultifrontalSolver& other, double tol = 1e-9) const { return Base::equals(other, tol); };
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
SymbolicBayesTreeOrdered::shared_ptr eliminate() const { return Base::eliminate(&EliminateSymbolic); };
/**
* Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. Returns the result as a factor graph.
*/
SymbolicFactorGraphOrdered::shared_ptr jointFactorGraph(const std::vector<Index>& js) const { return Base::jointFactorGraph(js, &EliminateSymbolic); };
/**
* Compute the marginal Gaussian density over a variable, by integrating out
* all of the other variables. This function returns the result as a factor.
*/
IndexFactorOrdered::shared_ptr marginalFactor(Index j) const { return Base::marginalFactor(j, &EliminateSymbolic); };
};
}

26
gtsam/inference/SymbolicSequentialSolverOrdered.cpp
View File

@ -1,26 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 SymbolicSequentialSolver.cpp
* @author Richard Roberts
* @date Oct 21, 2010
*/
#include <gtsam/inference/SymbolicSequentialSolverOrdered.h>
#include <gtsam/inference/GenericSequentialSolver-inl.h>
namespace gtsam {
// An explicit instantiation to be compiled into the library
//template class GenericSequentialSolver<IndexFactor>;
}

88
gtsam/inference/SymbolicSequentialSolverOrdered.h
View File

@ -1,88 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 SymbolicSequentialSolver.h
* @author Richard Roberts
* @date Oct 21, 2010
*/
#pragma once
#include <gtsam/inference/GenericSequentialSolver.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
namespace gtsam {
class SymbolicSequentialSolver : GenericSequentialSolver<IndexFactorOrdered> {
protected:
typedef GenericSequentialSolver<IndexFactorOrdered> Base;
public:
/**
* Construct the solver for a factor graph. This builds the junction
* tree, which does the symbolic elimination, identifies the cliques,
* and distributes all the factors to the right cliques.
*/
SymbolicSequentialSolver(const SymbolicFactorGraphOrdered& factorGraph) : Base(factorGraph) {};
/**
* Construct the solver with a shared pointer to a factor graph and to a
* VariableIndex. The solver will store these pointers, so this constructor
* is the fastest.
*/
SymbolicSequentialSolver(const SymbolicFactorGraphOrdered::shared_ptr& factorGraph,
const VariableIndexOrdered::shared_ptr& variableIndex) : Base(factorGraph, variableIndex) {};
/** Print to cout */
void print(const std::string& name = "SymbolicSequentialSolver: ") const { Base::print(name); };
/** Test whether is equal to another */
bool equals(const SymbolicSequentialSolver& other, double tol = 1e-9) const { return Base::equals(other, tol); };
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
SymbolicBayesNetOrdered::shared_ptr eliminate() const
{ return Base::eliminate(&EliminateSymbolic); };
/**
* Compute a conditional density P(F|S) while marginalizing out variables J
* P(F|S) is obtained by P(J,F,S)=P(J|F,S)P(F|S)P(S) and dropping P(S)
* Returns the result as a Bayes net.
*/
SymbolicBayesNetOrdered::shared_ptr conditionalBayesNet(const std::vector<Index>& js, size_t nrFrontals) const
{ return Base::conditionalBayesNet(js, nrFrontals, &EliminateSymbolic); };
/**
* Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. Returns the result as a Bayes net.
*/
SymbolicBayesNetOrdered::shared_ptr jointBayesNet(const std::vector<Index>& js) const
{ return Base::jointBayesNet(js, &EliminateSymbolic); };
/**
* Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. Returns the result as a factor graph.
*/
SymbolicFactorGraphOrdered::shared_ptr jointFactorGraph(const std::vector<Index>& js) const
{ return Base::jointFactorGraph(js, &EliminateSymbolic); };
/**
* Compute the marginal Gaussian density over a variable, by integrating out
* all of the other variables. This function returns the result as a factor.
*/
IndexFactorOrdered::shared_ptr marginalFactor(Index j) const { return Base::marginalFactor(j, &EliminateSymbolic); };
};
}

104
gtsam/inference/VariableIndexOrdered.cpp
View File

@ -1,104 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 VariableIndex.cpp
* @author Richard Roberts
* @date Oct 22, 2010
*/
#include <iostream>
#include <gtsam/inference/VariableIndexOrdered.h>
#include <gtsam/inference/PermutationOrdered.h>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
bool VariableIndexOrdered::equals(const VariableIndexOrdered& other, double tol) const {
if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) {
for(size_t var=0; var < max(this->index_.size(), other.index_.size()); ++var)
if(var >= this->index_.size() || var >= other.index_.size()) {
if(!((var >= this->index_.size() && other.index_[var].empty()) ||
(var >= other.index_.size() && this->index_[var].empty())))
return false;
} else if(this->index_[var] != other.index_[var])
return false;
} else
return false;
return true;
}
/* ************************************************************************* */
void VariableIndexOrdered::print(const string& str) const {
cout << str;
cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
for(Index var = 0; var < size(); ++var) {
cout << "var " << var << ":";
BOOST_FOREACH(const size_t factor, index_[var])
cout << " " << factor;
cout << "\n";
}
cout << flush;
}
/* ************************************************************************* */
void VariableIndexOrdered::outputMetisFormat(ostream& os) const {
os << size() << " " << nFactors() << "\n";
// run over variables, which will be hyper-edges.
BOOST_FOREACH(const Factors& variable, index_) {
// every variable is a hyper-edge covering its factors
BOOST_FOREACH(const size_t factor, variable)
os << (factor+1) << " "; // base 1
os << "\n";
}
os << flush;
}
/* ************************************************************************* */
void VariableIndexOrdered::permuteInPlace(const Permutation& permutation) {
// Create new index and move references to data into it in permuted order
vector<VariableIndexOrdered::Factors> newIndex(this->size());
for(Index i = 0; i < newIndex.size(); ++i)
newIndex[i].swap(this->index_[permutation[i]]);
// Move reference to entire index into the VariableIndex
index_.swap(newIndex);
}
/* ************************************************************************* */
void VariableIndexOrdered::permuteInPlace(const Permutation& selector, const Permutation& permutation) {
if(selector.size() != permutation.size())
throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes.");
// Create new index the size of the permuted entries
vector<VariableIndexOrdered::Factors> newIndex(selector.size());
// Permute the affected entries into the new index
for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot)
newIndex[dstSlot].swap(this->index_[selector[permutation[dstSlot]]]);
// Put the affected entries back in the new order
for(size_t slot = 0; slot < selector.size(); ++slot)
this->index_[selector[slot]].swap(newIndex[slot]);
}
/* ************************************************************************* */
void VariableIndexOrdered::removeUnusedAtEnd(size_t nToRemove) {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
for(size_t i = this->size() - nToRemove; i < this->size(); ++i)
if(!(*this)[i].empty())
throw std::invalid_argument("Attempting to remove non-empty variables with VariableIndex::removeUnusedAtEnd()");
#endif
index_.resize(this->size() - nToRemove);
}
}

292
gtsam/inference/VariableIndexOrdered.h
View File

@ -1,292 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 VariableIndex.h
* @author Richard Roberts
* @date Sep 12, 2010
*/
#pragma once
#include <vector>
#include <deque>
#include <stdexcept>
#include <boost/foreach.hpp>
#include <gtsam/base/FastList.h>
#include <gtsam/global_includes.h>
#include <gtsam/base/timing.h>
namespace gtsam {
class Permutation;
/**
* The VariableIndex class computes and stores the block column structure of a
* factor graph. The factor graph stores a collection of factors, each of
* which involves a set of variables. In contrast, the VariableIndex is built
* from a factor graph prior to elimination, and stores the list of factors
* that involve each variable. This information is stored as a deque of
* lists of factor indices.
* \nosubgrouping
*/
class GTSAM_EXPORT VariableIndexOrdered {
public:
typedef boost::shared_ptr<VariableIndexOrdered> shared_ptr;
typedef FastList<size_t> Factors;
typedef Factors::iterator Factor_iterator;
typedef Factors::const_iterator Factor_const_iterator;
protected:
std::vector<Factors> index_;
size_t nFactors_; // Number of factors in the original factor graph.
size_t nEntries_; // Sum of involved variable counts of each factor.
public:
/// @name Standard Constructors
/// @{
/** Default constructor, creates an empty VariableIndex */
VariableIndexOrdered() : nFactors_(0), nEntries_(0) {}
/**
* Create a VariableIndex that computes and stores the block column structure
* of a factor graph. This constructor is used when the number of variables
* is known beforehand.
*/
template<class FG> VariableIndexOrdered(const FG& factorGraph, Index nVariables);
/**
* Create a VariableIndex that computes and stores the block column structure
* of a factor graph.
*/
template<class FG> VariableIndexOrdered(const FG& factorGraph);
/// @}
/// @name Standard Interface
/// @{
/**
* The number of variable entries. This is one greater than the variable
* with the highest index.
*/
Index size() const { return index_.size(); }
/** The number of factors in the original factor graph */
size_t nFactors() const { return nFactors_; }
/** The number of nonzero blocks, i.e. the number of variable-factor entries */
size_t nEntries() const { return nEntries_; }
/** Access a list of factors by variable */
const Factors& operator[](Index variable) const { checkVar(variable); return index_[variable]; }
/// @}
/// @name Testable
/// @{
/** Test for equality (for unit tests and debug assertions). */
bool equals(const VariableIndexOrdered& other, double tol=0.0) const;
/** Print the variable index (for unit tests and debugging). */
void print(const std::string& str = "VariableIndex: ") const;
/**
* Output dual hypergraph to Metis file format for use with hmetis
* In the dual graph, variables are hyperedges, factors are nodes.
*/
void outputMetisFormat(std::ostream& os) const;
/// @}
/// @name Advanced Interface
/// @{
/**
* Augment the variable index with new factors. This can be used when
* solving problems incrementally.
*/
template<class FG> void augment(const FG& factors);
/**
* Remove entries corresponding to the specified factors.
* NOTE: We intentionally do not decrement nFactors_ because the factor
* indices need to remain consistent. Removing factors from a factor graph
* does not shift the indices of other factors. Also, we keep nFactors_
* one greater than the highest-numbered factor referenced in a VariableIndex.
*
* @param indices The indices of the factors to remove, which must match \c factors
* @param factors The factors being removed, which must symbolically correspond
* exactly to the factors with the specified \c indices that were added.
*/
template<typename CONTAINER, class FG> void remove(const CONTAINER& indices, const FG& factors);
/// Permute the variables in the VariableIndex according to the given permutation
void permuteInPlace(const Permutation& permutation);
/// Permute the variables in the VariableIndex according to the given partial permutation
void permuteInPlace(const Permutation& selector, const Permutation& permutation);
/** Remove unused empty variables at the end of the ordering (in debug mode
* verifies they are empty).
* @param nToRemove The number of unused variables at the end to remove
*/
void removeUnusedAtEnd(size_t nToRemove);
protected:
Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); }
Factor_iterator factorsEnd(Index variable) { checkVar(variable); return index_[variable].end(); }
Factor_const_iterator factorsBegin(Index variable) const { checkVar(variable); return index_[variable].begin(); }
Factor_const_iterator factorsEnd(Index variable) const { checkVar(variable); return index_[variable].end(); }
/// Internal constructor to allocate a VariableIndex of the requested size
VariableIndexOrdered(size_t nVars) : index_(nVars), nFactors_(0), nEntries_(0) {}
/// Internal check of the validity of a variable
void checkVar(Index variable) const { assert(variable < index_.size()); }
/// Internal function to populate the variable index from a factor graph
template<class FG> void fill(const FG& factorGraph);
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(index_);
ar & BOOST_SERIALIZATION_NVP(nFactors_);
ar & BOOST_SERIALIZATION_NVP(nEntries_);
}
};
/* ************************************************************************* */
template<class FG>
void VariableIndexOrdered::fill(const FG& factorGraph) {
gttic(VariableIndex_fill);
// Build index mapping from variable id to factor index
for(size_t fi=0; fi<factorGraph.size(); ++fi) {
if(factorGraph[fi]) {
BOOST_FOREACH(const Index key, factorGraph[fi]->keys()) {
if(key < index_.size()) {
index_[key].push_back(fi);
++ nEntries_;
}
}
}
++ nFactors_; // Increment factor count even if factors are null, to keep indices consistent
}
}
/* ************************************************************************* */
template<class FG>
VariableIndexOrdered::VariableIndexOrdered(const FG& factorGraph) :
nFactors_(0), nEntries_(0)
{
gttic(VariableIndex_constructor);
// If the factor graph is empty, return an empty index because inside this
// if block we assume at least one factor.
if(factorGraph.size() > 0) {
gttic(VariableIndex_constructor_find_highest);
// Find highest-numbered variable
Index maxVar = 0;
BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) {
if(factor) {
BOOST_FOREACH(const Index key, factor->keys()) {
if(key > maxVar)
maxVar = key;
}
}
}
gttoc(VariableIndex_constructor_find_highest);
// Allocate array
gttic(VariableIndex_constructor_allocate);
index_.resize(maxVar+1);
gttoc(VariableIndex_constructor_allocate);
fill(factorGraph);
}
}
/* ************************************************************************* */
template<class FG>
VariableIndexOrdered::VariableIndexOrdered(const FG& factorGraph, Index nVariables) :
nFactors_(0), nEntries_(0)
{
gttic(VariableIndex_constructor_allocate);
index_.resize(nVariables);
gttoc(VariableIndex_constructor_allocate);
fill(factorGraph);
}
/* ************************************************************************* */
template<class FG>
void VariableIndexOrdered::augment(const FG& factors) {
gttic(VariableIndex_augment);
// If the factor graph is empty, return an empty index because inside this
// if block we assume at least one factor.
if(factors.size() > 0) {
// Find highest-numbered variable
Index maxVar = 0;
BOOST_FOREACH(const typename FG::sharedFactor& factor, factors) {
if(factor) {
BOOST_FOREACH(const Index key, factor->keys()) {
if(key > maxVar)
maxVar = key;
}
}
}
// Allocate index
index_.resize(std::max(index_.size(), maxVar+1));
// Augment index mapping from variable id to factor index
size_t orignFactors = nFactors_;
for(size_t fi=0; fi<factors.size(); ++fi) {
if(factors[fi]) {
BOOST_FOREACH(const Index key, factors[fi]->keys()) {
index_[key].push_back(orignFactors + fi);
++ nEntries_;
}
}
++ nFactors_; // Increment factor count even if factors are null, to keep indices consistent
}
}
}
/* ************************************************************************* */
template<typename CONTAINER, class FG>
void VariableIndexOrdered::remove(const CONTAINER& indices, const FG& factors) {
gttic(VariableIndex_remove);
// NOTE: We intentionally do not decrement nFactors_ because the factor
// indices need to remain consistent. Removing factors from a factor graph
// does not shift the indices of other factors. Also, we keep nFactors_
// one greater than the highest-numbered factor referenced in a VariableIndex.
for(size_t fi=0; fi<factors.size(); ++fi)
if(factors[fi]) {
for(size_t ji = 0; ji < factors[fi]->keys().size(); ++ji) {
Factors& factorEntries = index_[factors[fi]->keys()[ji]];
Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), indices[fi]);
if(entry == factorEntries.end())
throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index");
factorEntries.erase(entry);
-- nEntries_;
}
}
}
}

88
gtsam/inference/inferenceOrdered-inl.h
View File

@ -1,88 +0,0 @@
/* ----------------------------------------------------------------------------
* 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/inferenceOrdered.h>
#include <gtsam/base/FastSet.h>
namespace gtsam {
namespace inference {
/* ************************************************************************* */
template<typename CONSTRAINED>
Permutation::shared_ptr PermutationCOLAMD(
const VariableIndexOrdered& variableIndex, const CONSTRAINED& constrainLast, bool forceOrder) {
gttic(PermutationCOLAMD_constrained);
size_t n = variableIndex.size();
std::vector<int> cmember(n, 0);
// If at least some variables are not constrained to be last, constrain the
// ones that should be constrained.
if(constrainLast.size() < n) {
BOOST_FOREACH(Index var, constrainLast) {
assert(var < n);
cmember[var] = 1;
}
}
Permutation::shared_ptr permutation = PermutationCOLAMD_(variableIndex, cmember);
if (forceOrder) {
Index j=n;
BOOST_REVERSE_FOREACH(Index c, constrainLast)
permutation->operator[](--j) = c;
}
return permutation;
}
/* ************************************************************************* */
template<typename CONSTRAINED_MAP>
Permutation::shared_ptr PermutationCOLAMDGrouped(
const VariableIndexOrdered& variableIndex, const CONSTRAINED_MAP& constraints) {
gttic(PermutationCOLAMD_grouped);
size_t n = variableIndex.size();
std::vector<int> cmember(n, 0);
typedef typename CONSTRAINED_MAP::value_type constraint_pair;
BOOST_FOREACH(const constraint_pair& p, constraints) {
assert(p.first < n);
// FIXME: check that no groups are skipped
cmember[p.first] = p.second;
}
return PermutationCOLAMD_(variableIndex, cmember);
}
/* ************************************************************************* */
inline Permutation::shared_ptr PermutationCOLAMD(const VariableIndexOrdered& variableIndex) {
gttic(PermutationCOLAMD_unconstrained);
size_t n = variableIndex.size();
std::vector<int> cmember(n, 0);
return PermutationCOLAMD_(variableIndex, cmember);
}
} // namespace inference
} // namespace gtsam

106
gtsam/inference/inferenceOrdered.cpp
View File

@ -1,106 +0,0 @@
/* ----------------------------------------------------------------------------
* 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.cpp
* @brief inference definitions
* @author Frank Dellaert
* @author Richard Roberts
*/
#include <gtsam/inference/inferenceOrdered.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <boost/format.hpp>
#include <stdexcept>
#include <iostream>
#include <vector>
#include <ccolamd.h>
using namespace std;
namespace gtsam {
namespace inference {
/* ************************************************************************* */
Permutation::shared_ptr PermutationCOLAMD_(const VariableIndexOrdered& variableIndex, std::vector<int>& cmember) {
gttic(PermutationCOLAMD_internal);
gttic(Prepare);
size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
// Convert to compressed column major format colamd wants it in (== MATLAB format!)
size_t Alen = ccolamd_recommended((int)nEntries, (int)nFactors, (int)nVars); /* colamd arg 3: size of the array A */
vector<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */
vector<int> p = vector<int>(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
static const bool debug = false;
p[0] = 0;
int count = 0;
for(Index var = 0; var < variableIndex.size(); ++var) {
const VariableIndexOrdered::Factors& column(variableIndex[var]);
size_t lastFactorId = numeric_limits<size_t>::max();
BOOST_FOREACH(size_t factorIndex, column) {
if(lastFactorId != numeric_limits<size_t>::max())
assert(factorIndex > lastFactorId);
A[count++] = (int)factorIndex; // copy sparse column
if(debug) cout << "A[" << count-1 << "] = " << factorIndex << endl;
}
p[var+1] = count; // column j (base 1) goes from A[j-1] to A[j]-1
}
assert((size_t)count == variableIndex.nEntries());
if(debug)
for(size_t i=0; i<nVars+1; ++i)
cout << "p[" << i << "] = " << p[i] << endl;
//double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
double knobs[CCOLAMD_KNOBS];
ccolamd_set_defaults(knobs);
knobs[CCOLAMD_DENSE_ROW]=-1;
knobs[CCOLAMD_DENSE_COL]=-1;
int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */
gttoc(Prepare);
// call colamd, result will be in p
/* returns (1) if successful, (0) otherwise*/
if(nVars > 0) {
gttic(ccolamd);
int rv = ccolamd((int)nFactors, (int)nVars, (int)Alen, &A[0], &p[0], knobs, stats, &cmember[0]);
if(rv != 1)
throw runtime_error((boost::format("ccolamd failed with return value %1%")%rv).str());
}
// ccolamd_report(stats);
gttic(Create_permutation);
// 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
permutation->operator[](j) = p[j];
if(debug) cout << "COLAMD: " << j << "->" << p[j] << endl;
}
if(debug) cout << "COLAMD: p[" << nVars << "] = " << p[nVars] << endl;
gttoc(Create_permutation);
return permutation;
}
/* ************************************************************************* */
} // \namespace inference
} // \namespace gtsam

82
gtsam/inference/inferenceOrdered.h
View File

@ -1,82 +0,0 @@
/* ----------------------------------------------------------------------------
* 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.h
* @brief Contains *generic* inference algorithms that convert between templated
* graphical models, i.e., factor graphs, Bayes nets, and Bayes trees
* @author Frank Dellaert
* @author Richard Roberts
*/
#pragma once
#include <gtsam/inference/VariableIndexOrdered.h>
#include <gtsam/inference/PermutationOrdered.h>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>
#include <deque>
namespace gtsam {
namespace inference {
/**
* Compute a permutation (variable ordering) using colamd
*/
GTSAM_EXPORT Permutation::shared_ptr PermutationCOLAMD(
const VariableIndexOrdered& variableIndex);
/**
* Compute a permutation (variable ordering) using constrained colamd to move
* a set of variables to the end of the ordering
* @param variableIndex is the variable index lookup from a graph
* @param constrainlast is a vector of keys that should be constrained
* @tparam constrainLast is a std::vector (or similar structure)
* @param forceOrder if true, will not allow re-ordering of constrained variables
*/
template<typename CONSTRAINED>
Permutation::shared_ptr PermutationCOLAMD(
const VariableIndexOrdered& variableIndex, const CONSTRAINED& constrainLast, bool forceOrder=false);
/**
* Compute a permutation of variable ordering using constrained colamd to
* move variables to the end in groups (0 = unconstrained, higher numbers at
* the end).
* @param variableIndex is the variable index lookup from a graph
* @param constraintMap is a map from variable index -> group number for constrained variables
* @tparam CONSTRAINED_MAP is an associative structure (like std::map), from size_t->int
*/
template<typename CONSTRAINED_MAP>
Permutation::shared_ptr PermutationCOLAMDGrouped(
const VariableIndexOrdered& variableIndex, const CONSTRAINED_MAP& constraints);
/**
* Compute a CCOLAMD permutation using the constraint groups in cmember.
* The format for cmember is a part of ccolamd.
*
* @param variableIndex is the variable structure from a graph
* @param cmember is the constraint group list for each variable, where
* 0 is the default, unconstrained group, and higher numbers move further to
* the back of the list
*
* AGC: does cmember change?
*/
GTSAM_EXPORT Permutation::shared_ptr PermutationCOLAMD_(
const VariableIndexOrdered& variableIndex, std::vector<int>& cmember);
} // \namespace inference
} // \namespace gtsam
#include <gtsam/inference/inferenceOrdered-inl.h>

563
gtsam/inference/tests/testBayesTreeObsolete.cpp
View File

@ -1,563 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testBayesTree.cpp
* @brief Unit tests for Bayes Tree
* @author Frank Dellaert
* @author Michael Kaess
* @author Viorela Ila
*/
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std/set.hpp>
#include <boost/assign/list_of.hpp>
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/BayesTreeOrdered.h>
#include <gtsam/inference/IndexFactorOrdered.h>
#include <gtsam/inference/SymbolicSequentialSolverOrdered.h>
using namespace std;
using namespace gtsam;
///* ************************************************************************* */
//// SLAM example from RSS sqrtSAM paper
static const Index _x3_=0, _x2_=1;
//static const Index _x1_=2, _l2_=3, _l1_=4; // unused
//IndexConditionalOrdered::shared_ptr
// x3(new IndexConditionalOrdered(_x3_)),
// x2(new IndexConditionalOrdered(_x2_,_x3_)),
// x1(new IndexConditionalOrdered(_x1_,_x2_,_x3_)),
// l1(new IndexConditionalOrdered(_l1_,_x1_,_x2_)),
// l2(new IndexConditionalOrdered(_l2_,_x1_,_x3_));
//
//// Bayes Tree for sqrtSAM example
//SymbolicBayesTreeOrdered createSlamSymbolicBayesTree(){
// // Create using insert
//// Ordering slamOrdering; slamOrdering += _x3_, _x2_, _x1_, _l2_, _l1_;
// SymbolicBayesTreeOrdered bayesTree_slam;
// bayesTree_slam.insert(x3);
// bayesTree_slam.insert(x2);
// bayesTree_slam.insert(x1);
// bayesTree_slam.insert(l2);
// bayesTree_slam.insert(l1);
// return bayesTree_slam;
//}
/* ************************************************************************* */
// Conditionals for ASIA example from the tutorial with A and D evidence
static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5;
static IndexConditionalOrdered::shared_ptr
B(new IndexConditionalOrdered(_B_)),
L(new IndexConditionalOrdered(_L_, _B_)),
E(new IndexConditionalOrdered(_E_, _L_, _B_)),
S(new IndexConditionalOrdered(_S_, _L_, _B_)),
T(new IndexConditionalOrdered(_T_, _E_, _L_)),
X(new IndexConditionalOrdered(_X_, _E_));
// Cliques
static IndexConditionalOrdered::shared_ptr
ELB(IndexConditionalOrdered::FromKeys(cref_list_of<3>(_E_)(_L_)(_B_), 3));
// Bayes Tree for Asia example
static SymbolicBayesTreeOrdered createAsiaSymbolicBayesTree() {
SymbolicBayesTreeOrdered bayesTree;
// Ordering asiaOrdering; asiaOrdering += _X_, _T_, _S_, _E_, _L_, _B_;
SymbolicBayesTreeOrdered::insert(bayesTree, B);
SymbolicBayesTreeOrdered::insert(bayesTree, L);
SymbolicBayesTreeOrdered::insert(bayesTree, E);
SymbolicBayesTreeOrdered::insert(bayesTree, S);
SymbolicBayesTreeOrdered::insert(bayesTree, T);
SymbolicBayesTreeOrdered::insert(bayesTree, X);
return bayesTree;
}
/* ************************************************************************* */
TEST( BayesTreeOrdered, constructor )
{
// Create using insert
SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree();
bayesTree.print("bayesTree (ordered): ");
// Check Size
LONGS_EQUAL(4,bayesTree.size());
EXPECT(!bayesTree.empty());
// Check root
boost::shared_ptr<IndexConditionalOrdered> actual_root = bayesTree.root()->conditional();
CHECK(assert_equal(*ELB,*actual_root));
// Create from symbolic Bayes chain in which we want to discover cliques
BayesNetOrdered<IndexConditionalOrdered> ASIA;
ASIA.push_back(X);
ASIA.push_back(T);
ASIA.push_back(S);
ASIA.push_back(E);
ASIA.push_back(L);
ASIA.push_back(B);
SymbolicBayesTreeOrdered bayesTree2(ASIA);
// Check whether the same
CHECK(assert_equal(bayesTree,bayesTree2));
// CHECK findParentClique, should *not depend on order of parents*
// Ordering ordering; ordering += _X_, _T_, _S_, _E_, _L_, _B_;
// IndexTable<Symbol> index(ordering);
list<Index> parents1; parents1 += _E_, _L_;
CHECK(assert_equal(_E_, bayesTree.findParentClique(parents1)));
list<Index> parents2; parents2 += _L_, _E_;
CHECK(assert_equal(_E_, bayesTree.findParentClique(parents2)));
list<Index> parents3; parents3 += _L_, _B_;
CHECK(assert_equal(_L_, bayesTree.findParentClique(parents3)));
}
/* ************************************************************************* */
TEST(BayesTreeOrdered, clear)
{
// SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree();
// bayesTree.clear();
//
// SymbolicBayesTreeOrdered expected;
//
// // Check whether cleared BayesTreeOrdered is equal to a new BayesTreeOrdered
// CHECK(assert_equal(expected, bayesTree));
}
/* ************************************************************************* *
Bayes Tree for testing conversion to a forest of orphans needed for incremental.
A,B
C|A E|B
D|C F|E
*/
/* ************************************************************************* */
TEST( BayesTreeOrdered, removePath )
{
const Index _A_=5, _B_=4, _C_=3, _D_=2, _E_=1, _F_=0;
IndexConditionalOrdered::shared_ptr
A(new IndexConditionalOrdered(_A_)),
B(new IndexConditionalOrdered(_B_, _A_)),
C(new IndexConditionalOrdered(_C_, _A_)),
D(new IndexConditionalOrdered(_D_, _C_)),
E(new IndexConditionalOrdered(_E_, _B_)),
F(new IndexConditionalOrdered(_F_, _E_));
SymbolicBayesTreeOrdered bayesTree;
EXPECT(bayesTree.empty());
// Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_;
SymbolicBayesTreeOrdered::insert(bayesTree, A);
SymbolicBayesTreeOrdered::insert(bayesTree, B);
SymbolicBayesTreeOrdered::insert(bayesTree, C);
SymbolicBayesTreeOrdered::insert(bayesTree, D);
SymbolicBayesTreeOrdered::insert(bayesTree, E);
SymbolicBayesTreeOrdered::insert(bayesTree, F);
// remove C, expected outcome: factor graph with ABC,
// Bayes Tree now contains two orphan trees: D|C and E|B,F|E
SymbolicFactorGraphOrdered expected;
expected.push_factor(_B_,_A_);
// expected.push_factor(_A_);
expected.push_factor(_C_,_A_);
SymbolicBayesTreeOrdered::Cliques expectedOrphans;
expectedOrphans += bayesTree[_D_], bayesTree[_E_];
BayesNetOrdered<IndexConditionalOrdered> bn;
SymbolicBayesTreeOrdered::Cliques orphans;
bayesTree.removePath(bayesTree[_C_], bn, orphans);
SymbolicFactorGraphOrdered factors(bn);
CHECK(assert_equal((SymbolicFactorGraphOrdered)expected, factors));
CHECK(assert_equal(expectedOrphans, orphans));
// remove E: factor graph with EB; E|B removed from second orphan tree
SymbolicFactorGraphOrdered expected2;
expected2.push_factor(_E_,_B_);
SymbolicBayesTreeOrdered::Cliques expectedOrphans2;
expectedOrphans2 += bayesTree[_F_];
BayesNetOrdered<IndexConditionalOrdered> bn2;
SymbolicBayesTreeOrdered::Cliques orphans2;
bayesTree.removePath(bayesTree[_E_], bn2, orphans2);
SymbolicFactorGraphOrdered factors2(bn2);
CHECK(assert_equal((SymbolicFactorGraphOrdered)expected2, factors2));
CHECK(assert_equal(expectedOrphans2, orphans2));
}
/* ************************************************************************* */
TEST( BayesTreeOrdered, removePath2 )
{
SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree();
// Call remove-path with clique B
BayesNetOrdered<IndexConditionalOrdered> bn;
SymbolicBayesTreeOrdered::Cliques orphans;
bayesTree.removePath(bayesTree[_B_], bn, orphans);
SymbolicFactorGraphOrdered factors(bn);
// Check expected outcome
SymbolicFactorGraphOrdered expected;
expected.push_factor(_E_,_L_,_B_);
// expected.push_factor(_L_,_B_);
// expected.push_factor(_B_);
CHECK(assert_equal(expected, factors));
SymbolicBayesTreeOrdered::Cliques expectedOrphans;
expectedOrphans += bayesTree[_S_], bayesTree[_T_], bayesTree[_X_];
CHECK(assert_equal(expectedOrphans, orphans));
}
/* ************************************************************************* */
TEST( BayesTreeOrdered, removePath3 )
{
SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree();
// Call remove-path with clique S
BayesNetOrdered<IndexConditionalOrdered> bn;
SymbolicBayesTreeOrdered::Cliques orphans;
bayesTree.removePath(bayesTree[_S_], bn, orphans);
SymbolicFactorGraphOrdered factors(bn);
// Check expected outcome
SymbolicFactorGraphOrdered expected;
expected.push_factor(_E_,_L_,_B_);
// expected.push_factor(_L_,_B_);
// expected.push_factor(_B_);
expected.push_factor(_S_,_L_,_B_);
CHECK(assert_equal(expected, factors));
SymbolicBayesTreeOrdered::Cliques expectedOrphans;
expectedOrphans += bayesTree[_T_], bayesTree[_X_];
CHECK(assert_equal(expectedOrphans, orphans));
}
void getAllCliques(const SymbolicBayesTreeOrdered::sharedClique& subtree, SymbolicBayesTreeOrdered::Cliques& cliques) {
// Check if subtree exists
if (subtree) {
cliques.push_back(subtree);
// Recursive call over all child cliques
BOOST_FOREACH(SymbolicBayesTreeOrdered::sharedClique& childClique, subtree->children()) {
getAllCliques(childClique,cliques);
}
}
}
/* ************************************************************************* */
TEST( BayesTreeOrdered, shortcutCheck )
{
const Index _A_=6, _B_=5, _C_=4, _D_=3, _E_=2, _F_=1, _G_=0;
IndexConditionalOrdered::shared_ptr
A(new IndexConditionalOrdered(_A_)),
B(new IndexConditionalOrdered(_B_, _A_)),
C(new IndexConditionalOrdered(_C_, _A_)),
D(new IndexConditionalOrdered(_D_, _C_)),
E(new IndexConditionalOrdered(_E_, _B_)),
F(new IndexConditionalOrdered(_F_, _E_)),
G(new IndexConditionalOrdered(_G_, _F_));
SymbolicBayesTreeOrdered bayesTree;
// Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_;
SymbolicBayesTreeOrdered::insert(bayesTree, A);
SymbolicBayesTreeOrdered::insert(bayesTree, B);
SymbolicBayesTreeOrdered::insert(bayesTree, C);
SymbolicBayesTreeOrdered::insert(bayesTree, D);
SymbolicBayesTreeOrdered::insert(bayesTree, E);
SymbolicBayesTreeOrdered::insert(bayesTree, F);
SymbolicBayesTreeOrdered::insert(bayesTree, G);
//bayesTree.print("BayesTreeOrdered");
//bayesTree.saveGraph("BT1.dot");
SymbolicBayesTreeOrdered::sharedClique rootClique= bayesTree.root();
//rootClique->printTree();
SymbolicBayesTreeOrdered::Cliques allCliques;
getAllCliques(rootClique,allCliques);
BayesNetOrdered<IndexConditionalOrdered> bn;
BOOST_FOREACH(SymbolicBayesTreeOrdered::sharedClique& clique, allCliques) {
//clique->print("Clique#");
bn = clique->shortcut(rootClique, &EliminateSymbolic);
//bn.print("Shortcut:\n");
//cout << endl;
}
// Check if all the cached shortcuts are cleared
rootClique->deleteCachedShortcuts();
BOOST_FOREACH(SymbolicBayesTreeOrdered::sharedClique& clique, allCliques) {
bool notCleared = clique->cachedSeparatorMarginal();
CHECK( notCleared == false);
}
EXPECT_LONGS_EQUAL(0, rootClique->numCachedSeparatorMarginals());
// BOOST_FOREACH(SymbolicBayesTreeOrdered::sharedClique& clique, allCliques) {
// clique->print("Clique#");
// if(clique->cachedShortcut()){
// bn = clique->cachedShortcut().get();
// bn.print("Shortcut:\n");
// }
// else
// cout << "Not Initialized" << endl;
// cout << endl;
// }
}
/* ************************************************************************* */
TEST( BayesTreeOrdered, removeTop )
{
SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree();
// create a new factor to be inserted
boost::shared_ptr<IndexFactorOrdered> newFactor(new IndexFactorOrdered(_S_,_B_));
// Remove the contaminated part of the Bayes tree
BayesNetOrdered<IndexConditionalOrdered> bn;
SymbolicBayesTreeOrdered::Cliques orphans;
list<Index> keys; keys += _B_,_S_;
bayesTree.removeTop(keys, bn, orphans);
SymbolicFactorGraphOrdered factors(bn);
// Check expected outcome
SymbolicFactorGraphOrdered expected;
expected.push_factor(_E_,_L_,_B_);
// expected.push_factor(_L_,_B_);
// expected.push_factor(_B_);
expected.push_factor(_S_,_L_,_B_);
CHECK(assert_equal(expected, factors));
SymbolicBayesTreeOrdered::Cliques expectedOrphans;
expectedOrphans += bayesTree[_T_], bayesTree[_X_];
CHECK(assert_equal(expectedOrphans, orphans));
// Try removeTop again with a factor that should not change a thing
boost::shared_ptr<IndexFactorOrdered> newFactor2(new IndexFactorOrdered(_B_));
BayesNetOrdered<IndexConditionalOrdered> bn2;
SymbolicBayesTreeOrdered::Cliques orphans2;
keys.clear(); keys += _B_;
bayesTree.removeTop(keys, bn2, orphans2);
SymbolicFactorGraphOrdered factors2(bn2);
SymbolicFactorGraphOrdered expected2;
CHECK(assert_equal(expected2, factors2));
SymbolicBayesTreeOrdered::Cliques expectedOrphans2;
CHECK(assert_equal(expectedOrphans2, orphans2));
}
/* ************************************************************************* */
TEST( BayesTreeOrdered, removeTop2 )
{
SymbolicBayesTreeOrdered bayesTree = createAsiaSymbolicBayesTree();
// create two factors to be inserted
SymbolicFactorGraphOrdered newFactors;
newFactors.push_factor(_B_);
newFactors.push_factor(_S_);
// Remove the contaminated part of the Bayes tree
BayesNetOrdered<IndexConditionalOrdered> bn;
SymbolicBayesTreeOrdered::Cliques orphans;
list<Index> keys; keys += _B_,_S_;
bayesTree.removeTop(keys, bn, orphans);
SymbolicFactorGraphOrdered factors(bn);
// Check expected outcome
SymbolicFactorGraphOrdered expected;
expected.push_factor(_E_,_L_,_B_);
// expected.push_factor(_L_,_B_);
// expected.push_factor(_B_);
expected.push_factor(_S_,_L_,_B_);
CHECK(assert_equal(expected, factors));
SymbolicBayesTreeOrdered::Cliques expectedOrphans;
expectedOrphans += bayesTree[_T_], bayesTree[_X_];
CHECK(assert_equal(expectedOrphans, orphans));
}
/* ************************************************************************* */
TEST( BayesTreeOrdered, removeTop3 )
{
const Index _x4_=5, _l5_=6;
// simple test case that failed after COLAMD was fixed/activated
IndexConditionalOrdered::shared_ptr
X(new IndexConditionalOrdered(_l5_)),
A(new IndexConditionalOrdered(_x4_, _l5_)),
B(new IndexConditionalOrdered(_x2_, _x4_)),
C(new IndexConditionalOrdered(_x3_, _x2_));
// Ordering newOrdering;
// newOrdering += _x3_, _x2_, _x1_, _l2_, _l1_, _x4_, _l5_;
SymbolicBayesTreeOrdered bayesTree;
SymbolicBayesTreeOrdered::insert(bayesTree, X);
SymbolicBayesTreeOrdered::insert(bayesTree, A);
SymbolicBayesTreeOrdered::insert(bayesTree, B);
SymbolicBayesTreeOrdered::insert(bayesTree, C);
// remove all
list<Index> keys;
keys += _l5_, _x2_, _x3_, _x4_;
BayesNetOrdered<IndexConditionalOrdered> bn;
SymbolicBayesTreeOrdered::Cliques orphans;
bayesTree.removeTop(keys, bn, orphans);
SymbolicFactorGraphOrdered factors(bn);
CHECK(orphans.size() == 0);
}
/* ************************************************************************* */
TEST( BayesTreeOrdered, permute )
{
// creates a permutation and ensures that the nodes listing is updated
// initial keys - more than just 6 variables - for a system with 9 variables
const Index _A0_=8, _B0_=7, _C0_=6, _D0_=5, _E0_=4, _F0_=0;
// reduced keys - back to just 6 variables
const Index _A_=5, _B_=4, _C_=3, _D_=2, _E_=1, _F_=0;
// Create and verify the permutation
std::set<Index> indices; indices += _A0_, _B0_, _C0_, _D0_, _E0_, _F0_;
Permutation actReducingPermutation = gtsam::internal::createReducingPermutation(indices);
Permutation expReducingPermutation(6);
expReducingPermutation[_A_] = _A0_;
expReducingPermutation[_B_] = _B0_;
expReducingPermutation[_C_] = _C0_;
expReducingPermutation[_D_] = _D0_;
expReducingPermutation[_E_] = _E0_;
expReducingPermutation[_F_] = _F0_;
EXPECT(assert_equal(expReducingPermutation, actReducingPermutation));
// Invert the permutation
gtsam::internal::Reduction inv_reduction = gtsam::internal::Reduction::CreateAsInverse(expReducingPermutation);
// Build a bayes tree around reduced keys as if just eliminated from subset of factors/variables
IndexConditionalOrdered::shared_ptr
A(new IndexConditionalOrdered(_A_)),
B(new IndexConditionalOrdered(_B_, _A_)),
C(new IndexConditionalOrdered(_C_, _A_)),
D(new IndexConditionalOrdered(_D_, _C_)),
E(new IndexConditionalOrdered(_E_, _B_)),
F(new IndexConditionalOrdered(_F_, _E_));
SymbolicBayesTreeOrdered bayesTreeReduced;
SymbolicBayesTreeOrdered::insert(bayesTreeReduced, A);
SymbolicBayesTreeOrdered::insert(bayesTreeReduced, B);
SymbolicBayesTreeOrdered::insert(bayesTreeReduced, C);
SymbolicBayesTreeOrdered::insert(bayesTreeReduced, D);
SymbolicBayesTreeOrdered::insert(bayesTreeReduced, E);
SymbolicBayesTreeOrdered::insert(bayesTreeReduced, F);
// bayesTreeReduced.print("Reduced bayes tree");
// P( 4 5)
// P( 3 | 5)
// P( 2 | 3)
// P( 1 | 4)
// P( 0 | 1)
// Apply the permutation - should add placeholders for variables not present in nodes
SymbolicBayesTreeOrdered actBayesTree = *bayesTreeReduced.clone();
actBayesTree.permuteWithInverse(expReducingPermutation);
// actBayesTree.print("Full bayes tree");
// P( 7 8)
// P( 6 | 8)
// P( 5 | 6)
// P( 4 | 7)
// P( 0 | 4)
// check keys in cliques
std::vector<Index> expRootIndices; expRootIndices += _B0_, _A0_;
IndexConditionalOrdered::shared_ptr
expRoot(new IndexConditionalOrdered(expRootIndices, 2)), // root
A0(new IndexConditionalOrdered(_A0_)),
B0(new IndexConditionalOrdered(_B0_, _A0_)),
C0(new IndexConditionalOrdered(_C0_, _A0_)), // leaf level 1
D0(new IndexConditionalOrdered(_D0_, _C0_)), // leaf level 2
E0(new IndexConditionalOrdered(_E0_, _B0_)), // leaf level 2
F0(new IndexConditionalOrdered(_F0_, _E0_)); // leaf level 3
CHECK(actBayesTree.root());
EXPECT(assert_equal(*expRoot, *actBayesTree.root()->conditional()));
EXPECT(assert_equal(*C0, *actBayesTree.root()->children().front()->conditional()));
EXPECT(assert_equal(*D0, *actBayesTree.root()->children().front()->children().front()->conditional()));
EXPECT(assert_equal(*E0, *actBayesTree.root()->children().back()->conditional()));
EXPECT(assert_equal(*F0, *actBayesTree.root()->children().back()->children().front()->conditional()));
// check nodes structure
LONGS_EQUAL(9, actBayesTree.nodes().size());
SymbolicBayesTreeOrdered expFullTree;
SymbolicBayesTreeOrdered::insert(expFullTree, A0);
SymbolicBayesTreeOrdered::insert(expFullTree, B0);
SymbolicBayesTreeOrdered::insert(expFullTree, C0);
SymbolicBayesTreeOrdered::insert(expFullTree, D0);
SymbolicBayesTreeOrdered::insert(expFullTree, E0);
SymbolicBayesTreeOrdered::insert(expFullTree, F0);
EXPECT(assert_equal(expFullTree, actBayesTree));
}
///* ************************************************************************* */
///**
// * x2 - x3 - x4 - x5
// * | / \ |
// * x1 / \ x6
// */
//TEST( BayesTreeOrdered, insert )
//{
// // construct bayes tree by split the graph along the separator x3 - x4
// const Index _x1_=0, _x2_=1, _x6_=2, _x5_=3, _x3_=4, _x4_=5;
// SymbolicFactorGraphOrdered fg1, fg2, fg3;
// fg1.push_factor(_x3_, _x4_);
// fg2.push_factor(_x1_, _x2_);
// fg2.push_factor(_x2_, _x3_);
// fg2.push_factor(_x1_, _x3_);
// fg3.push_factor(_x5_, _x4_);
// fg3.push_factor(_x6_, _x5_);
// fg3.push_factor(_x6_, _x4_);
//
//// Ordering ordering1; ordering1 += _x3_, _x4_;
//// Ordering ordering2; ordering2 += _x1_, _x2_;
//// Ordering ordering3; ordering3 += _x6_, _x5_;
//
// BayesNetOrdered<IndexConditionalOrdered> bn1, bn2, bn3;
// bn1 = *SymbolicSequentialSolver::EliminateUntil(fg1, _x4_+1);
// bn2 = *SymbolicSequentialSolver::EliminateUntil(fg2, _x2_+1);
// bn3 = *SymbolicSequentialSolver::EliminateUntil(fg3, _x5_+1);
//
// // insert child cliques
// SymbolicBayesTreeOrdered actual;
// list<SymbolicBayesTreeOrdered::sharedClique> children;
// SymbolicBayesTreeOrdered::sharedClique r1 = actual.insert(bn2, children);
// SymbolicBayesTreeOrdered::sharedClique r2 = actual.insert(bn3, children);
//
// // insert root clique
// children.push_back(r1);
// children.push_back(r2);
// actual.insert(bn1, children, true);
//
// // traditional way
// SymbolicFactorGraphOrdered fg;
// fg.push_factor(_x3_, _x4_);
// fg.push_factor(_x1_, _x2_);
// fg.push_factor(_x2_, _x3_);
// fg.push_factor(_x1_, _x3_);
// fg.push_factor(_x5_, _x4_);
// fg.push_factor(_x6_, _x5_);
// fg.push_factor(_x6_, _x4_);
//
//// Ordering ordering; ordering += _x1_, _x2_, _x6_, _x5_, _x3_, _x4_;
// BayesNetOrdered<IndexConditionalOrdered> bn(*SymbolicSequentialSolver(fg).eliminate());
// SymbolicBayesTreeOrdered expected(bn);
// CHECK(assert_equal(expected, actual));
//
//}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

43
gtsam/inference/tests/testClusterTreeObsolete.cpp
View File

@ -1,43 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testClusterTree.cpp
* @brief Unit tests for Bayes Tree
* @author Kai Ni
* @author Frank Dellaert
*/
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/ClusterTreeOrdered.h>
using namespace gtsam;
// explicit instantiation and typedef
namespace gtsam { template class ClusterTreeOrdered<SymbolicFactorGraphOrdered>; }
typedef ClusterTreeOrdered<SymbolicFactorGraphOrdered> SymbolicClusterTree;
/* ************************************************************************* */
TEST(ClusterTreeOrdered, constructor) {
SymbolicClusterTree tree;
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

106
gtsam/inference/tests/testConditionalObsolete.cpp
View File

@ -1,106 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testConditional.cpp
* @brief Unit tests for IndexConditional class
* @author Frank Dellaert
*/
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
#include <gtsam/inference/IndexFactorOrdered.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST( IndexConditionalOrdered, empty )
{
IndexConditionalOrdered c0;
LONGS_EQUAL(0,c0.nrFrontals())
LONGS_EQUAL(0,c0.nrParents())
}
/* ************************************************************************* */
TEST( IndexConditionalOrdered, noParents )
{
IndexConditionalOrdered c0(0);
LONGS_EQUAL(1,c0.nrFrontals())
LONGS_EQUAL(0,c0.nrParents())
}
/* ************************************************************************* */
TEST( IndexConditionalOrdered, oneParents )
{
IndexConditionalOrdered c0(0,1);
LONGS_EQUAL(1,c0.nrFrontals())
LONGS_EQUAL(1,c0.nrParents())
}
/* ************************************************************************* */
TEST( IndexConditionalOrdered, twoParents )
{
IndexConditionalOrdered c0(0,1,2);
LONGS_EQUAL(1,c0.nrFrontals())
LONGS_EQUAL(2,c0.nrParents())
}
/* ************************************************************************* */
TEST( IndexConditionalOrdered, threeParents )
{
IndexConditionalOrdered c0(0,1,2,3);
LONGS_EQUAL(1,c0.nrFrontals())
LONGS_EQUAL(3,c0.nrParents())
}
/* ************************************************************************* */
TEST( IndexConditionalOrdered, fourParents )
{
vector<Index> parents;
parents += 1,2,3,4;
IndexConditionalOrdered c0(0,parents);
LONGS_EQUAL(1,c0.nrFrontals())
LONGS_EQUAL(4,c0.nrParents())
}
/* ************************************************************************* */
TEST( IndexConditionalOrdered, FromRange )
{
vector<Index> keys;
keys += 1,2,3,4,5;
IndexConditionalOrdered::shared_ptr c0(new IndexConditionalOrdered(keys,2));
LONGS_EQUAL(2,c0->nrFrontals())
LONGS_EQUAL(3,c0->nrParents())
}
/* ************************************************************************* */
TEST( IndexConditionalOrdered, equals )
{
IndexConditionalOrdered c0(0, 1, 2), c1(0, 1, 2), c2(1, 2, 3), c3(3,4);
CHECK(c0.equals(c1));
CHECK(c1.equals(c0));
CHECK(!c0.equals(c2));
CHECK(!c2.equals(c0));
CHECK(!c0.equals(c3));
CHECK(!c3.equals(c0));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

119
gtsam/inference/tests/testEliminationTreeObsolete.cpp
View File

@ -1,119 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testEliminationTree.cpp
* @brief
* @author Richard Roberts
* @date Oct 14, 2010
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/EliminationTreeOrdered-inl.h>
#include <gtsam/inference/SymbolicSequentialSolverOrdered.h>
using namespace gtsam;
using namespace std;
class EliminationTreeOrderedTester {
public:
// build hardcoded tree
static SymbolicEliminationTreeOrdered::shared_ptr buildHardcodedTree(const SymbolicFactorGraphOrdered& fg) {
SymbolicEliminationTreeOrdered::shared_ptr leaf0(new SymbolicEliminationTreeOrdered);
leaf0->add(fg[0]);
leaf0->add(fg[1]);
SymbolicEliminationTreeOrdered::shared_ptr node1(new SymbolicEliminationTreeOrdered(1));
node1->add(fg[2]);
node1->add(leaf0);
SymbolicEliminationTreeOrdered::shared_ptr node2(new SymbolicEliminationTreeOrdered(2));
node2->add(fg[3]);
node2->add(node1);
SymbolicEliminationTreeOrdered::shared_ptr leaf3(new SymbolicEliminationTreeOrdered(3));
leaf3->add(fg[4]);
SymbolicEliminationTreeOrdered::shared_ptr etree(new SymbolicEliminationTreeOrdered(4));
etree->add(leaf3);
etree->add(node2);
return etree;
}
};
TEST(EliminationTreeOrdered, Create)
{
// create example factor graph
SymbolicFactorGraphOrdered fg;
fg.push_factor(0, 1);
fg.push_factor(0, 2);
fg.push_factor(1, 4);
fg.push_factor(2, 4);
fg.push_factor(3, 4);
SymbolicEliminationTreeOrdered::shared_ptr expected = EliminationTreeOrderedTester::buildHardcodedTree(fg);
// Build from factor graph
SymbolicEliminationTreeOrdered::shared_ptr actual = SymbolicEliminationTreeOrdered::Create(fg);
CHECK(assert_equal(*expected,*actual));
}
/* ************************************************************************* */
// Test to drive elimination tree development
// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4)
/* ************************************************************************* */
TEST(EliminationTreeOrdered, eliminate )
{
// create expected Chordal bayes Net
SymbolicBayesNetOrdered expected;
expected.push_front(boost::make_shared<IndexConditionalOrdered>(4));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(3,4));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(2,4));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(1,2,4));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(0,1,2));
// Create factor graph
SymbolicFactorGraphOrdered fg;
fg.push_factor(0, 1);
fg.push_factor(0, 2);
fg.push_factor(1, 4);
fg.push_factor(2, 4);
fg.push_factor(3, 4);
// eliminate
SymbolicBayesNetOrdered actual = *SymbolicSequentialSolver(fg).eliminate();
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST(EliminationTreeOrdered, disconnected_graph) {
SymbolicFactorGraphOrdered fg;
fg.push_factor(0, 1);
fg.push_factor(0, 2);
fg.push_factor(1, 2);
fg.push_factor(3, 4);
CHECK_EXCEPTION(SymbolicEliminationTreeOrdered::Create(fg), DisconnectedGraphException);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

65
gtsam/inference/tests/testFactorGraphObsolete.cpp
View File

@ -1,65 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testFactorgraph.cpp
* @brief Unit tests for IndexFactor Graphs
* @author Christian Potthast
**/
/*STL/C++*/
#include <list>
#include <iostream>
#include <boost/shared_ptr.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/set.hpp> // for operator +=
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST(FactorGraphOrdered, eliminateFrontals) {
SymbolicFactorGraphOrdered sfgOrig;
sfgOrig.push_factor(0,1);
sfgOrig.push_factor(0,2);
sfgOrig.push_factor(1,3);
sfgOrig.push_factor(1,4);
sfgOrig.push_factor(2,3);
sfgOrig.push_factor(4,5);
IndexConditionalOrdered::shared_ptr actualCond;
SymbolicFactorGraphOrdered actualSfg;
boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2);
vector<Index> condIndices;
condIndices += 0,1,2,3,4;
IndexConditionalOrdered expectedCond(condIndices, 2);
SymbolicFactorGraphOrdered expectedSfg;
expectedSfg.push_factor(2,3);
expectedSfg.push_factor(4,5);
expectedSfg.push_factor(2,3,4);
EXPECT(assert_equal(expectedSfg, actualSfg));
EXPECT(assert_equal(expectedCond, *actualCond));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

151
gtsam/inference/tests/testISAMObsolete.cpp
View File

@ -1,151 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testISAM.cpp
* @brief Unit tests for ISAM
* @author Michael Kaess
*/
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/BayesNetOrdered.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/ISAMOrdered.h>
using namespace std;
using namespace gtsam;
typedef ISAMOrdered<IndexConditionalOrdered> SymbolicISAM;
/* ************************************************************************* */
// Some numbers that should be consistent among all smoother tests
//static double sigmax1 = 0.786153, sigmax2 = 0.687131 ,sigmax3 = 0.671512,
//sigmax4 = 0.669534, sigmax5 = sigmax3,, sigmax7 = sigmax1, sigmax6 = sigmax2;
/* ************************************************************************* */
//// SLAM example from RSS sqrtSAM paper
//SymbolicConditional::shared_ptr x3(new SymbolicConditional("x3")),
// x2(new SymbolicConditional("x2","x3")),
// x1(new SymbolicConditional("x1","x2","x3")),
// l1(new SymbolicConditional("l1","x1","x2")),
// l2(new SymbolicConditional("l2","x1","x3"));
//
//// ISAM for sqrtSAM example
//SymbolicISAM createSlamSymbolicISAM(){
// // Create using insert
// SymbolicISAM bayesTree_slam;
// bayesTree_slam.insert(x3);
// bayesTree_slam.insert(x2);
// bayesTree_slam.insert(x1);
// bayesTree_slam.insert(l2);
// bayesTree_slam.insert(l1);
// return bayesTree_slam;
//}
/* ************************************************************************* */
//// Conditionals for ASIA example from the tutorial with A and D evidence
//SymbolicConditional::shared_ptr
// B(new SymbolicConditional("B")),
// L(new SymbolicConditional("L", "B")),
// E(new SymbolicConditional("E", "B", "L")),
// S(new SymbolicConditional("S", "L", "B")),
// T(new SymbolicConditional("T", "E", "L")),
// X(new SymbolicConditional("X", "E"));
//
//// ISAM for Asia example
//SymbolicISAM createAsiaSymbolicISAM() {
// SymbolicISAM bayesTree;
// bayesTree.insert(B);
// bayesTree.insert(L);
// bayesTree.insert(E);
// bayesTree.insert(S);
// bayesTree.insert(T);
// bayesTree.insert(X);
// return bayesTree;
//}
//
///* ************************************************************************* */
//TEST( ISAM, iSAM )
//{
// SymbolicISAM bayesTree = createAsiaSymbolicISAM();
//
// // Now we modify the Bayes tree by inserting a new factor over B and S
//
// // New conditionals in modified top of the tree
// SymbolicConditional::shared_ptr
// S_(new SymbolicConditional("S")),
// B_(new SymbolicConditional("B", "S")),
// L_(new SymbolicConditional("L", "B", "S"));
//
// // Create expected Bayes tree
// SymbolicISAM expected;
// expected.insert(S_);
// expected.insert(B_);
// expected.insert(L_);
// expected.insert(E);
// expected.insert(T);
// expected.insert(X);
//
// // create new factors to be inserted
// SymbolicFactorGraph factorGraph;
// factorGraph.push_factor("B","S");
// factorGraph.push_factor("B");
//
// // do incremental inference
// bayesTree.update(factorGraph);
//
// // Check whether the same
// CHECK(assert_equal(expected,bayesTree));
//}
//
///* ************************************************************************* */
//TEST( ISAM, iSAM_slam )
//{
// // Create using insert
// SymbolicISAM bayesTree_slam = createSlamSymbolicISAM();
//
// //New conditionals for the expected Bayes tree
// SymbolicConditional::shared_ptr
// l1_(new SymbolicConditional("l1","x1","x2","x3"));
//
// // Create expected Bayes tree
// SymbolicISAM expected_slam;
// expected_slam.insert(x3);
// expected_slam.insert(x2);
// expected_slam.insert(x1);
// expected_slam.insert(l1_);
// expected_slam.insert(l2);
//
//
// // create new factors to be inserted
// SymbolicFactorGraph factorGraph_slam;
// factorGraph_slam.push_factor("x3","l1");
// factorGraph_slam.push_factor("x3");
//
// // do incremental inference
// bayesTree_slam.update(factorGraph_slam);
//
// // Check whether the same
// CHECK(assert_equal(expected_slam,bayesTree_slam));
//}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

99
gtsam/inference/tests/testJunctionTreeObsolete.cpp
View File

@ -1,99 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testJunctionTree.cpp
* @brief Unit tests for Junction Tree
* @author Kai Ni
* @author Frank Dellaert
*/
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/vector.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/JunctionTreeOrdered.h>
#include <gtsam/inference/ClusterTreeOrdered.h>
#include <gtsam/inference/JunctionTreeOrdered.h>
#include <gtsam/inference/IndexFactorOrdered.h>
#include <gtsam/inference/SymbolicSequentialSolverOrdered.h>
using namespace gtsam;
using namespace std;
typedef JunctionTreeOrdered<SymbolicFactorGraphOrdered> SymbolicJunctionTree;
/* ************************************************************************* *
* x1 - x2 - x3 - x4
* x3 x4
* x2 x1 : x3
****************************************************************************/
TEST( JunctionTreeOrdered, constructor )
{
const Index x2=0, x1=1, x3=2, x4=3;
SymbolicFactorGraphOrdered fg;
fg.push_factor(x2,x1);
fg.push_factor(x2,x3);
fg.push_factor(x3,x4);
SymbolicJunctionTree actual(fg);
vector<Index> frontal1; frontal1 += x3, x4;
vector<Index> frontal2; frontal2 += x2, x1;
vector<Index> sep1;
vector<Index> sep2; sep2 += x3;
CHECK(assert_equal(frontal1, actual.root()->frontal));
CHECK(assert_equal(sep1, actual.root()->separator));
LONGS_EQUAL(1, actual.root()->size());
CHECK(assert_equal(frontal2, actual.root()->children().front()->frontal));
CHECK(assert_equal(sep2, actual.root()->children().front()->separator));
LONGS_EQUAL(2, actual.root()->children().front()->size());
CHECK(assert_equal(*fg[2], *(*actual.root())[0]));
CHECK(assert_equal(*fg[0], *(*actual.root()->children().front())[0]));
CHECK(assert_equal(*fg[1], *(*actual.root()->children().front())[1]));
}
/* ************************************************************************* *
* x1 - x2 - x3 - x4
* x3 x4
* x2 x1 : x3
****************************************************************************/
TEST( JunctionTreeOrdered, eliminate)
{
const Index x2=0, x1=1, x3=2, x4=3;
SymbolicFactorGraphOrdered fg;
fg.push_factor(x2,x1);
fg.push_factor(x2,x3);
fg.push_factor(x3,x4);
SymbolicJunctionTree jt(fg);
SymbolicBayesTreeOrdered::sharedClique actual = jt.eliminate(&EliminateSymbolic);
BayesNetOrdered<IndexConditionalOrdered> bn(*SymbolicSequentialSolver(fg).eliminate());
SymbolicBayesTreeOrdered expected(bn);
// cout << "BT from JT:\n";
// actual->printTree("");
CHECK(assert_equal(*expected.root(), *actual));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

134
gtsam/inference/tests/testPermutationObsolete.cpp
View File

@ -1,134 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testPermutation.cpp
* @date Sep 22, 2011
* @author richard
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/PermutationOrdered.h>
#include <vector>
using namespace gtsam;
using namespace std;
/* ************************************************************************* */
TEST(Permutation, Identity) {
Permutation expected(5);
expected[0] = 0;
expected[1] = 1;
expected[2] = 2;
expected[3] = 3;
expected[4] = 4;
Permutation actual = Permutation::Identity(5);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Permutation, PullToFront) {
Permutation expected(5);
expected[0] = 4;
expected[1] = 0;
expected[2] = 2;
expected[3] = 1;
expected[4] = 3;
std::vector<Index> toFront;
toFront.push_back(4);
toFront.push_back(0);
toFront.push_back(2);
Permutation actual = Permutation::PullToFront(toFront, 5);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Permutation, PushToBack) {
Permutation expected(5);
expected[0] = 1;
expected[1] = 3;
expected[2] = 4;
expected[3] = 0;
expected[4] = 2;
std::vector<Index> toBack;
toBack.push_back(4);
toBack.push_back(0);
toBack.push_back(2);
Permutation actual = Permutation::PushToBack(toBack, 5);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Permutation, compose) {
Permutation p1(5);
p1[0] = 1;
p1[1] = 2;
p1[2] = 3;
p1[3] = 4;
p1[4] = 0;
Permutation p2(5);
p2[0] = 1;
p2[1] = 2;
p2[2] = 4;
p2[3] = 3;
p2[4] = 0;
Permutation expected(5);
expected[0] = 2;
expected[1] = 3;
expected[2] = 0;
expected[3] = 4;
expected[4] = 1;
Permutation actual = *p1.permute(p2);
EXPECT(assert_equal(expected, actual));
LONGS_EQUAL(p1[p2[0]], actual[0]);
LONGS_EQUAL(p1[p2[1]], actual[1]);
LONGS_EQUAL(p1[p2[2]], actual[2]);
LONGS_EQUAL(p1[p2[3]], actual[3]);
LONGS_EQUAL(p1[p2[4]], actual[4]);
}
/* ************************************************************************* */
TEST(Reduction, CreateFromPartialPermutation) {
Permutation selector(3);
selector[0] = 2;
selector[1] = 4;
selector[2] = 6;
Permutation p(3);
p[0] = 2;
p[1] = 0;
p[2] = 1;
internal::Reduction expected;
expected.insert(make_pair(2,6));
expected.insert(make_pair(4,2));
expected.insert(make_pair(6,4));
internal::Reduction actual = internal::Reduction::CreateFromPartialPermutation(selector, p);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

210
gtsam/inference/tests/testSymbolicBayesNetObsolete.cpp
View File

@ -1,210 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testSymbolicBayesNet.cpp
* @brief Unit tests for a symbolic Bayes chain
* @author Frank Dellaert
*/
#include <boost/assign/list_inserter.hpp> // for 'insert()'
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
using namespace std;
using namespace gtsam;
static const Index _L_ = 0;
static const Index _A_ = 1;
static const Index _B_ = 2;
static const Index _C_ = 3;
static const Index _D_ = 4;
static const Index _E_ = 5;
static IndexConditionalOrdered::shared_ptr
B(new IndexConditionalOrdered(_B_)),
L(new IndexConditionalOrdered(_L_, _B_));
/* ************************************************************************* */
TEST( SymbolicBayesNetOrdered, equals )
{
BayesNetOrdered<IndexConditionalOrdered> f1;
f1.push_back(B);
f1.push_back(L);
BayesNetOrdered<IndexConditionalOrdered> f2;
f2.push_back(L);
f2.push_back(B);
CHECK(f1.equals(f1));
CHECK(!f1.equals(f2));
}
/* ************************************************************************* */
TEST( SymbolicBayesNetOrdered, pop_front )
{
IndexConditionalOrdered::shared_ptr
A(new IndexConditionalOrdered(_A_,_B_,_C_)),
B(new IndexConditionalOrdered(_B_,_C_)),
C(new IndexConditionalOrdered(_C_));
// Expected after pop_front
BayesNetOrdered<IndexConditionalOrdered> expected;
expected.push_back(B);
expected.push_back(C);
// Actual
BayesNetOrdered<IndexConditionalOrdered> actual;
actual.push_back(A);
actual.push_back(B);
actual.push_back(C);
actual.pop_front();
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( SymbolicBayesNetOrdered, combine )
{
IndexConditionalOrdered::shared_ptr
A(new IndexConditionalOrdered(_A_,_B_,_C_)),
B(new IndexConditionalOrdered(_B_,_C_)),
C(new IndexConditionalOrdered(_C_));
// p(A|BC)
BayesNetOrdered<IndexConditionalOrdered> p_ABC;
p_ABC.push_back(A);
// P(BC)=P(B|C)P(C)
BayesNetOrdered<IndexConditionalOrdered> p_BC;
p_BC.push_back(B);
p_BC.push_back(C);
// P(ABC) = P(A|BC) P(BC)
p_ABC.push_back(p_BC);
BayesNetOrdered<IndexConditionalOrdered> expected;
expected.push_back(A);
expected.push_back(B);
expected.push_back(C);
CHECK(assert_equal(expected,p_ABC));
}
/* ************************************************************************* */
TEST(SymbolicBayesNetOrdered, find) {
SymbolicBayesNetOrdered bn;
bn += IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(_A_, _B_));
std::vector<Index> keys;
keys.push_back(_B_);
keys.push_back(_C_);
keys.push_back(_D_);
bn += IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(keys,2));
bn += IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(_D_));
SymbolicBayesNetOrdered::iterator expected = bn.begin(); ++ expected;
SymbolicBayesNetOrdered::iterator actual = bn.find(_C_);
EXPECT(assert_equal(**expected, **actual));
}
/* ************************************************************************* */
TEST_UNSAFE(SymbolicBayesNetOrdered, popLeaf) {
IndexConditionalOrdered::shared_ptr
A(new IndexConditionalOrdered(_A_,_E_)),
B(new IndexConditionalOrdered(_B_,_E_)),
C(new IndexConditionalOrdered(_C_,_D_)),
D(new IndexConditionalOrdered(_D_,_E_)),
E(new IndexConditionalOrdered(_E_));
// BayesNet after popping A
SymbolicBayesNetOrdered expected1;
expected1 += B, C, D, E;
// BayesNet after popping C
SymbolicBayesNetOrdered expected2;
expected2 += A, B, D, E;
// BayesNet after popping C and D
SymbolicBayesNetOrdered expected3;
expected3 += A, B, E;
// BayesNet after popping C and A
SymbolicBayesNetOrdered expected4;
expected4 += B, D, E;
// BayesNet after popping A
SymbolicBayesNetOrdered actual1;
actual1 += A, B, C, D, E;
actual1.popLeaf(actual1.find(_A_));
// BayesNet after popping C
SymbolicBayesNetOrdered actual2;
actual2 += A, B, C, D, E;
actual2.popLeaf(actual2.find(_C_));
// BayesNet after popping C and D
SymbolicBayesNetOrdered actual3;
actual3 += A, B, C, D, E;
actual3.popLeaf(actual3.find(_C_));
actual3.popLeaf(actual3.find(_D_));
// BayesNet after popping C and A
SymbolicBayesNetOrdered actual4;
actual4 += A, B, C, D, E;
actual4.popLeaf(actual4.find(_C_));
actual4.popLeaf(actual4.find(_A_));
EXPECT(assert_equal(expected1, actual1));
EXPECT(assert_equal(expected2, actual2));
EXPECT(assert_equal(expected3, actual3));
EXPECT(assert_equal(expected4, actual4));
// Try to remove a non-leaf node (this test is not working in non-debug mode)
//#undef NDEBUG_SAVED
//#ifdef NDEBUG
//#define NDEBUG_SAVED
//#endif
//
//#undef NDEBUG
// SymbolicBayesNetOrdered actual5;
// actual5 += A, B, C, D, E;
// CHECK_EXCEPTION(actual5.popLeaf(actual5.find(_D_)), std::invalid_argument);
//
//#ifdef NDEBUG_SAVED
//#define NDEBUG
//#endif
}
/* ************************************************************************* */
TEST(SymbolicBayesNetOrdered, saveGraph) {
SymbolicBayesNetOrdered bn;
bn += IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(_A_, _B_));
std::vector<Index> keys;
keys.push_back(_B_);
keys.push_back(_C_);
keys.push_back(_D_);
bn += IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(keys,2));
bn += IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(_D_));
bn.saveGraph("SymbolicBayesNet.dot");
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

323
gtsam/inference/tests/testSymbolicBayesTreeObsolete.cpp
View File

@ -1,323 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testSymbolicBayesTree.cpp
* @date sept 15, 2012
* @author Frank Dellaert
*/
#include <gtsam/inference/SymbolicSequentialSolverOrdered.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/BayesTreeOrdered.h>
#include <boost/assign/list_of.hpp>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
static bool debug = false;
/* ************************************************************************* */
TEST_UNSAFE( SymbolicBayesTreeOrdered, thinTree ) {
// create a thin-tree Bayesnet, a la Jean-Guillaume
SymbolicBayesNetOrdered bayesNet;
bayesNet.push_front(boost::make_shared<IndexConditionalOrdered>(14));
bayesNet.push_front(boost::make_shared<IndexConditionalOrdered>(13, 14));
bayesNet.push_front(boost::make_shared<IndexConditionalOrdered>(12, 14));
bayesNet.push_front(boost::make_shared<IndexConditionalOrdered>(11, 13, 14));
bayesNet.push_front(boost::make_shared<IndexConditionalOrdered>(10, 13, 14));
bayesNet.push_front(boost::make_shared<IndexConditionalOrdered>(9, 12, 14));
bayesNet.push_front(boost::make_shared<IndexConditionalOrdered>(8, 12, 14));
bayesNet.push_front(boost::make_shared<IndexConditionalOrdered>(7, 11, 13));
bayesNet.push_front(boost::make_shared<IndexConditionalOrdered>(6, 11, 13));
bayesNet.push_front(boost::make_shared<IndexConditionalOrdered>(5, 10, 13));
bayesNet.push_front(boost::make_shared<IndexConditionalOrdered>(4, 10, 13));
bayesNet.push_front(boost::make_shared<IndexConditionalOrdered>(3, 9, 12));
bayesNet.push_front(boost::make_shared<IndexConditionalOrdered>(2, 9, 12));
bayesNet.push_front(boost::make_shared<IndexConditionalOrdered>(1, 8, 12));
bayesNet.push_front(boost::make_shared<IndexConditionalOrdered>(0, 8, 12));
if (debug) {
GTSAM_PRINT(bayesNet);
bayesNet.saveGraph("/tmp/symbolicBayesNet.dot");
}
// create a BayesTree out of a Bayes net
SymbolicBayesTreeOrdered bayesTree(bayesNet);
if (debug) {
GTSAM_PRINT(bayesTree);
bayesTree.saveGraph("/tmp/symbolicBayesTree.dot");
}
SymbolicBayesTreeOrdered::Clique::shared_ptr R = bayesTree.root();
{
// check shortcut P(S9||R) to root
SymbolicBayesTreeOrdered::Clique::shared_ptr c = bayesTree[9];
SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic);
SymbolicBayesNetOrdered expected;
EXPECT(assert_equal(expected, shortcut));
}
{
// check shortcut P(S8||R) to root
SymbolicBayesTreeOrdered::Clique::shared_ptr c = bayesTree[8];
SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic);
SymbolicBayesNetOrdered expected;
expected.push_front(boost::make_shared<IndexConditionalOrdered>(12, 14));
EXPECT(assert_equal(expected, shortcut));
}
{
// check shortcut P(S4||R) to root
SymbolicBayesTreeOrdered::Clique::shared_ptr c = bayesTree[4];
SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic);
SymbolicBayesNetOrdered expected;
expected.push_front(boost::make_shared<IndexConditionalOrdered>(10, 13, 14));
EXPECT(assert_equal(expected, shortcut));
}
{
// check shortcut P(S2||R) to root
SymbolicBayesTreeOrdered::Clique::shared_ptr c = bayesTree[2];
SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic);
SymbolicBayesNetOrdered expected;
expected.push_front(boost::make_shared<IndexConditionalOrdered>(12, 14));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(9, 12, 14));
EXPECT(assert_equal(expected, shortcut));
}
{
// check shortcut P(S0||R) to root
SymbolicBayesTreeOrdered::Clique::shared_ptr c = bayesTree[0];
SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic);
SymbolicBayesNetOrdered expected;
expected.push_front(boost::make_shared<IndexConditionalOrdered>(12, 14));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(8, 12, 14));
EXPECT(assert_equal(expected, shortcut));
}
SymbolicBayesNetOrdered::shared_ptr actualJoint;
// Check joint P(8,2)
if (false) { // TODO, not disjoint
actualJoint = bayesTree.jointBayesNet(8, 2, EliminateSymbolic);
SymbolicBayesNetOrdered expected;
expected.push_front(boost::make_shared<IndexConditionalOrdered>(8));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(2, 8));
EXPECT(assert_equal(expected, *actualJoint));
}
// Check joint P(1,2)
if (false) { // TODO, not disjoint
actualJoint = bayesTree.jointBayesNet(1, 2, EliminateSymbolic);
SymbolicBayesNetOrdered expected;
expected.push_front(boost::make_shared<IndexConditionalOrdered>(2));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(1, 2));
EXPECT(assert_equal(expected, *actualJoint));
}
// Check joint P(2,6)
if (true) {
actualJoint = bayesTree.jointBayesNet(2, 6, EliminateSymbolic);
SymbolicBayesNetOrdered expected;
expected.push_front(boost::make_shared<IndexConditionalOrdered>(6));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(2, 6));
EXPECT(assert_equal(expected, *actualJoint));
}
// Check joint P(4,6)
if (false) { // TODO, not disjoint
actualJoint = bayesTree.jointBayesNet(4, 6, EliminateSymbolic);
SymbolicBayesNetOrdered expected;
expected.push_front(boost::make_shared<IndexConditionalOrdered>(6));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(4, 6));
EXPECT(assert_equal(expected, *actualJoint));
}
}
/* ************************************************************************* *
Bayes tree for smoother with "natural" ordering:
C1 5 6
C2 4 : 5
C3 3 : 4
C4 2 : 3
C5 1 : 2
C6 0 : 1
**************************************************************************** */
TEST_UNSAFE( SymbolicBayesTreeOrdered, linear_smoother_shortcuts ) {
// Create smoother with 7 nodes
SymbolicFactorGraphOrdered smoother;
smoother.push_factor(0);
smoother.push_factor(0, 1);
smoother.push_factor(1, 2);
smoother.push_factor(2, 3);
smoother.push_factor(3, 4);
smoother.push_factor(4, 5);
smoother.push_factor(5, 6);
BayesNetOrdered<IndexConditionalOrdered> bayesNet =
*SymbolicSequentialSolver(smoother).eliminate();
if (debug) {
GTSAM_PRINT(bayesNet);
bayesNet.saveGraph("/tmp/symbolicBayesNet.dot");
}
// create a BayesTree out of a Bayes net
SymbolicBayesTreeOrdered bayesTree(bayesNet);
if (debug) {
GTSAM_PRINT(bayesTree);
bayesTree.saveGraph("/tmp/symbolicBayesTree.dot");
}
SymbolicBayesTreeOrdered::Clique::shared_ptr R = bayesTree.root();
{
// check shortcut P(S2||R) to root
SymbolicBayesTreeOrdered::Clique::shared_ptr c = bayesTree[4]; // 4 is frontal in C2
SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic);
SymbolicBayesNetOrdered expected;
EXPECT(assert_equal(expected, shortcut));
}
{
// check shortcut P(S3||R) to root
SymbolicBayesTreeOrdered::Clique::shared_ptr c = bayesTree[3]; // 3 is frontal in C3
SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic);
SymbolicBayesNetOrdered expected;
expected.push_front(boost::make_shared<IndexConditionalOrdered>(4, 5));
EXPECT(assert_equal(expected, shortcut));
}
{
// check shortcut P(S4||R) to root
SymbolicBayesTreeOrdered::Clique::shared_ptr c = bayesTree[2]; // 2 is frontal in C4
SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic);
SymbolicBayesNetOrdered expected;
expected.push_front(boost::make_shared<IndexConditionalOrdered>(3, 5));
EXPECT(assert_equal(expected, shortcut));
}
}
/* ************************************************************************* */
// from testSymbolicJunctionTree, which failed at one point
TEST(SymbolicBayesTreeOrdered, complicatedMarginal) {
// Create the conditionals to go in the BayesTree
list<Index> L;
L = list_of(1)(2)(5);
IndexConditionalOrdered::shared_ptr R_1_2(new IndexConditionalOrdered(L, 2));
L = list_of(3)(4)(6);
IndexConditionalOrdered::shared_ptr R_3_4(new IndexConditionalOrdered(L, 2));
L = list_of(5)(6)(7)(8);
IndexConditionalOrdered::shared_ptr R_5_6(new IndexConditionalOrdered(L, 2));
L = list_of(7)(8)(11);
IndexConditionalOrdered::shared_ptr R_7_8(new IndexConditionalOrdered(L, 2));
L = list_of(9)(10)(11)(12);
IndexConditionalOrdered::shared_ptr R_9_10(new IndexConditionalOrdered(L, 2));
L = list_of(11)(12);
IndexConditionalOrdered::shared_ptr R_11_12(new IndexConditionalOrdered(L, 2));
// Symbolic Bayes Tree
typedef SymbolicBayesTreeOrdered::Clique Clique;
typedef SymbolicBayesTreeOrdered::sharedClique sharedClique;
// Create Bayes Tree
SymbolicBayesTreeOrdered bt;
bt.insert(sharedClique(new Clique(R_11_12)));
bt.insert(sharedClique(new Clique(R_9_10)));
bt.insert(sharedClique(new Clique(R_7_8)));
bt.insert(sharedClique(new Clique(R_5_6)));
bt.insert(sharedClique(new Clique(R_3_4)));
bt.insert(sharedClique(new Clique(R_1_2)));
if (debug) {
GTSAM_PRINT(bt);
bt.saveGraph("/tmp/symbolicBayesTree.dot");
}
SymbolicBayesTreeOrdered::Clique::shared_ptr R = bt.root();
SymbolicBayesNetOrdered empty;
// Shortcut on 9
{
SymbolicBayesTreeOrdered::Clique::shared_ptr c = bt[9];
SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic);
EXPECT(assert_equal(empty, shortcut));
}
// Shortcut on 7
{
SymbolicBayesTreeOrdered::Clique::shared_ptr c = bt[7];
SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic);
EXPECT(assert_equal(empty, shortcut));
}
// Shortcut on 5
{
SymbolicBayesTreeOrdered::Clique::shared_ptr c = bt[5];
SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic);
SymbolicBayesNetOrdered expected;
expected.push_front(boost::make_shared<IndexConditionalOrdered>(8, 11));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(7, 8, 11));
EXPECT(assert_equal(expected, shortcut));
}
// Shortcut on 3
{
SymbolicBayesTreeOrdered::Clique::shared_ptr c = bt[3];
SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic);
SymbolicBayesNetOrdered expected;
expected.push_front(boost::make_shared<IndexConditionalOrdered>(6, 11));
EXPECT(assert_equal(expected, shortcut));
}
// Shortcut on 1
{
SymbolicBayesTreeOrdered::Clique::shared_ptr c = bt[1];
SymbolicBayesNetOrdered shortcut = c->shortcut(R, EliminateSymbolic);
SymbolicBayesNetOrdered expected;
expected.push_front(boost::make_shared<IndexConditionalOrdered>(5, 11));
EXPECT(assert_equal(expected, shortcut));
}
// Marginal on 5
{
IndexFactorOrdered::shared_ptr actual = bt.marginalFactor(5, EliminateSymbolic);
EXPECT(assert_equal(IndexFactorOrdered(5), *actual, 1e-1));
}
// Shortcut on 6
{
IndexFactorOrdered::shared_ptr actual = bt.marginalFactor(6, EliminateSymbolic);
EXPECT(assert_equal(IndexFactorOrdered(6), *actual, 1e-1));
}
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

107
gtsam/inference/tests/testSymbolicFactorGraphObsolete.cpp
View File

@ -1,107 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testSymbolicFactorGraph.cpp
* @brief Unit tests for a symbolic IndexFactor Graph
* @author Frank Dellaert
*/
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/BayesNetOrdered-inl.h>
#include <gtsam/inference/IndexFactorOrdered.h>
#include <gtsam/inference/FactorGraphOrdered.h>
#include <gtsam/inference/SymbolicSequentialSolverOrdered.h>
using namespace std;
using namespace gtsam;
static const Index vx2 = 0;
static const Index vx1 = 1;
static const Index vl1 = 2;
///* ************************************************************************* */
//TEST( SymbolicFactorGraphOrdered, EliminateOne )
//{
// // create a test graph
// SymbolicFactorGraphOrdered fg;
// fg.push_factor(vx2, vx1);
//
// SymbolicSequentialSolver::EliminateUntil(fg, vx2+1);
// SymbolicFactorGraphOrdered expected;
// expected.push_back(boost::shared_ptr<IndexFactor>());
// expected.push_factor(vx1);
//
// CHECK(assert_equal(expected, fg));
//}
/* ************************************************************************* */
TEST( SymbolicFactorGraphOrdered, constructFromBayesNet )
{
// create expected factor graph
SymbolicFactorGraphOrdered expected;
expected.push_factor(vx2, vx1, vl1);
expected.push_factor(vx1, vl1);
expected.push_factor(vx1);
// create Bayes Net
IndexConditionalOrdered::shared_ptr x2(new IndexConditionalOrdered(vx2, vx1, vl1));
IndexConditionalOrdered::shared_ptr l1(new IndexConditionalOrdered(vx1, vl1));
IndexConditionalOrdered::shared_ptr x1(new IndexConditionalOrdered(vx1));
BayesNetOrdered<IndexConditionalOrdered> bayesNet;
bayesNet.push_back(x2);
bayesNet.push_back(l1);
bayesNet.push_back(x1);
// create actual factor graph from a Bayes Net
SymbolicFactorGraphOrdered actual(bayesNet);
CHECK(assert_equal((SymbolicFactorGraphOrdered)expected,actual));
}
/* ************************************************************************* */
TEST( SymbolicFactorGraphOrdered, push_back )
{
// Create two factor graphs and expected combined graph
SymbolicFactorGraphOrdered fg1, fg2, expected;
fg1.push_factor(vx1);
fg1.push_factor(vx2, vx1);
fg2.push_factor(vx1, vl1);
fg2.push_factor(vx2, vl1);
expected.push_factor(vx1);
expected.push_factor(vx2, vx1);
expected.push_factor(vx1, vl1);
expected.push_factor(vx2, vl1);
// combine
SymbolicFactorGraphOrdered actual = combine(fg1, fg2);
CHECK(assert_equal(expected, actual));
// combine using push_back
fg1.push_back(fg2);
CHECK(assert_equal(expected, fg1));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

113
gtsam/inference/tests/testSymbolicFactorObsolete.cpp
View File

@ -1,113 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testSymbolicFactor.cpp
* @brief Unit tests for a symbolic IndexFactor
* @author Frank Dellaert
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/IndexFactorOrdered.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <boost/assign/std/vector.hpp>
#include <boost/tuple/tuple.hpp>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
/* ************************************************************************* */
TEST(SymbolicFactor, constructor) {
// Frontals sorted, parents not sorted
vector<Index> keys1; keys1 += 3, 4, 5, 9, 7, 8;
(void)IndexConditionalOrdered(keys1, 3);
// // Frontals not sorted
// vector<Index> keys2; keys2 += 3, 5, 4, 9, 7, 8;
// (void)IndexConditionalOrdered::FromRange(keys2.begin(), keys2.end(), 3);
// // Frontals not before parents
// vector<Index> keys3; keys3 += 3, 4, 5, 1, 7, 8;
// (void)IndexConditionalOrdered::FromRange(keys3.begin(), keys3.end(), 3);
}
/* ************************************************************************* */
#ifdef TRACK_ELIMINATE
TEST(SymbolicFactor, eliminate) {
vector<Index> keys; keys += 2, 3, 4, 6, 7, 9, 10, 11;
IndexFactorOrdered actual(keys.begin(), keys.end());
BayesNetOrdered<IndexConditionalOrdered> fragment = *actual.eliminate(3);
IndexFactorOrdered expected(keys.begin()+3, keys.end());
IndexConditionalOrdered::shared_ptr expected0 = IndexConditionalOrdered::FromRange(keys.begin(), keys.end(), 1);
IndexConditionalOrdered::shared_ptr expected1 = IndexConditionalOrdered::FromRange(keys.begin()+1, keys.end(), 1);
IndexConditionalOrdered::shared_ptr expected2 = IndexConditionalOrdered::FromRange(keys.begin()+2, keys.end(), 1);
CHECK(assert_equal(fragment.size(), size_t(3)));
CHECK(assert_equal(expected, actual));
BayesNetOrdered<IndexConditionalOrdered>::const_iterator fragmentCond = fragment.begin();
CHECK(assert_equal(**fragmentCond++, *expected0));
CHECK(assert_equal(**fragmentCond++, *expected1));
CHECK(assert_equal(**fragmentCond++, *expected2));
}
#endif
/* ************************************************************************* */
TEST(SymbolicFactor, EliminateSymbolic) {
SymbolicFactorGraphOrdered factors;
factors.push_factor(2,4,6);
factors.push_factor(1,2,5);
factors.push_factor(0,3);
IndexFactorOrdered expectedFactor(4,5,6);
std::vector<Index> keys; keys += 0,1,2,3,4,5,6;
IndexConditionalOrdered::shared_ptr expectedConditional(new IndexConditionalOrdered(keys, 4));
IndexFactorOrdered::shared_ptr actualFactor;
IndexConditionalOrdered::shared_ptr actualConditional;
boost::tie(actualConditional, actualFactor) = EliminateSymbolic(factors, 4);
CHECK(assert_equal(*expectedConditional, *actualConditional));
CHECK(assert_equal(expectedFactor, *actualFactor));
// BayesNetOrdered<IndexConditionalOrdered> expected_bn;
// vector<Index> parents;
//
// parents.clear(); parents += 1,2,3,4,5,6;
// expected_bn.push_back(IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(0, parents)));
//
// parents.clear(); parents += 2,3,4,5,6;
// expected_bn.push_back(IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(1, parents)));
//
// parents.clear(); parents += 3,4,5,6;
// expected_bn.push_back(IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(2, parents)));
//
// parents.clear(); parents += 4,5,6;
// expected_bn.push_back(IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(3, parents)));
//
// BayesNetOrdered<IndexConditionalOrdered>::shared_ptr actual_bn;
// IndexFactor::shared_ptr actual_factor;
// boost::tie(actual_bn, actual_factor) = EliminateSymbolic(factors, 4);
//
// CHECK(assert_equal(expected_bn, *actual_bn));
// CHECK(assert_equal(expected_factor, *actual_factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

130
gtsam/inference/tests/testSymbolicSequentialSolverObsolete.cpp
View File

@ -1,130 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testSymbolicSequentialSolver.cpp
* @brief Unit tests for a symbolic sequential solver routines
* @author Frank Dellaert
* @date Sept 16, 2012
*/
#include <gtsam/inference/SymbolicSequentialSolverOrdered.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST( SymbolicSequentialSolver, SymbolicSequentialSolver ) {
// create factor graph
SymbolicFactorGraphOrdered g;
g.push_factor(2, 1, 0);
g.push_factor(2, 0);
g.push_factor(2);
// test solver is Testable
SymbolicSequentialSolver solver(g);
// GTSAM_PRINT(solver);
EXPECT(assert_equal(solver,solver));
}
/* ************************************************************************* */
TEST( SymbolicSequentialSolver, inference ) {
// Create factor graph
SymbolicFactorGraphOrdered fg;
fg.push_factor(0, 1);
fg.push_factor(0, 2);
fg.push_factor(1, 4);
fg.push_factor(2, 4);
fg.push_factor(3, 4);
// eliminate
SymbolicSequentialSolver solver(fg);
SymbolicBayesNetOrdered::shared_ptr actual = solver.eliminate();
SymbolicBayesNetOrdered expected;
expected.push_front(boost::make_shared<IndexConditionalOrdered>(4));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(3, 4));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(2, 4));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(1, 2, 4));
expected.push_front(boost::make_shared<IndexConditionalOrdered>(0, 1, 2));
EXPECT(assert_equal(expected,*actual));
{
// jointBayesNet
vector<Index> js;
js.push_back(0);
js.push_back(4);
js.push_back(3);
SymbolicBayesNetOrdered::shared_ptr actualBN = solver.jointBayesNet(js);
SymbolicBayesNetOrdered expectedBN;
expectedBN.push_front(boost::make_shared<IndexConditionalOrdered>(3));
expectedBN.push_front(boost::make_shared<IndexConditionalOrdered>(4, 3));
expectedBN.push_front(boost::make_shared<IndexConditionalOrdered>(0, 4));
EXPECT( assert_equal(expectedBN,*actualBN));
// jointFactorGraph
SymbolicFactorGraphOrdered::shared_ptr actualFG = solver.jointFactorGraph(js);
SymbolicFactorGraphOrdered expectedFG;
expectedFG.push_factor(0, 4);
expectedFG.push_factor(4, 3);
expectedFG.push_factor(3);
EXPECT( assert_equal(expectedFG,(SymbolicFactorGraphOrdered)(*actualFG)));
}
{
// jointBayesNet
vector<Index> js;
js.push_back(0);
js.push_back(2);
js.push_back(3);
SymbolicBayesNetOrdered::shared_ptr actualBN = solver.jointBayesNet(js);
SymbolicBayesNetOrdered expectedBN;
expectedBN.push_front(boost::make_shared<IndexConditionalOrdered>(2));
expectedBN.push_front(boost::make_shared<IndexConditionalOrdered>(3, 2));
expectedBN.push_front(boost::make_shared<IndexConditionalOrdered>(0, 3, 2));
EXPECT( assert_equal(expectedBN,*actualBN));
// jointFactorGraph
SymbolicFactorGraphOrdered::shared_ptr actualFG = solver.jointFactorGraph(js);
SymbolicFactorGraphOrdered expectedFG;
expectedFG.push_factor(0, 3, 2);
expectedFG.push_factor(3, 2);
expectedFG.push_factor(2);
EXPECT( assert_equal(expectedFG,(SymbolicFactorGraphOrdered)(*actualFG)));
}
{
// conditionalBayesNet
vector<Index> js;
js.push_back(0);
js.push_back(2);
js.push_back(3);
size_t nrFrontals = 2;
SymbolicBayesNetOrdered::shared_ptr actualBN = //
solver.conditionalBayesNet(js, nrFrontals);
SymbolicBayesNetOrdered expectedBN;
expectedBN.push_front(boost::make_shared<IndexConditionalOrdered>(2, 3));
expectedBN.push_front(boost::make_shared<IndexConditionalOrdered>(0, 2, 3));
EXPECT( assert_equal(expectedBN,*actualBN));
}
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

123
gtsam/inference/tests/testVariableIndexObsolete.cpp
View File

@ -1,123 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testVariableIndex.cpp
* @brief
* @author Richard Roberts
* @date Sep 26, 2010
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/VariableIndexOrdered.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST(VariableIndexOrdered, augment) {
SymbolicFactorGraphOrdered fg1, fg2;
fg1.push_factor(0, 1);
fg1.push_factor(0, 2);
fg1.push_factor(5, 9);
fg1.push_factor(2, 3);
fg2.push_factor(1, 3);
fg2.push_factor(2, 4);
fg2.push_factor(3, 5);
fg2.push_factor(5, 6);
SymbolicFactorGraphOrdered fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2);
VariableIndexOrdered expected(fgCombined);
VariableIndexOrdered actual(fg1);
actual.augment(fg2);
LONGS_EQUAL(16, actual.nEntries());
LONGS_EQUAL(8, actual.nFactors());
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(VariableIndexOrdered, remove) {
SymbolicFactorGraphOrdered fg1, fg2;
fg1.push_factor(0, 1);
fg1.push_factor(0, 2);
fg1.push_factor(5, 9);
fg1.push_factor(2, 3);
fg2.push_factor(1, 3);
fg2.push_factor(2, 4);
fg2.push_factor(3, 5);
fg2.push_factor(5, 6);
SymbolicFactorGraphOrdered fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2);
// Create a factor graph containing only the factors from fg2 and with null
// factors in the place of those of fg1, so that the factor indices are correct.
SymbolicFactorGraphOrdered fg2removed(fgCombined);
fg2removed.remove(0); fg2removed.remove(1); fg2removed.remove(2); fg2removed.remove(3);
// The expected VariableIndex has the same factor indices as fgCombined but
// with entries from fg1 removed, and still has all 10 variables.
VariableIndexOrdered expected(fg2removed, 10);
VariableIndexOrdered actual(fgCombined);
vector<size_t> indices;
indices.push_back(0); indices.push_back(1); indices.push_back(2); indices.push_back(3);
actual.remove(indices, fg1);
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(VariableIndexOrdered, deep_copy) {
SymbolicFactorGraphOrdered fg1, fg2;
fg1.push_factor(0, 1);
fg1.push_factor(0, 2);
fg1.push_factor(5, 9);
fg1.push_factor(2, 3);
fg2.push_factor(1, 3);
fg2.push_factor(2, 4);
fg2.push_factor(3, 5);
fg2.push_factor(5, 6);
// Create original graph and VariableIndex
SymbolicFactorGraphOrdered fgOriginal; fgOriginal.push_back(fg1); fgOriginal.push_back(fg2);
VariableIndexOrdered original(fgOriginal);
VariableIndexOrdered expectedOriginal(fgOriginal);
// Create a factor graph containing only the factors from fg2 and with null
// factors in the place of those of fg1, so that the factor indices are correct.
SymbolicFactorGraphOrdered fg2removed(fgOriginal);
fg2removed.remove(0); fg2removed.remove(1); fg2removed.remove(2); fg2removed.remove(3);
VariableIndexOrdered expectedRemoved(fg2removed);
// Create a clone and modify the clone - the original should not change
VariableIndexOrdered clone(original);
vector<size_t> indices;
indices.push_back(0); indices.push_back(1); indices.push_back(2); indices.push_back(3);
clone.remove(indices, fg1);
// When modifying the clone, the original should have stayed the same
EXPECT(assert_equal(expectedOriginal, original));
EXPECT(assert_equal(expectedRemoved, clone));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

255
gtsam/linear/GaussianBayesNetOrdered.cpp
View File

@ -1,255 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GaussianBayesNet.cpp
* @brief Chordal Bayes Net, the result of eliminating a factor graph
* @author Frank Dellaert
*/
#include <gtsam/linear/GaussianBayesNetOrdered.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
#include <gtsam/linear/VectorValuesOrdered.h>
#include <gtsam/inference/BayesNetOrdered-inl.h>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/tuple/tuple.hpp>
#include <stdarg.h>
using namespace std;
using namespace gtsam;
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
#define REVERSE_FOREACH_PAIR( KEY, VAL, COL) BOOST_REVERSE_FOREACH (boost::tie(KEY,VAL),COL)
namespace gtsam {
// Explicitly instantiate so we don't have to include everywhere
template class BayesNetOrdered<GaussianConditionalOrdered>;
/* ************************************************************************* */
GaussianBayesNetOrdered scalarGaussian(Index key, double mu, double sigma) {
GaussianBayesNetOrdered bn;
GaussianConditionalOrdered::shared_ptr
conditional(new GaussianConditionalOrdered(key, Vector_(1,mu)/sigma, eye(1)/sigma, ones(1)));
bn.push_back(conditional);
return bn;
}
/* ************************************************************************* */
GaussianBayesNetOrdered simpleGaussian(Index key, const Vector& mu, double sigma) {
GaussianBayesNetOrdered bn;
size_t n = mu.size();
GaussianConditionalOrdered::shared_ptr
conditional(new GaussianConditionalOrdered(key, mu/sigma, eye(n)/sigma, ones(n)));
bn.push_back(conditional);
return bn;
}
/* ************************************************************************* */
void push_front(GaussianBayesNetOrdered& bn, Index key, Vector d, Matrix R,
Index name1, Matrix S, Vector sigmas) {
GaussianConditionalOrdered::shared_ptr cg(new GaussianConditionalOrdered(key, d, R, name1, S, sigmas));
bn.push_front(cg);
}
/* ************************************************************************* */
void push_front(GaussianBayesNetOrdered& bn, Index key, Vector d, Matrix R,
Index name1, Matrix S, Index name2, Matrix T, Vector sigmas) {
GaussianConditionalOrdered::shared_ptr cg(new GaussianConditionalOrdered(key, d, R, name1, S, name2, T, sigmas));
bn.push_front(cg);
}
/* ************************************************************************* */
boost::shared_ptr<VectorValuesOrdered> allocateVectorValues(const GaussianBayesNetOrdered& bn) {
vector<size_t> dimensions(bn.size());
Index var = 0;
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditionalOrdered> conditional, bn) {
dimensions[var++] = conditional->dim();
}
return boost::shared_ptr<VectorValuesOrdered>(new VectorValuesOrdered(dimensions));
}
/* ************************************************************************* */
VectorValuesOrdered optimize(const GaussianBayesNetOrdered& bn) {
VectorValuesOrdered x = *allocateVectorValues(bn);
optimizeInPlace(bn, x);
return x;
}
/* ************************************************************************* */
// (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
void optimizeInPlace(const GaussianBayesNetOrdered& bn, VectorValuesOrdered& x) {
/** solve each node in turn in topological sort order (parents first)*/
BOOST_REVERSE_FOREACH(const boost::shared_ptr<const GaussianConditionalOrdered> cg, bn) {
// i^th part of R*x=y, x=inv(R)*y
// (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
cg->solveInPlace(x);
}
}
/* ************************************************************************* */
VectorValuesOrdered backSubstitute(const GaussianBayesNetOrdered& bn, const VectorValuesOrdered& input) {
VectorValuesOrdered output = input;
BOOST_REVERSE_FOREACH(const boost::shared_ptr<const GaussianConditionalOrdered> cg, bn) {
const Index key = *(cg->beginFrontals());
Vector xS = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents());
xS = input[key] - cg->get_S() * xS;
output[key] = cg->get_R().triangularView<Eigen::Upper>().solve(xS);
}
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditionalOrdered> cg, bn) {
cg->scaleFrontalsBySigma(output);
}
return output;
}
/* ************************************************************************* */
// gy=inv(L)*gx by solving L*gy=gx.
// gy=inv(R'*inv(Sigma))*gx
// gz'*R'=gx', gy = gz.*sigmas
VectorValuesOrdered backSubstituteTranspose(const GaussianBayesNetOrdered& bn,
const VectorValuesOrdered& gx) {
// Initialize gy from gx
// TODO: used to insert zeros if gx did not have an entry for a variable in bn
VectorValuesOrdered gy = gx;
// we loop from first-eliminated to last-eliminated
// i^th part of L*gy=gx is done block-column by block-column of L
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditionalOrdered> cg, bn)
cg->solveTransposeInPlace(gy);
// Scale gy
BOOST_FOREACH(GaussianConditionalOrdered::shared_ptr cg, bn)
cg->scaleFrontalsBySigma(gy);
return gy;
}
/* ************************************************************************* */
VectorValuesOrdered optimizeGradientSearch(const GaussianBayesNetOrdered& Rd) {
gttic(Allocate_VectorValues);
VectorValuesOrdered grad = *allocateVectorValues(Rd);
gttoc(Allocate_VectorValues);
optimizeGradientSearchInPlace(Rd, grad);
return grad;
}
/* ************************************************************************* */
void optimizeGradientSearchInPlace(const GaussianBayesNetOrdered& Rd, VectorValuesOrdered& grad) {
gttic(Compute_Gradient);
// Compute gradient (call gradientAtZero function, which is defined for various linear systems)
gradientAtZero(Rd, grad);
double gradientSqNorm = grad.dot(grad);
gttoc(Compute_Gradient);
gttic(Compute_Rg);
// Compute R * g
FactorGraphOrdered<JacobianFactorOrdered> Rd_jfg(Rd);
Errors Rg = Rd_jfg * grad;
gttoc(Compute_Rg);
gttic(Compute_minimizing_step_size);
// Compute minimizing step size
double step = -gradientSqNorm / dot(Rg, Rg);
gttoc(Compute_minimizing_step_size);
gttic(Compute_point);
// Compute steepest descent point
scal(step, grad);
gttoc(Compute_point);
}
/* ************************************************************************* */
pair<Matrix,Vector> matrix(const GaussianBayesNetOrdered& bn) {
// add the dimensions of all variables to get matrix dimension
// and at the same time create a mapping from keys to indices
size_t N=0; map<Index,size_t> mapping;
BOOST_FOREACH(GaussianConditionalOrdered::shared_ptr cg,bn) {
GaussianConditionalOrdered::const_iterator it = cg->beginFrontals();
for (; it != cg->endFrontals(); ++it) {
mapping.insert(make_pair(*it,N));
N += cg->dim(it);
}
}
// create matrix and copy in values
Matrix R = zeros(N,N);
Vector d(N);
Index key; size_t I;
FOREACH_PAIR(key,I,mapping) {
// find corresponding conditional
boost::shared_ptr<const GaussianConditionalOrdered> cg = bn[key];
// get sigmas
Vector sigmas = cg->get_sigmas();
// get RHS and copy to d
GaussianConditionalOrdered::const_d_type d_ = cg->get_d();
const size_t n = d_.size();
for (size_t i=0;i<n;i++)
d(I+i) = d_(i)/sigmas(i);
// get leading R matrix and copy to R
GaussianConditionalOrdered::const_r_type R_ = cg->get_R();
for (size_t i=0;i<n;i++)
for(size_t j=0;j<n;j++)
R(I+i,I+j) = R_(i,j)/sigmas(i);
// loop over S matrices and copy them into R
GaussianConditionalOrdered::const_iterator keyS = cg->beginParents();
for (; keyS!=cg->endParents(); keyS++) {
Matrix S = cg->get_S(keyS); // get S matrix
const size_t m = S.rows(), n = S.cols(); // find S size
const size_t J = mapping[*keyS]; // find column index
for (size_t i=0;i<m;i++)
for(size_t j=0;j<n;j++)
R(I+i,J+j) = S(i,j)/sigmas(i);
} // keyS
} // keyI
return make_pair(R,d);
}
/* ************************************************************************* */
double determinant(const GaussianBayesNetOrdered& bayesNet) {
double logDet = 0.0;
BOOST_FOREACH(boost::shared_ptr<const GaussianConditionalOrdered> cg, bayesNet){
logDet += cg->get_R().diagonal().unaryExpr(ptr_fun<double,double>(log)).sum();
}
return exp(logDet);
}
/* ************************************************************************* */
VectorValuesOrdered gradient(const GaussianBayesNetOrdered& bayesNet, const VectorValuesOrdered& x0) {
return gradient(GaussianFactorGraphOrdered(bayesNet), x0);
}
/* ************************************************************************* */
void gradientAtZero(const GaussianBayesNetOrdered& bayesNet, VectorValuesOrdered& g) {
gradientAtZero(GaussianFactorGraphOrdered(bayesNet), g);
}
/* ************************************************************************* */
} // namespace gtsam

161
gtsam/linear/GaussianBayesNetOrdered.h
View File

@ -1,161 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GaussianBayesNet.h
* @brief Chordal Bayes Net, the result of eliminating a factor graph
* @brief GaussianBayesNet
* @author Frank Dellaert
*/
// \callgraph
#pragma once
#include <gtsam/global_includes.h>
#include <gtsam/linear/GaussianConditionalOrdered.h>
#include <gtsam/inference/BayesNetOrdered.h>
namespace gtsam {
/** A Bayes net made from linear-Gaussian densities */
typedef BayesNetOrdered<GaussianConditionalOrdered> GaussianBayesNetOrdered;
/** Create a scalar Gaussian */
GTSAM_EXPORT GaussianBayesNetOrdered scalarGaussian(Index key, double mu=0.0, double sigma=1.0);
/** Create a simple Gaussian on a single multivariate variable */
GTSAM_EXPORT GaussianBayesNetOrdered simpleGaussian(Index key, const Vector& mu, double sigma=1.0);
/**
* Add a conditional node with one parent
* |Rx+Sy-d|
*/
GTSAM_EXPORT void push_front(GaussianBayesNetOrdered& bn, Index key, Vector d, Matrix R,
Index name1, Matrix S, Vector sigmas);
/**
* Add a conditional node with two parents
* |Rx+Sy+Tz-d|
*/
GTSAM_EXPORT void push_front(GaussianBayesNetOrdered& bn, Index key, Vector d, Matrix R,
Index name1, Matrix S, Index name2, Matrix T, Vector sigmas);
/**
* Allocate a VectorValues for the variables in a BayesNet
*/
GTSAM_EXPORT boost::shared_ptr<VectorValuesOrdered> allocateVectorValues(const GaussianBayesNetOrdered& bn);
/**
* Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by
* back-substitution.
*/
GTSAM_EXPORT VectorValuesOrdered optimize(const GaussianBayesNetOrdered& bn);
/**
* Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by
* back-substitution, writes the solution \f$ x \f$ into a pre-allocated
* VectorValues. You can use allocateVectorValues(const GaussianBayesNet&)
* allocate it. See also optimize(const GaussianBayesNet&), which does not
* require pre-allocation.
*/
GTSAM_EXPORT void optimizeInPlace(const GaussianBayesNetOrdered& bn, VectorValuesOrdered& x);
/**
* Optimize along the gradient direction, with a closed-form computation to
* perform the line search. The gradient is computed about \f$ \delta x=0 \f$.
*
* This function returns \f$ \delta x \f$ that minimizes a reparametrized
* problem. The error function of a GaussianBayesNet is
*
* \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f]
*
* with gradient and Hessian
*
* \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f]
*
* This function performs the line search in the direction of the
* gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size
* \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$:
*
* \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f]
*
* Optimizing by setting the derivative to zero yields
* \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function
* evaluates the denominator without computing the Hessian \f$ G \f$, returning
*
* \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f]
*
* @param bn The GaussianBayesNet on which to perform this computation
* @return The resulting \f$ \delta x \f$ as described above
*/
GTSAM_EXPORT VectorValuesOrdered optimizeGradientSearch(const GaussianBayesNetOrdered& bn);
/** In-place version of optimizeGradientSearch(const GaussianBayesNet&) requiring pre-allocated VectorValues \c grad
*
* @param bn The GaussianBayesNet on which to perform this computation
* @param [out] grad The resulting \f$ \delta x \f$ as described in optimizeGradientSearch(const GaussianBayesNet&)
* */
GTSAM_EXPORT void optimizeGradientSearchInPlace(const GaussianBayesNetOrdered& bn, VectorValuesOrdered& grad);
/**
* Backsubstitute
* gy=inv(R*inv(Sigma))*gx
*/
GTSAM_EXPORT VectorValuesOrdered backSubstitute(const GaussianBayesNetOrdered& bn, const VectorValuesOrdered& gx);
/**
* Transpose Backsubstitute
* gy=inv(L)*gx by solving L*gy=gx.
* gy=inv(R'*inv(Sigma))*gx
* gz'*R'=gx', gy = gz.*sigmas
*/
GTSAM_EXPORT VectorValuesOrdered backSubstituteTranspose(const GaussianBayesNetOrdered& bn, const VectorValuesOrdered& gx);
/**
* Return (dense) upper-triangular matrix representation
*/
GTSAM_EXPORT std::pair<Matrix, Vector> matrix(const GaussianBayesNetOrdered&);
/**
* Computes the determinant of a GassianBayesNet
* A GaussianBayesNet is an upper triangular matrix and for an upper triangular matrix
* determinant is the product of the diagonal elements. Instead of actually multiplying
* we add the logarithms of the diagonal elements and take the exponent at the end
* because this is more numerically stable.
* @param bayesNet The input GaussianBayesNet
* @return The determinant
*/
GTSAM_EXPORT double determinant(const GaussianBayesNetOrdered& bayesNet);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$,
* centered around \f$ x = x_0 \f$.
* The gradient is \f$ R^T(Rx-d) \f$.
* @param bayesNet The Gaussian Bayes net $(R,d)$
* @param x0 The center about which to compute the gradient
* @return The gradient as a VectorValues
*/
GTSAM_EXPORT VectorValuesOrdered gradient(const GaussianBayesNetOrdered& bayesNet, const VectorValuesOrdered& x0);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$,
* centered around zero.
* The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&).
* @param bayesNet The Gaussian Bayes net $(R,d)$
* @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
* @return The gradient as a VectorValues
*/
GTSAM_EXPORT void gradientAtZero(const GaussianBayesNetOrdered& bayesNet, VectorValuesOrdered& g);
} /// namespace gtsam

96
gtsam/linear/GaussianBayesTreeOrdered.cpp
View File

@ -1,96 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GaussianBayesTree.cpp
* @brief Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree
* @brief GaussianBayesTree
* @author Frank Dellaert
* @author Richard Roberts
*/
#include <gtsam/linear/GaussianBayesTreeOrdered.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
namespace gtsam {
/* ************************************************************************* */
VectorValuesOrdered optimize(const GaussianBayesTreeOrdered& bayesTree) {
VectorValuesOrdered result = *allocateVectorValues(bayesTree);
optimizeInPlace(bayesTree, result);
return result;
}
/* ************************************************************************* */
void optimizeInPlace(const GaussianBayesTreeOrdered& bayesTree, VectorValuesOrdered& result) {
internal::optimizeInPlace<GaussianBayesTreeOrdered>(bayesTree.root(), result);
}
/* ************************************************************************* */
VectorValuesOrdered optimizeGradientSearch(const GaussianBayesTreeOrdered& bayesTree) {
gttic(Allocate_VectorValues);
VectorValuesOrdered grad = *allocateVectorValues(bayesTree);
gttoc(Allocate_VectorValues);
optimizeGradientSearchInPlace(bayesTree, grad);
return grad;
}
/* ************************************************************************* */
void optimizeGradientSearchInPlace(const GaussianBayesTreeOrdered& bayesTree, VectorValuesOrdered& grad) {
gttic(Compute_Gradient);
// Compute gradient (call gradientAtZero function, which is defined for various linear systems)
gradientAtZero(bayesTree, grad);
double gradientSqNorm = grad.dot(grad);
gttoc(Compute_Gradient);
gttic(Compute_Rg);
// Compute R * g
FactorGraphOrdered<JacobianFactorOrdered> Rd_jfg(bayesTree);
Errors Rg = Rd_jfg * grad;
gttoc(Compute_Rg);
gttic(Compute_minimizing_step_size);
// Compute minimizing step size
double step = -gradientSqNorm / dot(Rg, Rg);
gttoc(Compute_minimizing_step_size);
gttic(Compute_point);
// Compute steepest descent point
scal(step, grad);
gttoc(Compute_point);
}
/* ************************************************************************* */
VectorValuesOrdered gradient(const GaussianBayesTreeOrdered& bayesTree, const VectorValuesOrdered& x0) {
return gradient(FactorGraphOrdered<JacobianFactorOrdered>(bayesTree), x0);
}
/* ************************************************************************* */
void gradientAtZero(const GaussianBayesTreeOrdered& bayesTree, VectorValuesOrdered& g) {
gradientAtZero(FactorGraphOrdered<JacobianFactorOrdered>(bayesTree), g);
}
/* ************************************************************************* */
double determinant(const GaussianBayesTreeOrdered& bayesTree) {
if (!bayesTree.root())
return 0.0;
return exp(internal::logDeterminant<GaussianBayesTreeOrdered>(bayesTree.root()));
}
/* ************************************************************************* */
} // \namespace gtsam

114
gtsam/linear/GaussianBayesTreeOrdered.h
View File

@ -1,114 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GaussianBayesTree.h
* @brief Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree
* @brief GaussianBayesTree
* @author Frank Dellaert
* @author Richard Roberts
*/
#pragma once
#include <gtsam/inference/BayesTreeOrdered.h>
#include <gtsam/linear/GaussianConditionalOrdered.h>
#include <gtsam/linear/GaussianFactorOrdered.h>
namespace gtsam {
/// A Bayes Tree representing a Gaussian density
GTSAM_EXPORT typedef BayesTreeOrdered<GaussianConditionalOrdered> GaussianBayesTreeOrdered;
/// optimize the BayesTree, starting from the root
GTSAM_EXPORT VectorValuesOrdered optimize(const GaussianBayesTreeOrdered& bayesTree);
/// recursively optimize this conditional and all subtrees
GTSAM_EXPORT void optimizeInPlace(const GaussianBayesTreeOrdered& bayesTree, VectorValuesOrdered& result);
namespace internal {
template<class BAYESTREE>
void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValuesOrdered& result);
}
/**
* Optimize along the gradient direction, with a closed-form computation to
* perform the line search. The gradient is computed about \f$ \delta x=0 \f$.
*
* This function returns \f$ \delta x \f$ that minimizes a reparametrized
* problem. The error function of a GaussianBayesNet is
*
* \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f]
*
* with gradient and Hessian
*
* \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f]
*
* This function performs the line search in the direction of the
* gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size
* \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$:
*
* \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f]
*
* Optimizing by setting the derivative to zero yields
* \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$. For efficiency, this function
* evaluates the denominator without computing the Hessian \f$ G \f$, returning
*
* \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f]
*/
GTSAM_EXPORT VectorValuesOrdered optimizeGradientSearch(const GaussianBayesTreeOrdered& bayesTree);
/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */
GTSAM_EXPORT void optimizeGradientSearchInPlace(const GaussianBayesTreeOrdered& bayesTree, VectorValuesOrdered& grad);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$,
* centered around \f$ x = x_0 \f$.
* The gradient is \f$ R^T(Rx-d) \f$.
* @param bayesTree The Gaussian Bayes Tree $(R,d)$
* @param x0 The center about which to compute the gradient
* @return The gradient as a VectorValues
*/
GTSAM_EXPORT VectorValuesOrdered gradient(const GaussianBayesTreeOrdered& bayesTree, const VectorValuesOrdered& x0);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$,
* centered around zero.
* The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&).
* @param bayesTree The Gaussian Bayes Tree $(R,d)$
* @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
* @return The gradient as a VectorValues
*/
GTSAM_EXPORT void gradientAtZero(const GaussianBayesTreeOrdered& bayesTree, VectorValuesOrdered& g);
/**
* Computes the determinant of a GassianBayesTree
* A GassianBayesTree is an upper triangular matrix and for an upper triangular matrix
* determinant is the product of the diagonal elements. Instead of actually multiplying
* we add the logarithms of the diagonal elements and take the exponent at the end
* because this is more numerically stable.
* @param bayesTree The input GassianBayesTree
* @return The determinant
*/
GTSAM_EXPORT double determinant(const GaussianBayesTreeOrdered& bayesTree);
namespace internal {
template<class BAYESTREE>
double logDeterminant(const typename BAYESTREE::sharedClique& clique);
}
}
#include <gtsam/linear/GaussianBayesTreeOrdered-inl.h>

238
gtsam/linear/GaussianConditionalOrdered.cpp
View File

@ -1,238 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GaussianConditional.cpp
* @brief Conditional Gaussian Base class
* @author Christian Potthast
*/
#include <string.h>
#include <boost/format.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianConditionalOrdered.h>
#include <gtsam/linear/GaussianFactorOrdered.h>
#include <gtsam/linear/JacobianFactorOrdered.h>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
GaussianConditionalOrdered::GaussianConditionalOrdered() : rsd_(matrix_) {}
/* ************************************************************************* */
GaussianConditionalOrdered::GaussianConditionalOrdered(Index key) : IndexConditionalOrdered(key), rsd_(matrix_) {}
/* ************************************************************************* */
GaussianConditionalOrdered::GaussianConditionalOrdered(Index key,const Vector& d, const Matrix& R, const Vector& sigmas) :
IndexConditionalOrdered(key), rsd_(matrix_), sigmas_(sigmas) {
assert(R.rows() <= R.cols());
size_t dims[] = { R.cols(), 1 };
rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+2, d.size()));
rsd_(0) = R.triangularView<Eigen::Upper>();
get_d_() = d;
}
/* ************************************************************************* */
GaussianConditionalOrdered::GaussianConditionalOrdered(Index key, const Vector& d, const Matrix& R,
Index name1, const Matrix& S, const Vector& sigmas) :
IndexConditionalOrdered(key,name1), rsd_(matrix_), sigmas_(sigmas) {
assert(R.rows() <= R.cols());
size_t dims[] = { R.cols(), S.cols(), 1 };
rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+3, d.size()));
rsd_(0) = R.triangularView<Eigen::Upper>();
rsd_(1) = S;
get_d_() = d;
}
/* ************************************************************************* */
GaussianConditionalOrdered::GaussianConditionalOrdered(Index key, const Vector& d, const Matrix& R,
Index name1, const Matrix& S, Index name2, const Matrix& T, const Vector& sigmas) :
IndexConditionalOrdered(key,name1,name2), rsd_(matrix_), sigmas_(sigmas) {
assert(R.rows() <= R.cols());
size_t dims[] = { R.cols(), S.cols(), T.cols(), 1 };
rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+4, d.size()));
rsd_(0) = R.triangularView<Eigen::Upper>();
rsd_(1) = S;
rsd_(2) = T;
get_d_() = d;
}
/* ************************************************************************* */
GaussianConditionalOrdered::GaussianConditionalOrdered(Index key, const Vector& d,
const Matrix& R, const list<pair<Index, Matrix> >& parents, const Vector& sigmas) :
IndexConditionalOrdered(key, GetKeys(parents.size(), parents.begin(), parents.end())), rsd_(matrix_), sigmas_(sigmas) {
assert(R.rows() <= R.cols());
size_t* dims = (size_t*)alloca(sizeof(size_t)*(1+parents.size()+1)); // FIXME: alloca is bad, just ask Google.
dims[0] = R.cols();
size_t j=1;
std::list<std::pair<Index, Matrix> >::const_iterator parent=parents.begin();
for(; parent!=parents.end(); ++parent,++j)
dims[j] = parent->second.cols();
dims[j] = 1;
rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+1+parents.size()+1, d.size()));
rsd_(0) = R.triangularView<Eigen::Upper>();
j = 1;
for(std::list<std::pair<Index, Matrix> >::const_iterator parent=parents.begin(); parent!=parents.end(); ++parent) {
rsd_(j).noalias() = parent->second;
++ j;
}
get_d_() = d;
}
/* ************************************************************************* */
GaussianConditionalOrdered::GaussianConditionalOrdered(const std::list<std::pair<Index, Matrix> >& terms,
const size_t nrFrontals, const Vector& d, const Vector& sigmas) :
IndexConditionalOrdered(GetKeys(terms.size(), terms.begin(), terms.end()), nrFrontals),
rsd_(matrix_), sigmas_(sigmas) {
size_t* dims = (size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google.
size_t j=0;
typedef pair<Index, Matrix> Index_Matrix;
BOOST_FOREACH(const Index_Matrix& term, terms) {
dims[j] = term.second.cols();
++ j;
}
dims[j] = 1;
rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+terms.size()+1, d.size()));
j=0;
BOOST_FOREACH(const Index_Matrix& term, terms) {
rsd_(j) = term.second;
++ j;
}
get_d_() = d;
}
/* ************************************************************************* */
GaussianConditionalOrdered::GaussianConditionalOrdered(const GaussianConditionalOrdered& rhs) :
rsd_(matrix_) {
*this = rhs;
}
/* ************************************************************************* */
GaussianConditionalOrdered& GaussianConditionalOrdered::operator=(const GaussianConditionalOrdered& rhs) {
if(this != &rhs) {
this->Base::operator=(rhs); // Copy keys
rsd_.assignNoalias(rhs.rsd_); // Copy matrix and block configuration
sigmas_ = rhs.sigmas_; // Copy sigmas
}
return *this;
}
/* ************************************************************************* */
void GaussianConditionalOrdered::print(const string &s, const IndexFormatter& formatter) const
{
cout << s << ": density on ";
for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
}
cout << endl;
gtsam::print(Matrix(get_R()),"R");
for(const_iterator it = beginParents() ; it != endParents() ; ++it ) {
gtsam::print(Matrix(get_S(it)), (boost::format("A[%1%]")%(formatter(*it))).str());
}
gtsam::print(Vector(get_d()),"d");
gtsam::print(sigmas_,"sigmas");
}
/* ************************************************************************* */
bool GaussianConditionalOrdered::equals(const GaussianConditionalOrdered &c, double tol) const {
// check if the size of the parents_ map is the same
if (parents().size() != c.parents().size())
return false;
// check if R_ and d_ are linear independent
for (size_t i=0; i<rsd_.rows(); i++) {
list<Vector> rows1; rows1.push_back(Vector(get_R().row(i)));
list<Vector> rows2; rows2.push_back(Vector(c.get_R().row(i)));
// check if the matrices are the same
// iterate over the parents_ map
for (const_iterator it = beginParents(); it != endParents(); ++it) {
const_iterator it2 = c.beginParents() + (it-beginParents());
if(*it != *(it2))
return false;
rows1.push_back(row(get_S(it), i));
rows2.push_back(row(c.get_S(it2), i));
}
Vector row1 = concatVectors(rows1);
Vector row2 = concatVectors(rows2);
if (!linear_dependent(row1, row2, tol))
return false;
}
// check if sigmas are equal
if (!(equal_with_abs_tol(sigmas_, c.sigmas_, tol))) return false;
return true;
}
/* ************************************************************************* */
JacobianFactorOrdered::shared_ptr GaussianConditionalOrdered::toFactor() const {
return JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(*this));
}
/* ************************************************************************* */
void GaussianConditionalOrdered::solveInPlace(VectorValuesOrdered& x) const {
static const bool debug = false;
if(debug) this->print("Solving conditional in place");
Vector xS = internal::extractVectorValuesSlices(x, this->beginParents(), this->endParents());
xS = this->get_d() - this->get_S() * xS;
Vector soln = this->get_R().triangularView<Eigen::Upper>().solve(xS);
// Check for indeterminant solution
if(soln.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite<double>), boost::lambda::_1)).any())
throw IndeterminantLinearSystemException(this->keys().front());
if(debug) {
gtsam::print(Matrix(this->get_R()), "Calling backSubstituteUpper on ");
gtsam::print(soln, "full back-substitution solution: ");
}
internal::writeVectorValuesSlices(soln, x, this->beginFrontals(), this->endFrontals());
}
/* ************************************************************************* */
void GaussianConditionalOrdered::solveTransposeInPlace(VectorValuesOrdered& gy) const {
Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
frontalVec = gtsam::backSubstituteUpper(frontalVec,Matrix(get_R()));
// Check for indeterminant solution
if(frontalVec.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite<double>), boost::lambda::_1)).any())
throw IndeterminantLinearSystemException(this->keys().front());
GaussianConditionalOrdered::const_iterator it;
for (it = beginParents(); it!= endParents(); it++) {
const Index i = *it;
transposeMultiplyAdd(-1.0,get_S(it),frontalVec,gy[i]);
}
internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
}
/* ************************************************************************* */
void GaussianConditionalOrdered::scaleFrontalsBySigma(VectorValuesOrdered& gy) const {
Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
frontalVec = emul(frontalVec, get_sigmas());
internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
}
}

300
gtsam/linear/GaussianConditionalOrdered.h
View File

@ -1,300 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GaussianConditional.h
* @brief Conditional Gaussian Base class
* @author Christian Potthast
*/
// \callgraph
#pragma once
#include <boost/utility.hpp>
#include <gtsam/global_includes.h>
#include <gtsam/base/blockMatrices.h>
#include <gtsam/inference/IndexConditionalOrdered.h>
#include <gtsam/linear/VectorValuesOrdered.h>
// Forward declaration to friend unit tests
class JacobianFactorOrderedeliminate2Test;
class GaussianConditionalOrderedconstructorTest;
class GaussianFactorGraphOrderedeliminationTest;
class GaussianJunctionTreeOrderedcomplicatedMarginalTest;
class GaussianConditionalOrderedinformationTest;
class GaussianConditionalOrderedisGaussianFactorTest;
namespace gtsam {
// Forward declarations
class GaussianFactorOrdered;
class JacobianFactorOrdered;
/**
* A conditional Gaussian functions as the node in a Bayes network
* It has a set of parents y,z, etc. and implements a probability density on x.
* The negative log-probability is given by \f$ \frac{1}{2} |Rx - (d - Sy - Tz - ...)|^2 \f$
*/
class GTSAM_EXPORT GaussianConditionalOrdered : public IndexConditionalOrdered {
public:
typedef GaussianFactorOrdered FactorType;
typedef boost::shared_ptr<GaussianConditionalOrdered> shared_ptr;
/** Store the conditional matrix as upper-triangular column-major */
typedef Matrix RdMatrix;
typedef VerticalBlockView<RdMatrix> rsd_type;
typedef rsd_type::Block r_type;
typedef rsd_type::constBlock const_r_type;
typedef rsd_type::Column d_type;
typedef rsd_type::constColumn const_d_type;
protected:
/** Store the conditional as one big upper-triangular wide matrix, arranged
* as \f$ [ R S1 S2 ... d ] \f$. Access these blocks using a VerticalBlockView.
* */
RdMatrix matrix_;
rsd_type rsd_;
/** vector of standard deviations */
Vector sigmas_;
/** typedef to base class */
typedef IndexConditionalOrdered Base;
public:
/** default constructor needed for serialization */
GaussianConditionalOrdered();
/** constructor */
explicit GaussianConditionalOrdered(Index key);
/** constructor with no parents
* |Rx-d|
*/
GaussianConditionalOrdered(Index key, const Vector& d, const Matrix& R, const Vector& sigmas);
/** constructor with only one parent
* |Rx+Sy-d|
*/
GaussianConditionalOrdered(Index key, const Vector& d, const Matrix& R,
Index name1, const Matrix& S, const Vector& sigmas);
/** constructor with two parents
* |Rx+Sy+Tz-d|
*/
GaussianConditionalOrdered(Index key, const Vector& d, const Matrix& R,
Index name1, const Matrix& S, Index name2, const Matrix& T, const Vector& sigmas);
/**
* constructor with number of arbitrary parents (only used in unit tests,
* std::list is not efficient)
* \f$ |Rx+sum(Ai*xi)-d| \f$
*/
GaussianConditionalOrdered(Index key, const Vector& d,
const Matrix& R, const std::list<std::pair<Index, Matrix> >& parents, const Vector& sigmas);
/**
* Constructor with arbitrary number of frontals and parents (only used in unit tests,
* std::list is not efficient)
*/
GaussianConditionalOrdered(const std::list<std::pair<Index, Matrix> >& terms,
size_t nrFrontals, const Vector& d, const Vector& sigmas);
/**
* Constructor when matrices are already stored in a combined matrix, allows
* for multiple frontal variables.
*/
template<typename ITERATOR, class MATRIX>
GaussianConditionalOrdered(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals,
const VerticalBlockView<MATRIX>& matrices, const Vector& sigmas);
/** Copy constructor */
GaussianConditionalOrdered(const GaussianConditionalOrdered& rhs);
/** Combine several GaussianConditional into a single dense GC. The
* conditionals enumerated by \c first and \c last must be in increasing
* order, meaning that the parents of any conditional may not include a
* conditional coming before it.
* @param firstConditional Iterator to the first conditional to combine, must dereference to a shared_ptr<GaussianConditional>.
* @param lastConditional Iterator to after the last conditional to combine, must dereference to a shared_ptr<GaussianConditional>. */
template<typename ITERATOR>
static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional);
/** Assignment operator */
GaussianConditionalOrdered& operator=(const GaussianConditionalOrdered& rhs);
/** print */
void print(const std::string& = "GaussianConditional",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** equals function */
bool equals(const GaussianConditionalOrdered &cg, double tol = 1e-9) const;
/** dimension of multivariate variable (same as rows()) */
size_t dim() const { return rsd_.rows(); }
/** dimension of multivariate variable (same as dim()) */
size_t rows() const { return dim(); }
/** Compute the augmented information matrix as
* \f$ [ R S d ]^T [ R S d ] \f$
*/
Matrix augmentedInformation() const {
return rsd_.full().transpose() * rsd_.full().transpose();
}
/** Compute the information matrix */
Matrix information() const {
return get_R().transpose() * get_R();
}
/** Return a view of the upper-triangular R block of the conditional */
rsd_type::constBlock get_R() const { return rsd_.range(0, nrFrontals()); }
/** Return a view of the r.h.s. d vector */
const_d_type get_d() const { return rsd_.column(nrFrontals()+nrParents(), 0); }
/** get the dimension of a variable */
size_t dim(const_iterator variable) const { return rsd_(variable - this->begin()).cols(); }
/** Get a view of the parent block corresponding to the variable pointed to by the given key iterator */
rsd_type::constBlock get_S(const_iterator variable) const { return rsd_(variable - this->begin()); }
/** Get a view of the parent block corresponding to the variable pointed to by the given key iterator (non-const version) */
rsd_type::constBlock get_S() const { return rsd_.range(nrFrontals(), size()); }
/** Get the Vector of sigmas */
const Vector& get_sigmas() const {return sigmas_;}
protected:
const RdMatrix& matrix() const { return matrix_; }
const rsd_type& rsd() const { return rsd_; }
public:
/**
* Copy to a Factor (this creates a JacobianFactor and returns it as its
* base class GaussianFactor.
*/
boost::shared_ptr<JacobianFactorOrdered> toFactor() const;
/**
* Solves a conditional Gaussian and writes the solution into the entries of
* \c x for each frontal variable of the conditional. The parents are
* assumed to have already been solved in and their values are read from \c x.
* This function works for multiple frontal variables.
*
* Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2,
* where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator
* variables of this conditional, this solve function computes
* \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution.
*
* @param x VectorValues structure with solved parents \f$ x_s \f$, and into which the
* solution \f$ x_f \f$ will be written.
*/
void solveInPlace(VectorValuesOrdered& x) const;
// functions for transpose backsubstitution
/**
* Performs backsubstition in place on values
*/
void solveTransposeInPlace(VectorValuesOrdered& gy) const;
void scaleFrontalsBySigma(VectorValuesOrdered& gy) const;
protected:
rsd_type::Column get_d_() { return rsd_.column(nrFrontals()+nrParents(), 0); }
rsd_type::Block get_R_() { return rsd_.range(0, nrFrontals()); }
rsd_type::Block get_S_(iterator variable) { return rsd_(variable - this->begin()); }
private:
// Friends
friend class JacobianFactorOrdered;
friend class ::JacobianFactorOrderedeliminate2Test;
friend class ::GaussianConditionalOrderedconstructorTest;
friend class ::GaussianFactorGraphOrderedeliminationTest;
friend class ::GaussianJunctionTreeOrderedcomplicatedMarginalTest;
friend class ::GaussianConditionalOrderedinformationTest;
friend class ::GaussianConditionalOrderedisGaussianFactorTest;
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(IndexConditionalOrdered);
ar & BOOST_SERIALIZATION_NVP(matrix_);
ar & BOOST_SERIALIZATION_NVP(rsd_);
ar & BOOST_SERIALIZATION_NVP(sigmas_);
}
}; // GaussianConditional
/* ************************************************************************* */
template<typename ITERATOR, class MATRIX>
GaussianConditionalOrdered::GaussianConditionalOrdered(ITERATOR firstKey, ITERATOR lastKey,
size_t nrFrontals, const VerticalBlockView<MATRIX>& matrices,
const Vector& sigmas) :
IndexConditionalOrdered(std::vector<Index>(firstKey, lastKey), nrFrontals), rsd_(
matrix_), sigmas_(sigmas) {
rsd_.assignNoalias(matrices);
}
/* ************************************************************************* */
template<typename ITERATOR>
GaussianConditionalOrdered::shared_ptr GaussianConditionalOrdered::Combine(ITERATOR firstConditional, ITERATOR lastConditional) {
// TODO: check for being a clique
// Get dimensions from first conditional
std::vector<size_t> dims; dims.reserve((*firstConditional)->size() + 1);
for(const_iterator j = (*firstConditional)->begin(); j != (*firstConditional)->end(); ++j)
dims.push_back((*firstConditional)->dim(j));
dims.push_back(1);
// We assume the conditionals form clique, so the first n variables will be
// frontal variables in the new conditional.
size_t nFrontals = 0;
size_t nRows = 0;
for(ITERATOR c = firstConditional; c != lastConditional; ++c) {
nRows += dims[nFrontals];
++ nFrontals;
}
// Allocate combined conditional, has same keys as firstConditional
Matrix tempCombined;
VerticalBlockView<Matrix> tempBlockView(tempCombined, dims.begin(), dims.end(), 0);
GaussianConditionalOrdered::shared_ptr combinedConditional(new GaussianConditionalOrdered((*firstConditional)->begin(), (*firstConditional)->end(), nFrontals, tempBlockView, zero(nRows)));
// Resize to correct number of rows
combinedConditional->matrix_.resize(nRows, combinedConditional->matrix_.cols());
combinedConditional->rsd_.rowEnd() = combinedConditional->matrix_.rows();
// Copy matrix and sigmas
const size_t totalDims = combinedConditional->matrix_.cols();
size_t currentSlot = 0;
for(ITERATOR c = firstConditional; c != lastConditional; ++c) {
const size_t startRow = combinedConditional->rsd_.offset(currentSlot); // Start row is same as start column
combinedConditional->rsd_.range(0, currentSlot).block(startRow, 0, dims[currentSlot], combinedConditional->rsd_.offset(currentSlot)).operator=(
Matrix::Zero(dims[currentSlot], combinedConditional->rsd_.offset(currentSlot)));
combinedConditional->rsd_.range(currentSlot, dims.size()).block(startRow, 0, dims[currentSlot], totalDims - startRow).operator=(
(*c)->matrix_);
combinedConditional->sigmas_.segment(startRow, dims[currentSlot]) = (*c)->sigmas_;
++ currentSlot;
}
return combinedConditional;
}
} // gtsam

590
gtsam/linear/GaussianFactorGraphOrdered.cpp
View File

@ -1,590 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GaussianFactorGraph.cpp
* @brief Linear Factor Graph where all factors are Gaussians
* @author Kai Ni
* @author Christian Potthast
* @author Richard Roberts
* @author Frank Dellaert
*/
#include <gtsam/base/Testable.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/linear/HessianFactorOrdered.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
#include <gtsam/inference/BayesTreeOrdered.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/cholesky.h>
using namespace std;
using namespace gtsam;
namespace gtsam {
/* ************************************************************************* */
GaussianFactorGraphOrdered::GaussianFactorGraphOrdered(const GaussianBayesNetOrdered& CBN) : Base(CBN) {}
/* ************************************************************************* */
GaussianFactorGraphOrdered::Keys GaussianFactorGraphOrdered::keys() const {
FastSet<Index> keys;
BOOST_FOREACH(const sharedFactor& factor, *this)
if (factor) keys.insert(factor->begin(), factor->end());
return keys;
}
/* ************************************************************************* */
void GaussianFactorGraphOrdered::permuteWithInverse(
const Permutation& inversePermutation) {
BOOST_FOREACH(const sharedFactor& factor, factors_) {
if(factor)
factor->permuteWithInverse(inversePermutation);
}
}
/* ************************************************************************* */
void GaussianFactorGraphOrdered::reduceWithInverse(
const internal::Reduction& inverseReduction) {
BOOST_FOREACH(const sharedFactor& factor, factors_) {
if(factor)
factor->reduceWithInverse(inverseReduction);
}
}
/* ************************************************************************* */
void GaussianFactorGraphOrdered::combine(const GaussianFactorGraphOrdered &lfg) {
for (const_iterator factor = lfg.factors_.begin(); factor
!= lfg.factors_.end(); factor++) {
push_back(*factor);
}
}
/* ************************************************************************* */
GaussianFactorGraphOrdered GaussianFactorGraphOrdered::combine2(
const GaussianFactorGraphOrdered& lfg1, const GaussianFactorGraphOrdered& lfg2) {
// create new linear factor graph equal to the first one
GaussianFactorGraphOrdered fg = lfg1;
// add the second factors_ in the graph
for (const_iterator factor = lfg2.factors_.begin(); factor
!= lfg2.factors_.end(); factor++) {
fg.push_back(*factor);
}
return fg;
}
/* ************************************************************************* */
std::vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraphOrdered::sparseJacobian() const {
// First find dimensions of each variable
FastVector<size_t> dims;
BOOST_FOREACH(const sharedFactor& factor, *this) {
for(GaussianFactorOrdered::const_iterator pos = factor->begin(); pos != factor->end(); ++pos) {
if(dims.size() <= *pos)
dims.resize(*pos + 1, 0);
dims[*pos] = factor->getDim(pos);
}
}
// Compute first scalar column of each variable
vector<size_t> columnIndices(dims.size()+1, 0);
for(size_t j=1; j<dims.size()+1; ++j)
columnIndices[j] = columnIndices[j-1] + dims[j-1];
// Iterate over all factors, adding sparse scalar entries
typedef boost::tuple<size_t, size_t, double> triplet;
FastVector<triplet> entries;
size_t row = 0;
BOOST_FOREACH(const sharedFactor& factor, *this) {
// Convert to JacobianFactor if necessary
JacobianFactorOrdered::shared_ptr jacobianFactor(
boost::dynamic_pointer_cast<JacobianFactorOrdered>(factor));
if (!jacobianFactor) {
HessianFactorOrdered::shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactorOrdered>(factor));
if (hessian)
jacobianFactor.reset(new JacobianFactorOrdered(*hessian));
else
throw invalid_argument(
"GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor.");
}
// Whiten the factor and add entries for it
// iterate over all variables in the factor
const JacobianFactorOrdered whitened(jacobianFactor->whiten());
for(JacobianFactorOrdered::const_iterator pos=whitened.begin(); pos<whitened.end(); ++pos) {
JacobianFactorOrdered::constABlock whitenedA = whitened.getA(pos);
// find first column index for this key
size_t column_start = columnIndices[*pos];
for (size_t i = 0; i < (size_t) whitenedA.rows(); i++)
for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) {
double s = whitenedA(i,j);
if (std::abs(s) > 1e-12) entries.push_back(
boost::make_tuple(row+i, column_start+j, s));
}
}
JacobianFactorOrdered::constBVector whitenedb(whitened.getb());
size_t bcolumn = columnIndices.back();
for (size_t i = 0; i < (size_t) whitenedb.size(); i++)
entries.push_back(boost::make_tuple(row+i, bcolumn, whitenedb(i)));
// Increment row index
row += jacobianFactor->rows();
}
return vector<triplet>(entries.begin(), entries.end());
}
/* ************************************************************************* */
Matrix GaussianFactorGraphOrdered::sparseJacobian_() const {
// call sparseJacobian
typedef boost::tuple<size_t, size_t, double> triplet;
std::vector<triplet> result = sparseJacobian();
// translate to base 1 matrix
size_t nzmax = result.size();
Matrix IJS(3,nzmax);
for (size_t k = 0; k < result.size(); k++) {
const triplet& entry = result[k];
IJS(0,k) = double(entry.get<0>() + 1);
IJS(1,k) = double(entry.get<1>() + 1);
IJS(2,k) = entry.get<2>();
}
return IJS;
}
/* ************************************************************************* */
Matrix GaussianFactorGraphOrdered::augmentedJacobian() const {
// Convert to Jacobians
FactorGraphOrdered<JacobianFactorOrdered> jfg;
jfg.reserve(this->size());
BOOST_FOREACH(const sharedFactor& factor, *this) {
if(boost::shared_ptr<JacobianFactorOrdered> jf =
boost::dynamic_pointer_cast<JacobianFactorOrdered>(factor))
jfg.push_back(jf);
else
jfg.push_back(boost::make_shared<JacobianFactorOrdered>(*factor));
}
// combine all factors
JacobianFactorOrdered combined(*CombineJacobians(jfg, VariableSlots(*this)));
return combined.matrix_augmented();
}
/* ************************************************************************* */
std::pair<Matrix,Vector> GaussianFactorGraphOrdered::jacobian() const {
Matrix augmented = augmentedJacobian();
return make_pair(
augmented.leftCols(augmented.cols()-1),
augmented.col(augmented.cols()-1));
}
// Helper functions for Combine
static boost::tuple<vector<size_t>, size_t, size_t> countDims(const FactorGraphOrdered<JacobianFactorOrdered>& factors, const VariableSlots& variableSlots) {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
#else
vector<size_t> varDims(variableSlots.size());
#endif
size_t m = 0;
size_t n = 0;
{
Index jointVarpos = 0;
BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) {
assert(slots.second.size() == factors.size());
Index sourceFactorI = 0;
BOOST_FOREACH(const Index sourceVarpos, slots.second) {
if(sourceVarpos < numeric_limits<Index>::max()) {
const JacobianFactorOrdered& sourceFactor = *factors[sourceFactorI];
size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
if(varDims[jointVarpos] == numeric_limits<size_t>::max()) {
varDims[jointVarpos] = vardim;
n += vardim;
} else
assert(varDims[jointVarpos] == vardim);
#else
varDims[jointVarpos] = vardim;
n += vardim;
break;
#endif
}
++ sourceFactorI;
}
++ jointVarpos;
}
BOOST_FOREACH(const JacobianFactorOrdered::shared_ptr& factor, factors) {
m += factor->rows();
}
}
return boost::make_tuple(varDims, m, n);
}
/* ************************************************************************* */
JacobianFactorOrdered::shared_ptr CombineJacobians(
const FactorGraphOrdered<JacobianFactorOrdered>& factors,
const VariableSlots& variableSlots) {
const bool debug = ISDEBUG("CombineJacobians");
if (debug) factors.print("Combining factors: ");
if (debug) variableSlots.print();
if (debug) cout << "Determine dimensions" << endl;
gttic(countDims);
vector<size_t> varDims;
size_t m, n;
boost::tie(varDims, m, n) = countDims(factors, variableSlots);
if (debug) {
cout << "Dims: " << m << " x " << n << "\n";
BOOST_FOREACH(const size_t dim, varDims) cout << dim << " ";
cout << endl;
}
gttoc(countDims);
if (debug) cout << "Allocate new factor" << endl;
gttic(allocate);
JacobianFactorOrdered::shared_ptr combined(new JacobianFactorOrdered());
combined->allocate(variableSlots, varDims, m);
Vector sigmas(m);
gttoc(allocate);
if (debug) cout << "Copy blocks" << endl;
gttic(copy_blocks);
// Loop over slots in combined factor
Index combinedSlot = 0;
BOOST_FOREACH(const VariableSlots::value_type& varslot, variableSlots) {
JacobianFactorOrdered::ABlock destSlot(combined->getA(combined->begin()+combinedSlot));
// Loop over source factors
size_t nextRow = 0;
for(size_t factorI = 0; factorI < factors.size(); ++factorI) {
// Slot in source factor
const Index sourceSlot = varslot.second[factorI];
const size_t sourceRows = factors[factorI]->rows();
JacobianFactorOrdered::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows));
// Copy if exists in source factor, otherwise set zero
if(sourceSlot != numeric_limits<Index>::max())
destBlock = factors[factorI]->getA(factors[factorI]->begin()+sourceSlot);
else
destBlock.setZero();
nextRow += sourceRows;
}
++combinedSlot;
}
gttoc(copy_blocks);
if (debug) cout << "Copy rhs (b) and sigma" << endl;
gttic(copy_vectors);
bool anyConstrained = false;
// Loop over source factors
size_t nextRow = 0;
for(size_t factorI = 0; factorI < factors.size(); ++factorI) {
const size_t sourceRows = factors[factorI]->rows();
combined->getb().segment(nextRow, sourceRows) = factors[factorI]->getb();
sigmas.segment(nextRow, sourceRows) = factors[factorI]->get_model()->sigmas();
if (factors[factorI]->isConstrained()) anyConstrained = true;
nextRow += sourceRows;
}
gttoc(copy_vectors);
if (debug) cout << "Create noise model from sigmas" << endl;
gttic(noise_model);
combined->setModel(anyConstrained, sigmas);
gttoc(noise_model);
if (debug) cout << "Assert Invariants" << endl;
combined->assertInvariants();
return combined;
}
/* ************************************************************************* */
GaussianFactorGraphOrdered::EliminationResult EliminateJacobians(const FactorGraphOrdered<
JacobianFactorOrdered>& factors, size_t nrFrontals) {
gttic(Combine);
JacobianFactorOrdered::shared_ptr jointFactor =
CombineJacobians(factors, VariableSlots(factors));
gttoc(Combine);
gttic(eliminate);
GaussianConditionalOrdered::shared_ptr gbn = jointFactor->eliminate(nrFrontals);
gttoc(eliminate);
return make_pair(gbn, jointFactor);
}
/* ************************************************************************* */
Matrix GaussianFactorGraphOrdered::augmentedHessian() const {
// combine all factors and get upper-triangular part of Hessian
HessianFactorOrdered combined(*this);
Matrix result = combined.info();
// Fill in lower-triangular part of Hessian
result.triangularView<Eigen::StrictlyLower>() = result.transpose();
return result;
}
/* ************************************************************************* */
std::pair<Matrix,Vector> GaussianFactorGraphOrdered::hessian() const {
Matrix augmented = augmentedHessian();
return make_pair(
augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1),
augmented.col(augmented.rows()-1).head(augmented.rows()-1));
}
/* ************************************************************************* */
GaussianFactorGraphOrdered::EliminationResult EliminateCholeskyOrdered(const FactorGraphOrdered<
GaussianFactorOrdered>& factors, size_t nrFrontals) {
const bool debug = ISDEBUG("EliminateCholesky");
// Form Ab' * Ab
gttic(combine);
HessianFactorOrdered::shared_ptr combinedFactor(new HessianFactorOrdered(factors));
gttoc(combine);
// Do Cholesky, note that after this, the lower triangle still contains
// some untouched non-zeros that should be zero. We zero them while
// extracting submatrices next.
gttic(partial_Cholesky);
combinedFactor->partialCholesky(nrFrontals);
gttoc(partial_Cholesky);
// Extract conditional and fill in details of the remaining factor
gttic(split);
GaussianConditionalOrdered::shared_ptr conditional =
combinedFactor->splitEliminatedFactor(nrFrontals);
if (debug) {
conditional->print("Extracted conditional: ");
combinedFactor->print("Eliminated factor (L piece): ");
}
gttoc(split);
combinedFactor->assertInvariants();
return make_pair(conditional, combinedFactor);
}
/* ************************************************************************* */
static FactorGraphOrdered<JacobianFactorOrdered> convertToJacobians(const FactorGraphOrdered<
GaussianFactorOrdered>& factors) {
typedef JacobianFactorOrdered J;
typedef HessianFactorOrdered H;
const bool debug = ISDEBUG("convertToJacobians");
FactorGraphOrdered<J> jacobians;
jacobians.reserve(factors.size());
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, factors)
if (factor) {
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
if (jacobian) {
jacobians.push_back(jacobian);
if (debug) jacobian->print("Existing JacobianFactor: ");
} else {
H::shared_ptr hessian(boost::dynamic_pointer_cast<H>(factor));
if (!hessian) throw std::invalid_argument(
"convertToJacobians: factor is neither a JacobianFactor nor a HessianFactor.");
J::shared_ptr converted(new J(*hessian));
if (debug) {
cout << "Converted HessianFactor to JacobianFactor:\n";
hessian->print("HessianFactor: ");
converted->print("JacobianFactor: ");
if (!assert_equal(*hessian, HessianFactorOrdered(*converted), 1e-3)) throw runtime_error(
"convertToJacobians: Conversion between Jacobian and Hessian incorrect");
}
jacobians.push_back(converted);
}
}
return jacobians;
}
/* ************************************************************************* */
GaussianFactorGraphOrdered::EliminationResult EliminateQROrdered(const FactorGraphOrdered<
GaussianFactorOrdered>& factors, size_t nrFrontals) {
const bool debug = ISDEBUG("EliminateQR");
// Convert all factors to the appropriate type and call the type-specific EliminateGaussian.
if (debug) cout << "Using QR" << endl;
gttic(convert_to_Jacobian);
FactorGraphOrdered<JacobianFactorOrdered> jacobians = convertToJacobians(factors);
gttoc(convert_to_Jacobian);
gttic(Jacobian_EliminateGaussian);
GaussianConditionalOrdered::shared_ptr conditional;
GaussianFactorOrdered::shared_ptr factor;
boost::tie(conditional, factor) = EliminateJacobians(jacobians, nrFrontals);
gttoc(Jacobian_EliminateGaussian);
return make_pair(conditional, factor);
} // \EliminateQR
/* ************************************************************************* */
bool hasConstraints(const FactorGraphOrdered<GaussianFactorOrdered>& factors) {
typedef JacobianFactorOrdered J;
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, factors) {
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
if (jacobian && jacobian->get_model()->isConstrained()) {
return true;
}
}
return false;
}
/* ************************************************************************* */
GaussianFactorGraphOrdered::EliminationResult EliminatePreferCholeskyOrdered(
const FactorGraphOrdered<GaussianFactorOrdered>& factors, size_t nrFrontals) {
// If any JacobianFactors have constrained noise models, we have to convert
// all factors to JacobianFactors. Otherwise, we can convert all factors
// to HessianFactors. This is because QR can handle constrained noise
// models but Cholesky cannot.
if (hasConstraints(factors))
return EliminateQROrdered(factors, nrFrontals);
else {
GaussianFactorGraphOrdered::EliminationResult ret;
gttic(EliminateCholeskyOrdered);
ret = EliminateCholeskyOrdered(factors, nrFrontals);
gttoc(EliminateCholeskyOrdered);
return ret;
}
} // \EliminatePreferCholesky
/* ************************************************************************* */
static JacobianFactorOrdered::shared_ptr convertToJacobianFactorPtr(const GaussianFactorOrdered::shared_ptr &gf) {
JacobianFactorOrdered::shared_ptr result = boost::dynamic_pointer_cast<JacobianFactorOrdered>(gf);
if( !result ) {
result = boost::make_shared<JacobianFactorOrdered>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
}
return result;
}
/* ************************************************************************* */
Errors operator*(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x) {
Errors e;
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& Ai_G, fg) {
JacobianFactorOrdered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e.push_back((*Ai)*x);
}
return e;
}
/* ************************************************************************* */
void multiplyInPlace(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x, Errors& e) {
multiplyInPlace(fg,x,e.begin());
}
/* ************************************************************************* */
void multiplyInPlace(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x, const Errors::iterator& e) {
Errors::iterator ei = e;
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& Ai_G, fg) {
JacobianFactorOrdered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
*ei = (*Ai)*x;
ei++;
}
}
/* ************************************************************************* */
// x += alpha*A'*e
void transposeMultiplyAdd(const GaussianFactorGraphOrdered& fg, double alpha, const Errors& e, VectorValuesOrdered& x) {
// For each factor add the gradient contribution
Errors::const_iterator ei = e.begin();
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& Ai_G, fg) {
JacobianFactorOrdered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
Ai->transposeMultiplyAdd(alpha,*(ei++),x);
}
}
/* ************************************************************************* */
VectorValuesOrdered gradient(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x0) {
// It is crucial for performance to make a zero-valued clone of x
VectorValuesOrdered g = VectorValuesOrdered::Zero(x0);
Errors e;
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& Ai_G, fg) {
JacobianFactorOrdered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e.push_back(Ai->error_vector(x0));
}
transposeMultiplyAdd(fg, 1.0, e, g);
return g;
}
/* ************************************************************************* */
void gradientAtZero(const GaussianFactorGraphOrdered& fg, VectorValuesOrdered& g) {
// Zero-out the gradient
g.setZero();
Errors e;
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& Ai_G, fg) {
JacobianFactorOrdered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e.push_back(-Ai->getb());
}
transposeMultiplyAdd(fg, 1.0, e, g);
}
/* ************************************************************************* */
void residual(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered &x, VectorValuesOrdered &r) {
Index i = 0 ;
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& Ai_G, fg) {
JacobianFactorOrdered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
r[i] = Ai->getb();
i++;
}
VectorValuesOrdered Ax = VectorValuesOrdered::SameStructure(r);
multiply(fg,x,Ax);
axpy(-1.0,Ax,r);
}
/* ************************************************************************* */
void multiply(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered &x, VectorValuesOrdered &r) {
r.setZero();
Index i = 0;
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& Ai_G, fg) {
JacobianFactorOrdered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
Vector &y = r[i];
for(JacobianFactorOrdered::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
y += Ai->getA(j) * x[*j];
}
++i;
}
}
/* ************************************************************************* */
void transposeMultiply(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered &r, VectorValuesOrdered &x) {
x.setZero();
Index i = 0;
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& Ai_G, fg) {
JacobianFactorOrdered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
for(JacobianFactorOrdered::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
x[*j] += Ai->getA(j).transpose() * r[i];
}
++i;
}
}
/* ************************************************************************* */
boost::shared_ptr<Errors> gaussianErrors_(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x) {
boost::shared_ptr<Errors> e(new Errors);
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& Ai_G, fg) {
JacobianFactorOrdered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e->push_back(Ai->error_vector(x));
}
return e;
}
} // namespace gtsam

388
gtsam/linear/GaussianFactorGraphOrdered.h
View File

@ -1,388 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GaussianFactorGraph.h
* @brief Linear Factor Graph where all factors are Gaussians
* @author Kai Ni
* @author Christian Potthast
* @author Alireza Fathi
* @author Richard Roberts
* @author Frank Dellaert
*/
#pragma once
#include <boost/tuple/tuple.hpp>
#include <gtsam/base/FastSet.h>
#include <gtsam/linear/Errors.h>
#include <gtsam/linear/HessianFactorOrdered.h>
#include <gtsam/linear/JacobianFactorOrdered.h>
#include <gtsam/linear/GaussianBayesNetOrdered.h>
namespace gtsam {
// Forward declaration to use as default argument, documented declaration below.
GTSAM_EXPORT FactorGraphOrdered<GaussianFactorOrdered>::EliminationResult
EliminateQROrdered(const FactorGraphOrdered<GaussianFactorOrdered>& factors, size_t nrFrontals);
/**
* A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
* Factor == GaussianFactor
* VectorValues = A values structure of vectors
* Most of the time, linear factor graphs arise by linearizing a non-linear factor graph.
*/
class GaussianFactorGraphOrdered : public FactorGraphOrdered<GaussianFactorOrdered> {
public:
typedef boost::shared_ptr<GaussianFactorGraphOrdered> shared_ptr;
typedef FactorGraphOrdered<GaussianFactorOrdered> Base;
/**
* Default constructor
*/
GaussianFactorGraphOrdered() {}
/**
* Constructor that receives a Chordal Bayes Net and returns a GaussianFactorGraph
*/
GTSAM_EXPORT GaussianFactorGraphOrdered(const GaussianBayesNetOrdered& CBN);
/**
* Constructor that receives a BayesTree and returns a GaussianFactorGraph
*/
template<class CLIQUE>
GaussianFactorGraphOrdered(const BayesTreeOrdered<GaussianConditionalOrdered,CLIQUE>& gbt) : Base(gbt) {}
/** Constructor from a factor graph of GaussianFactor or a derived type */
template<class DERIVEDFACTOR>
GaussianFactorGraphOrdered(const FactorGraphOrdered<DERIVEDFACTOR>& fg) {
push_back(fg);
}
/** Add a factor by value - makes a copy */
void add(const GaussianFactorOrdered& factor) {
factors_.push_back(factor.clone());
}
/** Add a factor by pointer - stores pointer without copying the factor */
void add(const sharedFactor& factor) {
factors_.push_back(factor);
}
/** Add a null factor */
void add(const Vector& b) {
add(JacobianFactorOrdered(b));
}
/** Add a unary factor */
void add(Index key1, const Matrix& A1,
const Vector& b, const SharedDiagonal& model) {
add(JacobianFactorOrdered(key1,A1,b,model));
}
/** Add a binary factor */
void add(Index key1, const Matrix& A1,
Index key2, const Matrix& A2,
const Vector& b, const SharedDiagonal& model) {
add(JacobianFactorOrdered(key1,A1,key2,A2,b,model));
}
/** Add a ternary factor */
void add(Index key1, const Matrix& A1,
Index key2, const Matrix& A2,
Index key3, const Matrix& A3,
const Vector& b, const SharedDiagonal& model) {
add(JacobianFactorOrdered(key1,A1,key2,A2,key3,A3,b,model));
}
/** Add an n-ary factor */
void add(const std::vector<std::pair<Index, Matrix> > &terms,
const Vector &b, const SharedDiagonal& model) {
add(JacobianFactorOrdered(terms,b,model));
}
/**
* Return the set of variables involved in the factors (computes a set
* union).
*/
typedef FastSet<Index> Keys;
GTSAM_EXPORT Keys keys() const;
/** Eliminate the first \c n frontal variables, returning the resulting
* conditional and remaining factor graph - this is very inefficient for
* eliminating all variables, to do that use EliminationTree or
* JunctionTree. Note that this version simply calls
* FactorGraph<GaussianFactor>::eliminateFrontals with EliminateQR as the
* eliminate function argument.
*/
std::pair<sharedConditional, GaussianFactorGraphOrdered> eliminateFrontals(size_t nFrontals, const Eliminate& function = EliminateQROrdered) const {
return Base::eliminateFrontals(nFrontals, function); }
/** 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.
* Note that this version simply calls
* FactorGraph<GaussianFactor>::eliminate with EliminateQR as the eliminate
* function argument.
*/
std::pair<sharedConditional, GaussianFactorGraphOrdered> eliminate(const std::vector<Index>& variables, const Eliminate& function = EliminateQROrdered) const {
return Base::eliminate(variables, function); }
/** Eliminate a single variable, by calling GaussianFactorGraph::eliminate. */
std::pair<sharedConditional, GaussianFactorGraphOrdered> eliminateOne(Index variable, const Eliminate& function = EliminateQROrdered) const {
return Base::eliminateOne(variable, function); }
/** Permute the variables in the factors */
GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation);
/** Apply a reduction, which is a remapping of variable indices. */
GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction);
/** unnormalized error */
double error(const VectorValuesOrdered& x) const {
double total_error = 0.;
BOOST_FOREACH(const sharedFactor& factor, *this)
total_error += factor->error(x);
return total_error;
}
/** Unnormalized probability. O(n) */
double probPrime(const VectorValuesOrdered& c) const {
return exp(-0.5 * error(c));
}
/**
* Static function that combines two factor graphs.
* @param lfg1 Linear factor graph
* @param lfg2 Linear factor graph
* @return a new combined factor graph
*/
GTSAM_EXPORT static GaussianFactorGraphOrdered combine2(const GaussianFactorGraphOrdered& lfg1,
const GaussianFactorGraphOrdered& lfg2);
/**
* combine two factor graphs
* @param *lfg Linear factor graph
*/
GTSAM_EXPORT void combine(const GaussianFactorGraphOrdered &lfg);
/**
* Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix,
* where i(k) and j(k) are the base 0 row and column indices, s(k) a double.
* The standard deviations are baked into A and b
*/
GTSAM_EXPORT std::vector<boost::tuple<size_t, size_t, double> > sparseJacobian() const;
/**
* Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries
* such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse.
* The standard deviations are baked into A and b
*/
GTSAM_EXPORT Matrix sparseJacobian_() const;
/**
* Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$
* Jacobian matrix, augmented with b with the noise models baked
* into A and b. The negative log-likelihood is
* \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also
* GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.
*/
GTSAM_EXPORT Matrix augmentedJacobian() const;
/**
* Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$,
* with the noise models baked into A and b. The negative log-likelihood
* is \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also
* GaussianFactorGraph::augmentedJacobian and
* GaussianFactorGraph::sparseJacobian.
*/
GTSAM_EXPORT std::pair<Matrix,Vector> jacobian() const;
/**
* Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian
* matrix, augmented with the information vector \f$ \eta \f$. The
* augmented Hessian is
\f[ \left[ \begin{array}{ccc}
\Lambda & \eta \\
\eta^T & c
\end{array} \right] \f]
and the negative log-likelihood is
\f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$.
*/
GTSAM_EXPORT Matrix augmentedHessian() const;
/**
* Return the dense Hessian \f$ \Lambda \f$ and information vector
* \f$ \eta \f$, with the noise models baked in. The negative log-likelihood
* is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also
* GaussianFactorGraph::augmentedHessian.
*/
GTSAM_EXPORT std::pair<Matrix,Vector> hessian() const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
};
/**
* Combine and eliminate several factors.
* \addtogroup LinearSolving
*/
GTSAM_EXPORT JacobianFactorOrdered::shared_ptr CombineJacobians(
const FactorGraphOrdered<JacobianFactorOrdered>& factors,
const VariableSlots& variableSlots);
/**
* Evaluates whether linear factors have any constrained noise models
* @return true if any factor is constrained.
*/
GTSAM_EXPORT bool hasConstraints(const FactorGraphOrdered<GaussianFactorOrdered>& factors);
/**
* Densely combine and partially eliminate JacobianFactors to produce a
* single conditional with nrFrontals frontal variables and a remaining
* factor.
* Variables are eliminated in the natural order of the variable indices of in the factors.
* @param factors Factors to combine and eliminate
* @param nrFrontals Number of frontal variables to eliminate.
* @return The conditional and remaining factor
* \addtogroup LinearSolving
*/
GTSAM_EXPORT GaussianFactorGraphOrdered::EliminationResult EliminateJacobians(const FactorGraphOrdered<
JacobianFactorOrdered>& factors, size_t nrFrontals = 1);
/**
* Densely partially eliminate with QR factorization. HessianFactors are
* first factored with Cholesky decomposition to produce JacobianFactors,
* by calling the conversion constructor JacobianFactor(const HessianFactor&).
* Variables are eliminated in the natural order of the variable indices of in
* the factors.
* @param factors Factors to combine and eliminate
* @param nrFrontals Number of frontal variables to eliminate.
* @return The conditional and remaining factor
* \addtogroup LinearSolving
*/
GTSAM_EXPORT GaussianFactorGraphOrdered::EliminationResult EliminateQROrdered(const FactorGraphOrdered<
GaussianFactorOrdered>& factors, size_t nrFrontals = 1);
/**
* Densely partially eliminate with Cholesky factorization. JacobianFactors
* are left-multiplied with their transpose to form the Hessian using the
* conversion constructor HessianFactor(const JacobianFactor&).
*
* If any factors contain constrained noise models (any sigmas equal to
* zero), QR factorization will be performed instead, because our current
* implementation cannot handle constrained noise models in Cholesky
* factorization. EliminateCholesky(), on the other hand, will fail if any
* factors contain constrained noise models.
*
* Variables are eliminated in the natural order of the variable indices of in
* the factors.
* @param factors Factors to combine and eliminate
* @param nrFrontals Number of frontal variables to eliminate.
* @return The conditional and remaining factor
* \addtogroup LinearSolving
*/
GTSAM_EXPORT GaussianFactorGraphOrdered::EliminationResult EliminatePreferCholeskyOrdered(const FactorGraphOrdered<
GaussianFactorOrdered>& factors, size_t nrFrontals = 1);
/**
* Densely partially eliminate with Cholesky factorization. JacobianFactors
* are left-multiplied with their transpose to form the Hessian using the
* conversion constructor HessianFactor(const JacobianFactor&).
*
* If any factors contain constrained noise models, this function will fail
* because our current implementation cannot handle constrained noise models
* in Cholesky factorization. The function EliminatePreferCholesky()
* automatically does QR instead when this is the case.
*
* Variables are eliminated in the natural order of the variable indices of in
* the factors.
* @param factors Factors to combine and eliminate
* @param nrFrontals Number of frontal variables to eliminate.
* @return The conditional and remaining factor
* \addtogroup LinearSolving
*/
GTSAM_EXPORT GaussianFactorGraphOrdered::EliminationResult EliminateCholeskyOrdered(const FactorGraphOrdered<
GaussianFactorOrdered>& factors, size_t nrFrontals = 1);
/****** Linear Algebra Opeations ******/
/** return A*x */
GTSAM_EXPORT Errors operator*(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x);
/** In-place version e <- A*x that overwrites e. */
GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x, Errors& e);
/** In-place version e <- A*x that takes an iterator. */
GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x, const Errors::iterator& e);
/** x += alpha*A'*e */
GTSAM_EXPORT void transposeMultiplyAdd(const GaussianFactorGraphOrdered& fg, double alpha, const Errors& e, VectorValuesOrdered& x);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
* centered around \f$ x = x_0 \f$.
* The gradient is \f$ A^T(Ax-b) \f$.
* @param fg The Jacobian factor graph $(A,b)$
* @param x0 The center about which to compute the gradient
* @return The gradient as a VectorValues
*/
GTSAM_EXPORT VectorValuesOrdered gradient(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x0);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
* centered around zero.
* The gradient is \f$ A^T(Ax-b) \f$.
* @param fg The Jacobian factor graph $(A,b)$
* @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
* @return The gradient as a VectorValues
*/
GTSAM_EXPORT void gradientAtZero(const GaussianFactorGraphOrdered& fg, VectorValuesOrdered& g);
/* matrix-vector operations */
GTSAM_EXPORT void residual(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered &x, VectorValuesOrdered &r);
GTSAM_EXPORT void multiply(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered &x, VectorValuesOrdered &r);
GTSAM_EXPORT void transposeMultiply(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered &r, VectorValuesOrdered &x);
/** shared pointer version
* \todo Make this a member function - affects SubgraphPreconditioner */
GTSAM_EXPORT boost::shared_ptr<Errors> gaussianErrors_(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x);
/** return A*x-b
* \todo Make this a member function - affects SubgraphPreconditioner */
inline Errors gaussianErrors(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x) {
return *gaussianErrors_(fg, x); }
} // namespace gtsam

30
gtsam/linear/GaussianFactorOrdered.cpp
View File

@ -1,30 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GaussianFactor.cpp
* @brief Linear Factor....A Gaussian
* @brief linearFactor
* @author Richard Roberts, Christian Potthast
*/
#include <gtsam/linear/GaussianFactorOrdered.h>
#include <gtsam/linear/GaussianConditionalOrdered.h>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
GaussianFactorOrdered::GaussianFactorOrdered(const GaussianConditionalOrdered& c) :
IndexFactorOrdered(c) {}
}

156
gtsam/linear/GaussianFactorOrdered.h
View File

@ -1,156 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GaussianFactor.h
* @brief A factor with a quadratic error function - a Gaussian
* @brief GaussianFactor
* @author Richard Roberts, Christian Potthast
*/
// \callgraph
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/inference/IndexFactorOrdered.h>
#include <boost/lexical_cast.hpp>
#include <string>
#include <utility>
namespace gtsam {
// Forward declarations
class VectorValuesOrdered;
class Permutation;
class GaussianConditionalOrdered;
template<class C> class BayesNetOrdered;
/**
* An abstract virtual base class for JacobianFactor and HessianFactor.
* A GaussianFactor has a quadratic error function.
* GaussianFactor is non-mutable (all methods const!).
* The factor value is exp(-0.5*||Ax-b||^2)
*/
class GTSAM_EXPORT GaussianFactorOrdered: public IndexFactorOrdered {
protected:
/** Copy constructor */
GaussianFactorOrdered(const This& f) : IndexFactorOrdered(f) {}
/** Construct from derived type */
GaussianFactorOrdered(const GaussianConditionalOrdered& c);
/** Constructor from a collection of keys */
template<class KeyIterator> GaussianFactorOrdered(KeyIterator beginKey, KeyIterator endKey) :
Base(beginKey, endKey) {}
/** Default constructor for I/O */
GaussianFactorOrdered() {}
/** Construct unary factor */
GaussianFactorOrdered(Index j) : IndexFactorOrdered(j) {}
/** Construct binary factor */
GaussianFactorOrdered(Index j1, Index j2) : IndexFactorOrdered(j1, j2) {}
/** Construct ternary factor */
GaussianFactorOrdered(Index j1, Index j2, Index j3) : IndexFactorOrdered(j1, j2, j3) {}
/** Construct 4-way factor */
GaussianFactorOrdered(Index j1, Index j2, Index j3, Index j4) : IndexFactorOrdered(j1, j2, j3, j4) {}
/** Construct n-way factor */
GaussianFactorOrdered(const std::set<Index>& js) : IndexFactorOrdered(js) {}
/** Construct n-way factor */
GaussianFactorOrdered(const std::vector<Index>& js) : IndexFactorOrdered(js) {}
public:
typedef GaussianConditionalOrdered ConditionalType;
typedef boost::shared_ptr<GaussianFactorOrdered> shared_ptr;
// Implementing Testable interface
virtual void print(const std::string& s = "",
const IndexFormatter& formatter = DefaultIndexFormatter) const = 0;
virtual bool equals(const GaussianFactorOrdered& lf, double tol = 1e-9) const = 0;
virtual double error(const VectorValuesOrdered& c) const = 0; /** 0.5*(A*x-b)'*D*(A*x-b) */
/** Return the dimension of the variable pointed to by the given key iterator */
virtual size_t getDim(const_iterator variable) const = 0;
/** Return the augmented information matrix represented by this GaussianFactor.
* The augmented information matrix contains the information matrix with an
* additional column holding the information vector, and an additional row
* holding the transpose of the information vector. The lower-right entry
* contains the constant error term (when \f$ \delta x = 0 \f$). The
* augmented information matrix is described in more detail in HessianFactor,
* which in fact stores an augmented information matrix.
*/
virtual Matrix augmentedInformation() const = 0;
/** Return the non-augmented information matrix represented by this
* GaussianFactor.
*/
virtual Matrix information() const = 0;
/** Clone a factor (make a deep copy) */
virtual GaussianFactorOrdered::shared_ptr clone() const = 0;
/**
* Permutes the GaussianFactor, but for efficiency requires the permutation
* to already be inverted. This acts just as a change-of-name for each
* variable. The order of the variables within the factor is not changed.
*/
virtual void permuteWithInverse(const Permutation& inversePermutation) {
IndexFactorOrdered::permuteWithInverse(inversePermutation);
}
/**
* Apply a reduction, which is a remapping of variable indices.
*/
virtual void reduceWithInverse(const internal::Reduction& inverseReduction) {
IndexFactorOrdered::reduceWithInverse(inverseReduction);
}
/**
* Construct the corresponding anti-factor to negate information
* stored stored in this factor.
* @return a HessianFactor with negated Hessian matrices
*/
virtual GaussianFactorOrdered::shared_ptr negate() const = 0;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(IndexFactorOrdered);
}
}; // GaussianFactor
/** make keys from list, vector, or map of matrices */
template<typename ITERATOR>
static std::vector<Index> GetKeys(size_t n, ITERATOR begin, ITERATOR end) {
std::vector<Index> keys;
keys.reserve(n);
for (ITERATOR it=begin;it!=end;++it) keys.push_back(it->first);
return keys;
}
} // namespace gtsam

67
gtsam/linear/GaussianISAMOrdered.cpp
View File

@ -1,67 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GaussianISAM.cpp
* @brief Linear ISAM only
* @author Michael Kaess
*/
#include <gtsam/linear/GaussianISAMOrdered.h>
#include <gtsam/linear/GaussianBayesTreeOrdered.h>
#include <gtsam/inference/ISAMOrdered.h>
using namespace std;
using namespace gtsam;
namespace gtsam {
// Explicitly instantiate so we don't have to include everywhere
template class ISAMOrdered<GaussianConditionalOrdered>;
/* ************************************************************************* */
GaussianFactorOrdered::shared_ptr GaussianISAMOrdered::marginalFactor(Index j) const {
return Super::marginalFactor(j, &EliminateQROrdered);
}
/* ************************************************************************* */
BayesNetOrdered<GaussianConditionalOrdered>::shared_ptr GaussianISAMOrdered::marginalBayesNet(Index j) const {
return Super::marginalBayesNet(j, &EliminateQROrdered);
}
/* ************************************************************************* */
Matrix GaussianISAMOrdered::marginalCovariance(Index j) const {
GaussianConditionalOrdered::shared_ptr conditional = marginalBayesNet(j)->front();
return conditional->information().inverse();
}
/* ************************************************************************* */
BayesNetOrdered<GaussianConditionalOrdered>::shared_ptr GaussianISAMOrdered::jointBayesNet(
Index key1, Index key2) const {
return Super::jointBayesNet(key1, key2, &EliminateQROrdered);
}
/* ************************************************************************* */
VectorValuesOrdered optimize(const GaussianISAMOrdered& isam) {
VectorValuesOrdered result(isam.dims_);
// Call optimize for BayesTree
optimizeInPlace((const BayesTreeOrdered<GaussianConditionalOrdered>&)isam, result);
return result;
}
/* ************************************************************************* */
BayesNetOrdered<GaussianConditionalOrdered> GaussianISAMOrdered::shortcut(sharedClique clique, sharedClique root) {
return clique->shortcut(root,&EliminateQROrdered);
}
/* ************************************************************************* */
} // \namespace gtsam

102
gtsam/linear/GaussianISAMOrdered.h
View File

@ -1,102 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GaussianISAM.h
* @brief Linear ISAM only
* @author Michael Kaess
*/
// \callgraph
#pragma once
#include <boost/optional.hpp>
#include <gtsam/inference/ISAMOrdered.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
#include <gtsam/linear/GaussianJunctionTreeOrdered.h>
namespace gtsam {
class GaussianISAMOrdered : public ISAMOrdered<GaussianConditionalOrdered> {
typedef ISAMOrdered<GaussianConditionalOrdered> Super;
std::vector<size_t> dims_;
public:
typedef std::vector<size_t> Dims;
/** Create an empty Bayes Tree */
GaussianISAMOrdered() : Super() {}
/** Create a Bayes Tree from a Bayes Net */
// GaussianISAM(const GaussianBayesNet& bayesNet) : Super(bayesNet) {}
GaussianISAMOrdered(const BayesTreeOrdered<GaussianConditionalOrdered>& bayesTree) : Super(bayesTree) {
GaussianJunctionTreeOrdered::countDims(bayesTree, dims_);
}
/** Override update_internal to also keep track of variable dimensions. */
template<class FACTORGRAPH>
void update_internal(const FACTORGRAPH& newFactors, Cliques& orphans) {
Super::update_internal(newFactors, orphans, &EliminateQROrdered); // TODO: why does this force QR?
update_dimensions(newFactors);
}
template<class FACTORGRAPH>
void update(const FACTORGRAPH& newFactors) {
Cliques orphans;
this->update_internal(newFactors, orphans);
}
template<class FACTORGRAPH>
inline void update_dimensions(const FACTORGRAPH& newFactors) {
BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, newFactors) {
for(typename FACTORGRAPH::FactorType::const_iterator key = factor->begin(); key != factor->end(); ++key) {
if(*key >= dims_.size())
dims_.resize(*key + 1);
if(dims_[*key] == 0)
dims_[*key] = factor->getDim(key);
else
assert(dims_[*key] == factor->getDim(key));
}
}
}
void clear() {
Super::clear();
dims_.clear();
}
// access
const Dims& dims() const { return dims_; } ///< Const access to dimensions structure
Dims& dims() { return dims_; } ///< non-const access to dimensions structure (advanced interface)
GTSAM_EXPORT friend VectorValuesOrdered optimize(const GaussianISAMOrdered&);
/** return marginal on any variable as a factor, Bayes net, or mean/cov */
GTSAM_EXPORT GaussianFactorOrdered::shared_ptr marginalFactor(Index j) const;
GTSAM_EXPORT BayesNetOrdered<GaussianConditionalOrdered>::shared_ptr marginalBayesNet(Index key) const;
GTSAM_EXPORT Matrix marginalCovariance(Index key) const;
/** return joint between two variables, as a Bayes net */
GTSAM_EXPORT BayesNetOrdered<GaussianConditionalOrdered>::shared_ptr jointBayesNet(Index key1, Index key2) const;
/** return the conditional P(S|Root) on the separator given the root */
GTSAM_EXPORT static BayesNetOrdered<GaussianConditionalOrdered> shortcut(sharedClique clique, sharedClique root);
}; // \class GaussianISAM
// optimize the BayesTree, starting from the root
GTSAM_EXPORT VectorValuesOrdered optimize(const GaussianISAMOrdered& isam);
} // \namespace gtsam

59
gtsam/linear/GaussianJunctionTreeOrdered.cpp
View File

@ -1,59 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GaussianJunctionTree.cpp
* @date Jul 12, 2010
* @author Kai Ni
* @author Frank Dellaert
* @brief: the Gaussian junction tree
*/
#include <gtsam/inference/ClusterTreeOrdered.h>
#include <gtsam/inference/JunctionTreeOrdered.h>
#include <gtsam/linear/GaussianJunctionTreeOrdered.h>
#include <gtsam/linear/GaussianBayesTreeOrdered.h>
#include <vector>
#include <boost/foreach.hpp>
namespace gtsam {
// explicit template instantiation
template class JunctionTreeOrdered<GaussianFactorGraphOrdered>;
template class ClusterTreeOrdered<GaussianFactorGraphOrdered>;
using namespace std;
/* ************************************************************************* */
VectorValuesOrdered GaussianJunctionTreeOrdered::optimize(Eliminate function) const {
gttic(GJT_eliminate);
// eliminate from leaves to the root
BTClique::shared_ptr rootClique(this->eliminate(function));
gttoc(GJT_eliminate);
// Allocate solution vector and copy RHS
gttic(allocate_VectorValues);
vector<size_t> dims(rootClique->conditional()->back()+1, 0);
countDims(rootClique, dims);
VectorValuesOrdered result(dims);
gttoc(allocate_VectorValues);
// back-substitution
gttic(backsubstitute);
internal::optimizeInPlace<GaussianBayesTreeOrdered>(rootClique, result);
gttoc(backsubstitute);
return result;
}
/* ************************************************************************* */
} //namespace gtsam

88
gtsam/linear/GaussianJunctionTreeOrdered.h
View File

@ -1,88 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GaussianJunctionTree.h
* @date Jul 12, 2010
* @author Kai Ni
* @author Frank Dellaert
* @brief: the Gaussian junction tree
*/
#pragma once
#include <boost/foreach.hpp>
#include <gtsam/inference/JunctionTreeOrdered.h>
#include <gtsam/linear/GaussianBayesTreeOrdered.h>
#include <gtsam/linear/GaussianConditionalOrdered.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
namespace gtsam {
/**
* A JunctionTree where all the factors are of type GaussianFactor.
*
* In GTSAM, typically, a GaussianJunctionTree is created directly from a GaussianFactorGraph,
* after which you call optimize() to solve for the mean, or JunctionTree::eliminate() to
* create a BayesTree<GaussianConditional>. In both cases, you need to provide a basic
* GaussianFactorGraph::Eliminate function that will be used to
*
* \addtogroup Multifrontal
*/
class GaussianJunctionTreeOrdered: public JunctionTreeOrdered<GaussianFactorGraphOrdered> {
public:
typedef boost::shared_ptr<GaussianJunctionTreeOrdered> shared_ptr;
typedef JunctionTreeOrdered<GaussianFactorGraphOrdered> Base;
typedef Base::sharedClique sharedClique;
typedef GaussianFactorGraphOrdered::Eliminate Eliminate;
public :
/** Default constructor */
GaussianJunctionTreeOrdered() : Base() {}
/** Constructor from a factor graph. Builds a VariableIndex. */
GaussianJunctionTreeOrdered(const GaussianFactorGraphOrdered& fg) : Base(fg) {}
/** Construct from a factor graph and a pre-computed variable index. */
GaussianJunctionTreeOrdered(const GaussianFactorGraphOrdered& fg, const VariableIndexOrdered& variableIndex)
: Base(fg, variableIndex) {}
/**
* optimize the linear graph
*/
GTSAM_EXPORT VectorValuesOrdered optimize(Eliminate function) const;
// convenient function to return dimensions of all variables in the BayesTree<GaussianConditional>
template<class DIM_CONTAINER, class CLIQUE>
static void countDims(const BayesTreeOrdered<GaussianConditionalOrdered,CLIQUE>& bayesTree, DIM_CONTAINER& dims) {
dims = DIM_CONTAINER(bayesTree.root()->conditional()->back()+1, 0);
countDims(bayesTree.root(), dims);
}
private:
template<class DIM_CONTAINER, class CLIQUE>
static void countDims(const boost::shared_ptr<CLIQUE>& clique, DIM_CONTAINER& dims) {
GaussianConditionalOrdered::const_iterator it = clique->conditional()->beginFrontals();
for (; it != clique->conditional()->endFrontals(); ++it) {
assert(dims.at(*it) == 0);
dims.at(*it) = clique->conditional()->dim(it);
}
BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children()) {
countDims(child, dims);
}
}
}; // GaussianJunctionTree
} // namespace gtsam

92
gtsam/linear/GaussianMultifrontalSolver.cpp
View File

@ -1,92 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GaussianMultifrontalSolver.cpp
* @author Richard Roberts
* @date Oct 21, 2010
*/
#include <gtsam/linear/GaussianMultifrontalSolver.h>
namespace gtsam {
/* ************************************************************************* */
GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraphOrdered<GaussianFactorOrdered>& factorGraph, bool useQR) :
Base(factorGraph), useQR_(useQR) {}
/* ************************************************************************* */
GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraphOrdered<GaussianFactorOrdered>::shared_ptr& factorGraph,
const VariableIndexOrdered::shared_ptr& variableIndex, bool useQR) :
Base(factorGraph, variableIndex), useQR_(useQR) {}
/* ************************************************************************* */
GaussianMultifrontalSolver::shared_ptr
GaussianMultifrontalSolver::Create(const FactorGraphOrdered<GaussianFactorOrdered>::shared_ptr& factorGraph,
const VariableIndexOrdered::shared_ptr& variableIndex, bool useQR) {
return shared_ptr(new GaussianMultifrontalSolver(factorGraph, variableIndex, useQR));
}
/* ************************************************************************* */
void GaussianMultifrontalSolver::replaceFactors(const FactorGraphOrdered<GaussianFactorOrdered>::shared_ptr& factorGraph) {
Base::replaceFactors(factorGraph);
}
/* ************************************************************************* */
GaussianBayesTreeOrdered::shared_ptr GaussianMultifrontalSolver::eliminate() const {
if (useQR_)
return Base::eliminate(&EliminateQROrdered);
else
return Base::eliminate(&EliminatePreferCholeskyOrdered);
}
/* ************************************************************************* */
VectorValuesOrdered::shared_ptr GaussianMultifrontalSolver::optimize() const {
gttic(GaussianMultifrontalSolver_optimize);
VectorValuesOrdered::shared_ptr values;
if (useQR_)
values = VectorValuesOrdered::shared_ptr(new VectorValuesOrdered(junctionTree_->optimize(&EliminateQROrdered)));
else
values= VectorValuesOrdered::shared_ptr(new VectorValuesOrdered(junctionTree_->optimize(&EliminatePreferCholeskyOrdered)));
return values;
}
/* ************************************************************************* */
GaussianFactorGraphOrdered::shared_ptr GaussianMultifrontalSolver::jointFactorGraph(const std::vector<Index>& js) const {
if (useQR_)
return GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered(*Base::jointFactorGraph(js,&EliminateQROrdered)));
else
return GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered(*Base::jointFactorGraph(js,&EliminatePreferCholeskyOrdered)));
}
/* ************************************************************************* */
GaussianFactorOrdered::shared_ptr GaussianMultifrontalSolver::marginalFactor(Index j) const {
if (useQR_)
return Base::marginalFactor(j,&EliminateQROrdered);
else
return Base::marginalFactor(j,&EliminatePreferCholeskyOrdered);
}
/* ************************************************************************* */
Matrix GaussianMultifrontalSolver::marginalCovariance(Index j) const {
FactorGraphOrdered<GaussianFactorOrdered> fg;
GaussianConditionalOrdered::shared_ptr conditional;
if (useQR_) {
fg.push_back(Base::marginalFactor(j,&EliminateQROrdered));
conditional = EliminateQROrdered(fg,1).first;
} else {
fg.push_back(Base::marginalFactor(j,&EliminatePreferCholeskyOrdered));
conditional = EliminatePreferCholeskyOrdered(fg,1).first;
}
return conditional->information().inverse();
}
}

132
gtsam/linear/GaussianMultifrontalSolver.h
View File

@ -1,132 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GaussianMultifrontalSolver.h
* @author Richard Roberts
* @date Oct 21, 2010
*/
#pragma once
#include <gtsam/inference/GenericMultifrontalSolver.h>
#include <gtsam/linear/GaussianBayesTreeOrdered.h>
#include <gtsam/linear/GaussianJunctionTreeOrdered.h>
#include <gtsam/linear/VectorValuesOrdered.h>
#include <utility>
namespace gtsam {
/**
* This solver uses multifrontal elimination to solve a GaussianFactorGraph,
* i.e. a sparse linear system. Underlying this is a junction tree, which is
* eliminated into a Bayes tree.
*
* The elimination ordering is "baked in" to the variable indices at this
* stage, i.e. elimination proceeds in order from '0'. A fill-reducing
* ordering is computed symbolically from the NonlinearFactorGraph, on the
* nonlinear side of gtsam. (It is actually also possible to permute an
* existing GaussianFactorGraph into a COLAMD ordering instead, this is done
* when computing marginals).
*
* The JunctionTree recursively produces a BayesTree<GaussianConditional>,
* on which this class calls optimize(...) to perform back-substitution.
*/
class GaussianMultifrontalSolver : GenericMultifrontalSolver<GaussianFactorOrdered, GaussianJunctionTreeOrdered> {
protected:
typedef GenericMultifrontalSolver<GaussianFactorOrdered, GaussianJunctionTreeOrdered> Base;
typedef boost::shared_ptr<const GaussianMultifrontalSolver> shared_ptr;
/** flag to determine whether to use Cholesky or QR */
bool useQR_;
public:
/**
* Construct the solver for a factor graph. This builds the junction
* tree, which already does some of the work of elimination.
*/
GTSAM_EXPORT GaussianMultifrontalSolver(const FactorGraphOrdered<GaussianFactorOrdered>& factorGraph, bool useQR = false);
/**
* Construct the solver with a shared pointer to a factor graph and to a
* VariableIndex. The solver will store these pointers, so this constructor
* is the fastest.
*/
GTSAM_EXPORT GaussianMultifrontalSolver(const FactorGraphOrdered<GaussianFactorOrdered>::shared_ptr& factorGraph,
const VariableIndexOrdered::shared_ptr& variableIndex, bool useQR = false);
/**
* Named constructor to return a shared_ptr. This builds the elimination
* tree, which already does some of the symbolic work of elimination.
*/
GTSAM_EXPORT static shared_ptr Create(const FactorGraphOrdered<GaussianFactorOrdered>::shared_ptr& factorGraph,
const VariableIndexOrdered::shared_ptr& variableIndex, bool useQR = false);
/**
* Return a new solver that solves the given factor graph, which must have
* the *same structure* as the one this solver solves. For some solvers this
* is more efficient than constructing the solver from scratch. This can be
* used in cases where the numerical values of the linear problem change,
* e.g. during iterative nonlinear optimization.
*/
GTSAM_EXPORT void replaceFactors(const FactorGraphOrdered<GaussianFactorOrdered>::shared_ptr& factorGraph);
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
GTSAM_EXPORT GaussianBayesTreeOrdered::shared_ptr eliminate() const;
/**
* Compute the least-squares solution of the GaussianFactorGraph. This
* eliminates to create a BayesNet and then back-substitutes this BayesNet to
* obtain the solution.
*/
GTSAM_EXPORT VectorValuesOrdered::shared_ptr optimize() const;
/**
* Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. This function returns the result as a factor
* graph.
*
* NOTE: This function is limited to computing a joint on 2 variables
*/
GTSAM_EXPORT GaussianFactorGraphOrdered::shared_ptr jointFactorGraph(const std::vector<Index>& js) const;
/**
* Compute the marginal Gaussian density over a variable, by integrating out
* all of the other variables. This function returns the result as an upper-
* triangular R factor and right-hand-side, i.e. a GaussianConditional with
* R*x = d. To get a mean and covariance matrix, use marginalStandard(...)
*/
GTSAM_EXPORT GaussianFactorOrdered::shared_ptr marginalFactor(Index j) const;
/**
* Compute the marginal Gaussian density over a variable, by integrating out
* all of the other variables. This function returns the result as a mean
* vector and covariance matrix. Compared to marginalCanonical, which
* returns a GaussianConditional, this function back-substitutes the R factor
* to obtain the mean, then computes \f$ \Sigma = (R^T * R)^{-1} \f$.
*/
GTSAM_EXPORT Matrix marginalCovariance(Index j) const;
/** @return true if using QR */
inline bool usingQR() const { return useQR_; }
};
}

120
gtsam/linear/GaussianSequentialSolver.cpp
View File

@ -1,120 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GaussianSequentialSolver.cpp
* @author Richard Roberts
* @date Oct 19, 2010
*/
#include <gtsam/base/timing.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
namespace gtsam {
/* ************************************************************************* */
GaussianSequentialSolver::GaussianSequentialSolver(
const FactorGraphOrdered<GaussianFactorOrdered>& factorGraph, bool useQR) :
Base(factorGraph), useQR_(useQR) {
}
/* ************************************************************************* */
GaussianSequentialSolver::GaussianSequentialSolver(
const FactorGraphOrdered<GaussianFactorOrdered>::shared_ptr& factorGraph,
const VariableIndexOrdered::shared_ptr& variableIndex, bool useQR) :
Base(factorGraph, variableIndex), useQR_(useQR) {
}
/* ************************************************************************* */
GaussianSequentialSolver::shared_ptr GaussianSequentialSolver::Create(
const FactorGraphOrdered<GaussianFactorOrdered>::shared_ptr& factorGraph,
const VariableIndexOrdered::shared_ptr& variableIndex, bool useQR) {
return shared_ptr(
new GaussianSequentialSolver(factorGraph, variableIndex, useQR));
}
/* ************************************************************************* */
void GaussianSequentialSolver::replaceFactors(
const FactorGraphOrdered<GaussianFactorOrdered>::shared_ptr& factorGraph) {
Base::replaceFactors(factorGraph);
}
/* ************************************************************************* */
GaussianBayesNetOrdered::shared_ptr GaussianSequentialSolver::eliminate() const {
if (useQR_)
return Base::eliminate(&EliminateQROrdered);
else
return Base::eliminate(&EliminatePreferCholeskyOrdered);
}
/* ************************************************************************* */
VectorValuesOrdered::shared_ptr GaussianSequentialSolver::optimize() const {
static const bool debug = false;
if(debug) this->factors_->print("GaussianSequentialSolver, eliminating ");
if(debug) this->eliminationTree_->print("GaussianSequentialSolver, elimination tree ");
gttic(eliminate);
// Eliminate using the elimination tree
GaussianBayesNetOrdered::shared_ptr bayesNet(this->eliminate());
gttoc(eliminate);
if(debug) bayesNet->print("GaussianSequentialSolver, Bayes net ");
// Allocate the solution vector if it is not already allocated
// VectorValuesOrdered::shared_ptr solution = allocateVectorValues(*bayesNet);
gttic(optimize);
// Back-substitute
VectorValuesOrdered::shared_ptr solution(
new VectorValuesOrdered(gtsam::optimize(*bayesNet)));
gttoc(optimize);
if(debug) solution->print("GaussianSequentialSolver, solution ");
return solution;
}
/* ************************************************************************* */
GaussianFactorOrdered::shared_ptr GaussianSequentialSolver::marginalFactor(Index j) const {
if (useQR_)
return Base::marginalFactor(j,&EliminateQROrdered);
else
return Base::marginalFactor(j,&EliminatePreferCholeskyOrdered);
}
/* ************************************************************************* */
Matrix GaussianSequentialSolver::marginalCovariance(Index j) const {
FactorGraphOrdered<GaussianFactorOrdered> fg;
GaussianConditionalOrdered::shared_ptr conditional;
if (useQR_) {
fg.push_back(Base::marginalFactor(j, &EliminateQROrdered));
conditional = EliminateQROrdered(fg, 1).first;
} else {
fg.push_back(Base::marginalFactor(j, &EliminatePreferCholeskyOrdered));
conditional = EliminatePreferCholeskyOrdered(fg, 1).first;
}
return conditional->information().inverse();
}
/* ************************************************************************* */
GaussianFactorGraphOrdered::shared_ptr
GaussianSequentialSolver::jointFactorGraph(const std::vector<Index>& js) const {
if (useQR_)
return GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered(
*Base::jointFactorGraph(js, &EliminateQROrdered)));
else
return GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered(
*Base::jointFactorGraph(js, &EliminatePreferCholeskyOrdered)));
}
} /// namespace gtsam

133
gtsam/linear/GaussianSequentialSolver.h
View File

@ -1,133 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 GaussianSequentialSolver.h
* @brief Solves a GaussianFactorGraph (i.e. a sparse linear system) using sequential variable elimination.
* @author Richard Roberts
* @date Oct 19, 2010
*/
#pragma once
#include <gtsam/inference/GenericSequentialSolver.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
#include <gtsam/linear/VectorValuesOrdered.h>
#include <utility>
namespace gtsam {
/** This solver uses sequential variable elimination to solve a
* GaussianFactorGraph, i.e. a sparse linear system. Underlying this is a
* column elimination tree (inference/EliminationTree), see Gilbert 2001 BIT.
*
* The elimination ordering is "baked in" to the variable indices at this
* stage, i.e. elimination proceeds in order from '0'. A fill-reducing
* ordering is computed symbolically from the NonlinearFactorGraph, on the
* nonlinear side of gtsam. (To be precise, it is possible to permute an
* existing GaussianFactorGraph into a COLAMD ordering instead, this is done
* when computing marginals).
*
* This is not the most efficient algorithm we provide, most efficient is the
* MultifrontalSolver, which performs Multi-frontal QR factorization. However,
* sequential variable elimination is easier to understand so this is a good
* starting point to learn about these algorithms and our implementation.
* Additionally, the first step of MFQR is symbolic sequential elimination.
*
* The EliminationTree recursively produces a BayesNet<GaussianConditional>,
* typedef'ed in linear/GaussianBayesNet, on which this class calls
* optimize(...) to perform back-substitution.
*/
class GaussianSequentialSolver : GenericSequentialSolver<GaussianFactorOrdered> {
protected:
typedef GenericSequentialSolver<GaussianFactorOrdered> Base;
typedef boost::shared_ptr<const GaussianSequentialSolver> shared_ptr;
/** flag to determine whether to use Cholesky or QR */
bool useQR_;
public:
/**
* Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the work of elimination.
*/
GTSAM_EXPORT GaussianSequentialSolver(const FactorGraphOrdered<GaussianFactorOrdered>& factorGraph, bool useQR = false);
/**
* Construct the solver with a shared pointer to a factor graph and to a
* VariableIndex. The solver will store these pointers, so this constructor
* is the fastest.
*/
GTSAM_EXPORT GaussianSequentialSolver(const FactorGraphOrdered<GaussianFactorOrdered>::shared_ptr& factorGraph,
const VariableIndexOrdered::shared_ptr& variableIndex, bool useQR = false);
/**
* Named constructor to return a shared_ptr. This builds the elimination
* tree, which already does some of the symbolic work of elimination.
*/
GTSAM_EXPORT static shared_ptr Create(const FactorGraphOrdered<GaussianFactorOrdered>::shared_ptr& factorGraph,
const VariableIndexOrdered::shared_ptr& variableIndex, bool useQR = false);
/**
* Return a new solver that solves the given factor graph, which must have
* the *same structure* as the one this solver solves. For some solvers this
* is more efficient than constructing the solver from scratch. This can be
* used in cases where the numerical values of the linear problem change,
* e.g. during iterative nonlinear optimization.
*/
GTSAM_EXPORT void replaceFactors(const FactorGraphOrdered<GaussianFactorOrdered>::shared_ptr& factorGraph);
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
GTSAM_EXPORT GaussianBayesNetOrdered::shared_ptr eliminate() const;
/**
* Compute the least-squares solution of the GaussianFactorGraph. This
* eliminates to create a BayesNet and then back-substitutes this BayesNet to
* obtain the solution.
*/
GTSAM_EXPORT VectorValuesOrdered::shared_ptr optimize() const;
/**
* Compute the marginal Gaussian density over a variable, by integrating out
* all of the other variables. This function returns the result as an upper-
* triangular R factor and right-hand-side, i.e. a GaussianConditional with
* R*x = d. To get a mean and covariance matrix, use marginalStandard(...)
*/
GTSAM_EXPORT GaussianFactorOrdered::shared_ptr marginalFactor(Index j) const;
/**
* Compute the marginal Gaussian density over a variable, by integrating out
* all of the other variables. This function returns the result as a mean
* vector and covariance matrix. Compared to marginalCanonical, which
* returns a GaussianConditional, this function back-substitutes the R factor
* to obtain the mean, then computes \f$ \Sigma = (R^T * R)^{-1} \f$.
*/
GTSAM_EXPORT Matrix marginalCovariance(Index j) const;
/**
* Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. This function returns the result as an upper-
* triangular R factor and right-hand-side, i.e. a GaussianBayesNet with
* R*x = d. To get a mean and covariance matrix, use jointStandard(...)
*/
GTSAM_EXPORT GaussianFactorGraphOrdered::shared_ptr jointFactorGraph(const std::vector<Index>& js) const;
};
}

560
gtsam/linear/HessianFactorOrdered.cpp
View File

@ -1,560 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 HessianFactor.cpp
* @author Richard Roberts
* @date Dec 8, 2010
*/
#include <sstream>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <boost/tuple/tuple.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianConditionalOrdered.h>
#include <gtsam/linear/HessianFactorOrdered.h>
#include <gtsam/linear/JacobianFactorOrdered.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
#include <gtsam/linear/GaussianBayesNetOrdered.h>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
string SlotEntryOrdered::toString() const {
ostringstream oss;
oss << "SlotEntryOrdered: slot=" << slot << ", dim=" << dimension;
return oss.str();
}
/* ************************************************************************* */
ScatterOrdered::ScatterOrdered(const FactorGraphOrdered<GaussianFactorOrdered>& gfg) {
// First do the set union.
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, gfg) {
if(factor) {
for(GaussianFactorOrdered::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) {
this->insert(make_pair(*variable, SlotEntryOrdered(0, factor->getDim(variable))));
}
}
}
// Next fill in the slot indices (we can only get these after doing the set
// union.
size_t slot = 0;
BOOST_FOREACH(value_type& var_slot, *this) {
var_slot.second.slot = (slot ++);
}
}
/* ************************************************************************* */
void HessianFactorOrdered::assertInvariants() const {
GaussianFactorOrdered::assertInvariants(); // The base class checks for unique keys
}
/* ************************************************************************* */
HessianFactorOrdered::HessianFactorOrdered(const HessianFactorOrdered& gf) :
GaussianFactorOrdered(gf), info_(matrix_) {
info_.assignNoalias(gf.info_);
assertInvariants();
}
/* ************************************************************************* */
HessianFactorOrdered::HessianFactorOrdered() : info_(matrix_) {
// The empty HessianFactor has only a constant error term of zero
FastVector<size_t> dims;
dims.push_back(1);
info_.resize(dims.begin(), dims.end(), false);
info_(0,0)(0,0) = 0.0;
assertInvariants();
}
/* ************************************************************************* */
HessianFactorOrdered::HessianFactorOrdered(Index j, const Matrix& G, const Vector& g, double f) :
GaussianFactorOrdered(j), info_(matrix_) {
if(G.rows() != G.cols() || G.rows() != g.size())
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
size_t dims[] = { G.rows(), 1 };
InfoMatrix fullMatrix(G.rows() + 1, G.rows() + 1);
BlockInfo infoMatrix(fullMatrix, dims, dims+2);
infoMatrix(0,0) = G;
infoMatrix.column(0,1,0) = g;
infoMatrix(1,1)(0,0) = f;
infoMatrix.swap(info_);
assertInvariants();
}
/* ************************************************************************* */
// error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu)
// where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g
HessianFactorOrdered::HessianFactorOrdered(Index j, const Vector& mu, const Matrix& Sigma) :
GaussianFactorOrdered(j), info_(matrix_) {
if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument(
"Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
Matrix G = inverse(Sigma);
Vector g = G * mu;
double f = dot(mu, g);
size_t dims[] = { G.rows(), 1 };
InfoMatrix fullMatrix(G.rows() + 1, G.rows() + 1);
BlockInfo infoMatrix(fullMatrix, dims, dims + 2);
infoMatrix(0, 0) = G;
infoMatrix.column(0, 1, 0) = g;
infoMatrix(1, 1)(0, 0) = f;
infoMatrix.swap(info_);
assertInvariants();
}
/* ************************************************************************* */
HessianFactorOrdered::HessianFactorOrdered(Index j1, Index j2,
const Matrix& G11, const Matrix& G12, const Vector& g1,
const Matrix& G22, const Vector& g2, double f) :
GaussianFactorOrdered(j1, j2), info_(matrix_) {
if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != g1.size() ||
G22.cols() != G12.cols() || G22.cols() != g2.size())
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
size_t dims[] = { G11.rows(), G22.rows(), 1 };
InfoMatrix fullMatrix(G11.rows() + G22.rows() + 1, G11.rows() + G22.rows() + 1);
BlockInfo infoMatrix(fullMatrix, dims, dims+3);
infoMatrix(0,0) = G11;
infoMatrix(0,1) = G12;
infoMatrix.column(0,2,0) = g1;
infoMatrix(1,1) = G22;
infoMatrix.column(1,2,0) = g2;
infoMatrix(2,2)(0,0) = f;
infoMatrix.swap(info_);
assertInvariants();
}
/* ************************************************************************* */
HessianFactorOrdered::HessianFactorOrdered(Index j1, Index j2, Index j3,
const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1,
const Matrix& G22, const Matrix& G23, const Vector& g2,
const Matrix& G33, const Vector& g3, double f) :
GaussianFactorOrdered(j1, j2, j3), info_(matrix_) {
if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows() || G11.rows() != g1.size() ||
G22.cols() != G12.cols() || G33.cols() != G13.cols() || G22.cols() != g2.size() || G33.cols() != g3.size())
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
size_t dims[] = { G11.rows(), G22.rows(), G33.rows(), 1 };
InfoMatrix fullMatrix(G11.rows() + G22.rows() + G33.rows() + 1, G11.rows() + G22.rows() + G33.rows() + 1);
BlockInfo infoMatrix(fullMatrix, dims, dims+4);
infoMatrix(0,0) = G11;
infoMatrix(0,1) = G12;
infoMatrix(0,2) = G13;
infoMatrix.column(0,3,0) = g1;
infoMatrix(1,1) = G22;
infoMatrix(1,2) = G23;
infoMatrix.column(1,3,0) = g2;
infoMatrix(2,2) = G33;
infoMatrix.column(2,3,0) = g3;
infoMatrix(3,3)(0,0) = f;
infoMatrix.swap(info_);
assertInvariants();
}
/* ************************************************************************* */
HessianFactorOrdered::HessianFactorOrdered(const std::vector<Index>& js, const std::vector<Matrix>& Gs,
const std::vector<Vector>& gs, double f) : GaussianFactorOrdered(js), info_(matrix_) {
// Get the number of variables
size_t variable_count = js.size();
// Verify the provided number of entries in the vectors are consistent
if(gs.size() != variable_count || Gs.size() != (variable_count*(variable_count+1))/2)
throw invalid_argument("Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \
in js must match the number of linear vector pieces in gs. The number of upper-diagonal blocks in Gs must be n*(n+1)/2");
// Verify the dimensions of each provided matrix are consistent
// Note: equations for calculating the indices derived from the "sum of an arithmetic sequence" formula
for(size_t i = 0; i < variable_count; ++i){
int block_size = gs[i].size();
// Check rows
for(size_t j = 0; j < variable_count-i; ++j){
size_t index = i*(2*variable_count - i + 1)/2 + j;
if(Gs[index].rows() != block_size){
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
}
}
// Check cols
for(size_t j = 0; j <= i; ++j){
size_t index = j*(2*variable_count - j + 1)/2 + (i-j);
if(Gs[index].cols() != block_size){
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
}
}
}
// Create the dims vector
size_t* dims = (size_t*)alloca(sizeof(size_t)*(variable_count+1)); // FIXME: alloca is bad, just ask Google.
size_t total_size = 0;
for(unsigned int i = 0; i < variable_count; ++i){
dims[i] = gs[i].size();
total_size += gs[i].size();
}
dims[variable_count] = 1;
total_size += 1;
// Fill in the internal matrix with the supplied blocks
InfoMatrix fullMatrix(total_size, total_size);
BlockInfo infoMatrix(fullMatrix, dims, dims+variable_count+1);
size_t index = 0;
for(size_t i = 0; i < variable_count; ++i){
for(size_t j = i; j < variable_count; ++j){
infoMatrix(i,j) = Gs[index++];
}
infoMatrix.column(i,variable_count,0) = gs[i];
}
infoMatrix(variable_count,variable_count)(0,0) = f;
// update the BlockView variable
infoMatrix.swap(info_);
assertInvariants();
}
/* ************************************************************************* */
HessianFactorOrdered::HessianFactorOrdered(const GaussianConditionalOrdered& cg) : GaussianFactorOrdered(cg), info_(matrix_) {
JacobianFactorOrdered jf(cg);
info_.copyStructureFrom(jf.Ab_);
matrix_.noalias() = jf.matrix_.transpose() * jf.matrix_;
assertInvariants();
}
/* ************************************************************************* */
HessianFactorOrdered::HessianFactorOrdered(const GaussianFactorOrdered& gf) : info_(matrix_) {
// Copy the variable indices
(GaussianFactorOrdered&)(*this) = gf;
// Copy the matrix data depending on what type of factor we're copying from
if(dynamic_cast<const JacobianFactorOrdered*>(&gf)) {
const JacobianFactorOrdered& jf(static_cast<const JacobianFactorOrdered&>(gf));
if(jf.model_->isConstrained())
throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model");
else {
Vector invsigmas = jf.model_->invsigmas().cwiseProduct(jf.model_->invsigmas());
info_.copyStructureFrom(jf.Ab_);
BlockInfo::constBlock A = jf.Ab_.full();
matrix_.noalias() = A.transpose() * invsigmas.asDiagonal() * A;
}
} else if(dynamic_cast<const HessianFactorOrdered*>(&gf)) {
const HessianFactorOrdered& hf(static_cast<const HessianFactorOrdered&>(gf));
info_.assignNoalias(hf.info_);
} else
throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor");
assertInvariants();
}
/* ************************************************************************* */
HessianFactorOrdered::HessianFactorOrdered(const FactorGraphOrdered<GaussianFactorOrdered>& factors) : info_(matrix_)
{
ScatterOrdered scatter(factors);
// Pull out keys and dimensions
gttic(keys);
vector<size_t> dimensions(scatter.size() + 1);
BOOST_FOREACH(const ScatterOrdered::value_type& var_slot, scatter) {
dimensions[var_slot.second.slot] = var_slot.second.dimension;
}
// This is for the r.h.s. vector
dimensions.back() = 1;
gttoc(keys);
const bool debug = ISDEBUG("EliminateCholesky");
// Form Ab' * Ab
gttic(allocate);
info_.resize(dimensions.begin(), dimensions.end(), false);
// Fill in keys
keys_.resize(scatter.size());
std::transform(scatter.begin(), scatter.end(), keys_.begin(), boost::bind(&ScatterOrdered::value_type::first, ::_1));
gttoc(allocate);
gttic(zero);
matrix_.noalias() = Matrix::Zero(matrix_.rows(),matrix_.cols());
gttoc(zero);
gttic(update);
if (debug) cout << "Combining " << factors.size() << " factors" << endl;
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, factors)
{
if(factor) {
if(shared_ptr hessian = boost::dynamic_pointer_cast<HessianFactorOrdered>(factor))
updateATA(*hessian, scatter);
else if(JacobianFactorOrdered::shared_ptr jacobianFactor = boost::dynamic_pointer_cast<JacobianFactorOrdered>(factor))
updateATA(*jacobianFactor, scatter);
else
throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian");
}
}
gttoc(update);
if (debug) gtsam::print(matrix_, "Ab' * Ab: ");
assertInvariants();
}
/* ************************************************************************* */
HessianFactorOrdered& HessianFactorOrdered::operator=(const HessianFactorOrdered& rhs) {
this->Base::operator=(rhs); // Copy keys
info_.assignNoalias(rhs.info_); // Copy matrix and block structure
return *this;
}
/* ************************************************************************* */
void HessianFactorOrdered::print(const std::string& s, const IndexFormatter& formatter) const {
cout << s << "\n";
cout << " keys: ";
for(const_iterator key=this->begin(); key!=this->end(); ++key)
cout << formatter(*key) << "(" << this->getDim(key) << ") ";
cout << "\n";
gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView<Eigen::Upper>()), "Ab^T * Ab: ");
}
/* ************************************************************************* */
bool HessianFactorOrdered::equals(const GaussianFactorOrdered& lf, double tol) const {
if(!dynamic_cast<const HessianFactorOrdered*>(&lf))
return false;
else {
Matrix thisMatrix = this->info_.full().selfadjointView<Eigen::Upper>();
thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
Matrix rhsMatrix = static_cast<const HessianFactorOrdered&>(lf).info_.full().selfadjointView<Eigen::Upper>();
rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0;
return equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
}
}
/* ************************************************************************* */
Matrix HessianFactorOrdered::augmentedInformation() const {
return info_.full().selfadjointView<Eigen::Upper>();
}
/* ************************************************************************* */
Matrix HessianFactorOrdered::information() const {
return info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>();
}
/* ************************************************************************* */
double HessianFactorOrdered::error(const VectorValuesOrdered& c) const {
// error 0.5*(f - 2*x'*g + x'*G*x)
const double f = constantTerm();
double xtg = 0, xGx = 0;
// extract the relevant subset of the VectorValues
// NOTE may not be as efficient
const Vector x = c.vector(this->keys());
xtg = x.dot(linearTerm());
xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>() * x;
return 0.5 * (f - 2.0 * xtg + xGx);
}
/* ************************************************************************* */
void HessianFactorOrdered::updateATA(const HessianFactorOrdered& update, const ScatterOrdered& scatter) {
// This function updates 'combined' with the information in 'update'.
// 'scatter' maps variables in the update factor to slots in the combined
// factor.
const bool debug = ISDEBUG("updateATA");
// First build an array of slots
gttic(slots);
size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google.
size_t slot = 0;
BOOST_FOREACH(Index j, update) {
slots[slot] = scatter.find(j)->second.slot;
++ slot;
}
gttoc(slots);
if(debug) {
this->print("Updating this: ");
update.print("with (Hessian): ");
}
// Apply updates to the upper triangle
gttic(update);
for(size_t j2=0; j2<update.info_.nBlocks(); ++j2) {
size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
for(size_t j1=0; j1<=j2; ++j1) {
size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1];
if(slot2 > slot1) {
if(debug)
cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() +=
update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols());
} else if(slot1 > slot2) {
if(debug)
cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl;
matrix_.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).noalias() +=
update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols()).transpose();
} else {
if(debug)
cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).triangularView<Eigen::Upper>() +=
update.matrix_.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).rows(), update.info_(j1,j2).cols());
}
if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n";
if(debug) this->print();
}
}
gttoc(update);
}
/* ************************************************************************* */
void HessianFactorOrdered::updateATA(const JacobianFactorOrdered& update, const ScatterOrdered& scatter) {
// This function updates 'combined' with the information in 'update'.
// 'scatter' maps variables in the update factor to slots in the combined
// factor.
const bool debug = ISDEBUG("updateATA");
// First build an array of slots
gttic(slots);
size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google.
size_t slot = 0;
BOOST_FOREACH(Index j, update) {
slots[slot] = scatter.find(j)->second.slot;
++ slot;
}
gttoc(slots);
gttic(form_ATA);
if(update.model_->isConstrained())
throw invalid_argument("Cannot update HessianFactor from JacobianFactor with constrained noise model");
if(debug) {
this->print("Updating this: ");
update.print("with (Jacobian): ");
}
typedef Eigen::Block<const JacobianFactorOrdered::AbMatrix> BlockUpdateMatrix;
BlockUpdateMatrix updateA(update.matrix_.block(
update.Ab_.rowStart(),update.Ab_.offset(0), update.Ab_.full().rows(), update.Ab_.full().cols()));
if (debug) cout << "updateA: \n" << updateA << endl;
Matrix updateInform;
if(boost::dynamic_pointer_cast<noiseModel::Unit>(update.model_)) {
updateInform.noalias() = updateA.transpose() * updateA;
} else {
noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast<noiseModel::Diagonal>(update.model_));
if(diagonal) {
Vector invsigmas2 = update.model_->invsigmas().cwiseProduct(update.model_->invsigmas());
updateInform.noalias() = updateA.transpose() * invsigmas2.asDiagonal() * updateA;
} else
throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal");
}
if (debug) cout << "updateInform: \n" << updateInform << endl;
gttoc(form_ATA);
// Apply updates to the upper triangle
gttic(update);
for(size_t j2=0; j2<update.Ab_.nBlocks(); ++j2) {
size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
for(size_t j1=0; j1<=j2; ++j1) {
size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1];
size_t off0 = update.Ab_.offset(0);
if(slot2 > slot1) {
if(debug)
cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).noalias() +=
updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols());
} else if(slot1 > slot2) {
if(debug)
cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl;
matrix_.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).rows(), info_(slot2,slot1).cols()).noalias() +=
updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols()).transpose();
} else {
if(debug)
cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl;
matrix_.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).rows(), info_(slot1,slot2).cols()).triangularView<Eigen::Upper>() +=
updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).cols(), update.Ab_(j2).cols());
}
if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n";
if(debug) this->print();
}
}
gttoc(update);
}
/* ************************************************************************* */
void HessianFactorOrdered::partialCholesky(size_t nrFrontals) {
if(!choleskyPartial(matrix_, info_.offset(nrFrontals)))
throw IndeterminantLinearSystemException(this->keys().front());
}
/* ************************************************************************* */
GaussianConditionalOrdered::shared_ptr HessianFactorOrdered::splitEliminatedFactor(size_t nrFrontals) {
static const bool debug = false;
// Extract conditionals
gttic(extract_conditionals);
GaussianConditionalOrdered::shared_ptr conditional(new GaussianConditionalOrdered());
typedef VerticalBlockView<Matrix> BlockAb;
BlockAb Ab(matrix_, info_);
size_t varDim = info_.offset(nrFrontals);
Ab.rowEnd() = Ab.rowStart() + varDim;
// Create one big conditionals with many frontal variables.
gttic(construct_cond);
Vector sigmas = Vector::Ones(varDim);
conditional = boost::make_shared<ConditionalType>(keys_.begin(), keys_.end(), nrFrontals, Ab, sigmas);
gttoc(construct_cond);
if(debug) conditional->print("Extracted conditional: ");
gttoc(extract_conditionals);
// Take lower-right block of Ab_ to get the new factor
gttic(remaining_factor);
info_.blockStart() = nrFrontals;
// Assign the keys
vector<Index> remainingKeys(keys_.size() - nrFrontals);
remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end());
keys_.swap(remainingKeys);
gttoc(remaining_factor);
return conditional;
}
/* ************************************************************************* */
GaussianFactorOrdered::shared_ptr HessianFactorOrdered::negate() const {
// Copy Hessian Blocks from Hessian factor and invert
std::vector<Index> js;
std::vector<Matrix> Gs;
std::vector<Vector> gs;
double f;
js.insert(js.end(), begin(), end());
for(size_t i = 0; i < js.size(); ++i){
for(size_t j = i; j < js.size(); ++j){
Gs.push_back( -info(begin()+i, begin()+j) );
}
gs.push_back( -linearTerm(begin()+i) );
}
f = -constantTerm();
// Create the Anti-Hessian Factor from the negated blocks
return HessianFactorOrdered::shared_ptr(new HessianFactorOrdered(js, Gs, gs, f));
}
} // gtsam

377
gtsam/linear/HessianFactorOrdered.h
View File

@ -1,377 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 HessianFactor.h
* @brief Contains the HessianFactor class, a general quadratic factor
* @author Richard Roberts
* @date Dec 8, 2010
*/
#pragma once
#include <gtsam/base/blockMatrices.h>
#include <gtsam/inference/FactorGraphOrdered.h>
#include <gtsam/linear/GaussianFactorOrdered.h>
// Forward declarations for friend unit tests
class HessianFactorOrderedConversionConstructorTest;
class HessianFactorOrderedConstructor1Test;
class HessianFactorOrderedConstructor1bTest;
class HessianFactorOrderedcombineTest;
namespace gtsam {
// Forward declarations
class JacobianFactorOrdered;
class GaussianConditionalOrdered;
template<class C> class BayesNetOrdered;
// Definition of ScatterOrdered, which is an intermediate data structure used when
// building a HessianFactor incrementally, to get the keys in the right
// order.
// The "scatter" is a map from global variable indices to slot indices in the
// union of involved variables. We also include the dimensionality of the
// variable.
struct GTSAM_EXPORT SlotEntryOrdered {
size_t slot;
size_t dimension;
SlotEntryOrdered(size_t _slot, size_t _dimension)
: slot(_slot), dimension(_dimension) {}
std::string toString() const;
};
class ScatterOrdered : public FastMap<Index, SlotEntryOrdered> {
public:
ScatterOrdered() {}
ScatterOrdered(const FactorGraphOrdered<GaussianFactorOrdered>& gfg);
};
/**
* @brief A Gaussian factor using the canonical parameters (information form)
*
* HessianFactor implements a general quadratic factor of the form
* \f[ E(x) = 0.5 x^T G x - x^T g + 0.5 f \f]
* that stores the matrix \f$ G \f$, the vector \f$ g \f$, and the constant term \f$ f \f$.
*
* When \f$ G \f$ is positive semidefinite, this factor represents a Gaussian,
* in which case \f$ G \f$ is the information matrix \f$ \Lambda \f$,
* \f$ g \f$ is the information vector \f$ \eta \f$, and \f$ f \f$ is the residual
* sum-square-error at the mean, when \f$ x = \mu \f$.
*
* Indeed, the negative log-likelihood of a Gaussian is (up to a constant)
* @f$ E(x) = 0.5(x-\mu)^T P^{-1} (x-\mu) @f$
* with @f$ \mu @f$ the mean and @f$ P @f$ the covariance matrix. Expanding the product we get
* @f[
* E(x) = 0.5 x^T P^{-1} x - x^T P^{-1} \mu + 0.5 \mu^T P^{-1} \mu
* @f]
* We define the Information matrix (or Hessian) @f$ \Lambda = P^{-1} @f$
* and the information vector @f$ \eta = P^{-1} \mu = \Lambda \mu @f$
* to arrive at the canonical form of the Gaussian:
* @f[
* E(x) = 0.5 x^T \Lambda x - x^T \eta + 0.5 \mu^T \Lambda \mu
* @f]
*
* This factor is one of the factors that can be in a GaussianFactorGraph.
* It may be returned from NonlinearFactor::linearize(), but is also
* used internally to store the Hessian during Cholesky elimination.
*
* This can represent a quadratic factor with characteristics that cannot be
* represented using a JacobianFactor (which has the form
* \f$ E(x) = \Vert Ax - b \Vert^2 \f$ and stores the Jacobian \f$ A \f$
* and error vector \f$ b \f$, i.e. is a sum-of-squares factor). For example,
* a HessianFactor need not be positive semidefinite, it can be indefinite or
* even negative semidefinite.
*
* If a HessianFactor is indefinite or negative semi-definite, then in order
* for solving the linear system to be possible,
* the Hessian of the full system must be positive definite (i.e. when all
* small Hessians are combined, the result must be positive definite). If
* this is not the case, an error will occur during elimination.
*
* This class stores G, g, and f as an augmented matrix HessianFactor::matrix_.
* The upper-left n x n blocks of HessianFactor::matrix_ store the upper-right
* triangle of G, the upper-right-most column of length n of HessianFactor::matrix_
* stores g, and the lower-right entry of HessianFactor::matrix_ stores f, i.e.
* \code
HessianFactor::matrix_ = [ G11 G12 G13 ... g1
0 G22 G23 ... g2
0 0 G33 ... g3
: : : :
0 0 0 ... f ]
\endcode
Blocks can be accessed as follows:
\code
G11 = info(begin(), begin());
G12 = info(begin(), begin()+1);
G23 = info(begin()+1, begin()+2);
g2 = linearTerm(begin()+1);
f = constantTerm();
.......
\endcode
*/
class GTSAM_EXPORT HessianFactorOrdered : public GaussianFactorOrdered {
protected:
typedef Matrix InfoMatrix; ///< The full augmented Hessian
typedef SymmetricBlockView<InfoMatrix> BlockInfo; ///< A blockwise view of the Hessian
typedef GaussianFactorOrdered Base; ///< Typedef to base class
InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1]
BlockInfo info_; ///< The block view of the full information matrix.
public:
typedef boost::shared_ptr<HessianFactorOrdered> shared_ptr; ///< A shared_ptr to this
typedef BlockInfo::Block Block; ///< A block from the Hessian matrix
typedef BlockInfo::constBlock constBlock; ///< A block from the Hessian matrix (const version)
typedef BlockInfo::Column Column; ///< A column containing the linear term h
typedef BlockInfo::constColumn constColumn; ///< A column containing the linear term h (const version)
/** Copy constructor */
HessianFactorOrdered(const HessianFactorOrdered& gf);
/** default constructor for I/O */
HessianFactorOrdered();
/** Construct a unary factor. G is the quadratic term (Hessian matrix), g
* the linear term (a vector), and f the constant term. The quadratic
* error is:
* 0.5*(f - 2*x'*g + x'*G*x)
*/
HessianFactorOrdered(Index j, const Matrix& G, const Vector& g, double f);
/** Construct a unary factor, given a mean and covariance matrix.
* error is 0.5*(x-mu)'*inv(Sigma)*(x-mu)
*/
HessianFactorOrdered(Index j, const Vector& mu, const Matrix& Sigma);
/** Construct a binary factor. Gxx are the upper-triangle blocks of the
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
* term, and f the constant term.
* JacobianFactor error is \f[ 0.5* (Ax-b)' M (Ax-b) = 0.5*x'A'MAx - x'A'Mb + 0.5*b'Mb \f]
* HessianFactor error is \f[ 0.5*(x'Gx - 2x'g + f) = 0.5*x'Gx - x'*g + 0.5*f \f]
* So, with \f$ A = [A1 A2] \f$ and \f$ G=A*'M*A = [A1';A2']*M*[A1 A2] \f$ we have
\code
n1*n1 G11 = A1'*M*A1
n1*n2 G12 = A1'*M*A2
n2*n2 G22 = A2'*M*A2
n1*1 g1 = A1'*M*b
n2*1 g2 = A2'*M*b
1*1 f = b'*M*b
\endcode
*/
HessianFactorOrdered(Index j1, Index j2,
const Matrix& G11, const Matrix& G12, const Vector& g1,
const Matrix& G22, const Vector& g2, double f);
/** Construct a ternary factor. Gxx are the upper-triangle blocks of the
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
* term, and f the constant term.
*/
HessianFactorOrdered(Index j1, Index j2, Index j3,
const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1,
const Matrix& G22, const Matrix& G23, const Vector& g2,
const Matrix& G33, const Vector& g3, double f);
/** Construct an n-way factor. Gs contains the upper-triangle blocks of the
* quadratic term (the Hessian matrix) provided in row-order, gs the pieces
* of the linear vector term, and f the constant term.
*/
HessianFactorOrdered(const std::vector<Index>& js, const std::vector<Matrix>& Gs,
const std::vector<Vector>& gs, double f);
/** Construct from Conditional Gaussian */
explicit HessianFactorOrdered(const GaussianConditionalOrdered& cg);
/** Convert from a JacobianFactor (computes A^T * A) or HessianFactor */
explicit HessianFactorOrdered(const GaussianFactorOrdered& factor);
/** Combine a set of factors into a single dense HessianFactor */
HessianFactorOrdered(const FactorGraphOrdered<GaussianFactorOrdered>& factors);
/** Destructor */
virtual ~HessianFactorOrdered() {}
/** Aassignment operator */
HessianFactorOrdered& operator=(const HessianFactorOrdered& rhs);
/** Clone this JacobianFactor */
virtual GaussianFactorOrdered::shared_ptr clone() const {
return boost::static_pointer_cast<GaussianFactorOrdered>(
shared_ptr(new HessianFactorOrdered(*this)));
}
/** Print the factor for debugging and testing (implementing Testable) */
virtual void print(const std::string& s = "",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** Compare to another factor for testing (implementing Testable) */
virtual bool equals(const GaussianFactorOrdered& lf, double tol = 1e-9) const;
/** Evaluate the factor error f(x), see above. */
virtual double error(const VectorValuesOrdered& c) const; /** 0.5*[x -1]'*H*[x -1] (also see constructor documentation) */
/** Return the dimension of the variable pointed to by the given key iterator
* todo: Remove this in favor of keeping track of dimensions with variables?
* @param variable An iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
*/
virtual size_t getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).rows(); }
/** Return the number of columns and rows of the Hessian matrix */
size_t rows() const { return info_.rows(); }
/**
* Construct the corresponding anti-factor to negate information
* stored stored in this factor.
* @return a HessianFactor with negated Hessian matrices
*/
virtual GaussianFactorOrdered::shared_ptr negate() const;
/** Return a view of the block at (j1,j2) of the <em>upper-triangular part</em> of the
* information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation
* above to explain that only the upper-triangular part of the information matrix is stored
* and returned by this function.
* @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @return A view of the requested block, not a copy.
*/
constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); }
/** Return a view of the block at (j1,j2) of the <em>upper-triangular part</em> of the
* information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation
* above to explain that only the upper-triangular part of the information matrix is stored
* and returned by this function.
* @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @return A view of the requested block, not a copy.
*/
Block info(iterator j1, iterator j2) { return info_(j1-begin(), j2-begin()); }
/** Return the <em>upper-triangular part</em> of the full *augmented* information matrix,
* as described above. See HessianFactor class documentation above to explain that only the
* upper-triangular part of the information matrix is stored and returned by this function.
*/
constBlock info() const { return info_.full(); }
/** Return the <em>upper-triangular part</em> of the full *augmented* information matrix,
* as described above. See HessianFactor class documentation above to explain that only the
* upper-triangular part of the information matrix is stored and returned by this function.
*/
Block info() { return info_.full(); }
/** Return the constant term \f$ f \f$ as described above
* @return The constant term \f$ f \f$
*/
double constantTerm() const { return info_(this->size(), this->size())(0,0); }
/** Return the constant term \f$ f \f$ as described above
* @return The constant term \f$ f \f$
*/
double& constantTerm() { return info_(this->size(), this->size())(0,0); }
/** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable.
* @param j Which block row to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @return The linear term \f$ g \f$ */
constColumn linearTerm(const_iterator j) const { return info_.column(j-begin(), size(), 0); }
/** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable.
* @param j Which block row to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @return The linear term \f$ g \f$ */
Column linearTerm(iterator j) { return info_.column(j-begin(), size(), 0); }
/** Return the complete linear term \f$ g \f$ as described above.
* @return The linear term \f$ g \f$ */
constColumn linearTerm() const { return info_.rangeColumn(0, this->size(), this->size(), 0); }
/** Return the complete linear term \f$ g \f$ as described above.
* @return The linear term \f$ g \f$ */
Column linearTerm() { return info_.rangeColumn(0, this->size(), this->size(), 0); }
/** Return the augmented information matrix represented by this GaussianFactor.
* The augmented information matrix contains the information matrix with an
* additional column holding the information vector, and an additional row
* holding the transpose of the information vector. The lower-right entry
* contains the constant error term (when \f$ \delta x = 0 \f$). The
* augmented information matrix is described in more detail in HessianFactor,
* which in fact stores an augmented information matrix.
*
* For HessianFactor, this is the same as info() except that this function
* returns a complete symmetric matrix whereas info() returns a matrix where
* only the upper triangle is valid, but should be interpreted as symmetric.
* This is because info() returns only a reference to the internal
* representation of the augmented information matrix, which stores only the
* upper triangle.
*/
virtual Matrix augmentedInformation() const;
/** Return the non-augmented information matrix represented by this
* GaussianFactor.
*/
virtual Matrix information() const;
// Friend unit test classes
friend class ::HessianFactorOrderedConversionConstructorTest;
friend class ::HessianFactorOrderedConstructor1Test;
friend class ::HessianFactorOrderedConstructor1bTest;
friend class ::HessianFactorOrderedcombineTest;
// Friend JacobianFactor for conversion
friend class JacobianFactorOrdered;
// used in eliminateCholesky:
/**
* Do Cholesky. Note that after this, the lower triangle still contains
* some untouched non-zeros that should be zero. We zero them while
* extracting submatrices in splitEliminatedFactor. Frank says :-(
*/
void partialCholesky(size_t nrFrontals);
/** split partially eliminated factor */
boost::shared_ptr<GaussianConditionalOrdered> splitEliminatedFactor(size_t nrFrontals);
/** Update the factor by adding the information from the JacobianFactor
* (used internally during elimination).
* @param update The JacobianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const JacobianFactorOrdered& update, const ScatterOrdered& scatter);
/** Update the factor by adding the information from the HessianFactor
* (used internally during elimination).
* @param update The HessianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const HessianFactorOrdered& update, const ScatterOrdered& scatter);
/** assert invariants */
void assertInvariants() const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactorOrdered);
ar & BOOST_SERIALIZATION_NVP(info_);
ar & BOOST_SERIALIZATION_NVP(matrix_);
}
};
}

533
gtsam/linear/JacobianFactorOrdered.cpp
View File

@ -1,533 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 JacobianFactor.cpp
* @author Richard Roberts
* @date Dec 8, 2010
*/
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianConditionalOrdered.h>
#include <gtsam/linear/JacobianFactorOrdered.h>
#include <gtsam/linear/HessianFactorOrdered.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/cholesky.h>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <cmath>
#include <sstream>
#include <stdexcept>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
void JacobianFactorOrdered::assertInvariants() const {
#ifndef NDEBUG
GaussianFactorOrdered::assertInvariants(); // The base class checks for unique keys
assert((size() == 0 && Ab_.rows() == 0 && Ab_.nBlocks() == 0) || size()+1 == Ab_.nBlocks());
#endif
}
/* ************************************************************************* */
JacobianFactorOrdered::JacobianFactorOrdered(const JacobianFactorOrdered& gf) :
GaussianFactorOrdered(gf), model_(gf.model_), Ab_(matrix_) {
Ab_.assignNoalias(gf.Ab_);
assertInvariants();
}
/* ************************************************************************* */
JacobianFactorOrdered::JacobianFactorOrdered(const GaussianFactorOrdered& gf) : Ab_(matrix_) {
// Copy the matrix data depending on what type of factor we're copying from
if(const JacobianFactorOrdered* rhs = dynamic_cast<const JacobianFactorOrdered*>(&gf))
*this = JacobianFactorOrdered(*rhs);
else if(const HessianFactorOrdered* rhs = dynamic_cast<const HessianFactorOrdered*>(&gf))
*this = JacobianFactorOrdered(*rhs);
else
throw std::invalid_argument("In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
assertInvariants();
}
/* ************************************************************************* */
JacobianFactorOrdered::JacobianFactorOrdered() : Ab_(matrix_) { assertInvariants(); }
/* ************************************************************************* */
JacobianFactorOrdered::JacobianFactorOrdered(const Vector& b_in) : Ab_(matrix_) {
size_t dims[] = { 1 };
Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+1, b_in.size()));
getb() = b_in;
model_ = noiseModel::Unit::Create(this->rows());
assertInvariants();
}
/* ************************************************************************* */
JacobianFactorOrdered::JacobianFactorOrdered(Index i1, const Matrix& A1,
const Vector& b, const SharedDiagonal& model) :
GaussianFactorOrdered(i1), model_(model), Ab_(matrix_) {
if(model->dim() != (size_t) b.size())
throw InvalidNoiseModel(b.size(), model->dim());
size_t dims[] = { A1.cols(), 1};
Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+2, b.size()));
Ab_(0) = A1;
getb() = b;
assertInvariants();
}
/* ************************************************************************* */
JacobianFactorOrdered::JacobianFactorOrdered(Index i1, const Matrix& A1, Index i2, const Matrix& A2,
const Vector& b, const SharedDiagonal& model) :
GaussianFactorOrdered(i1,i2), model_(model), Ab_(matrix_) {
if(model->dim() != (size_t) b.size())
throw InvalidNoiseModel(b.size(), model->dim());
size_t dims[] = { A1.cols(), A2.cols(), 1};
Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+3, b.size()));
Ab_(0) = A1;
Ab_(1) = A2;
getb() = b;
assertInvariants();
}
/* ************************************************************************* */
JacobianFactorOrdered::JacobianFactorOrdered(Index i1, const Matrix& A1, Index i2, const Matrix& A2,
Index i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) :
GaussianFactorOrdered(i1,i2,i3), model_(model), Ab_(matrix_) {
if(model->dim() != (size_t) b.size())
throw InvalidNoiseModel(b.size(), model->dim());
size_t dims[] = { A1.cols(), A2.cols(), A3.cols(), 1};
Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+4, b.size()));
Ab_(0) = A1;
Ab_(1) = A2;
Ab_(2) = A3;
getb() = b;
assertInvariants();
}
/* ************************************************************************* */
JacobianFactorOrdered::JacobianFactorOrdered(const std::vector<std::pair<Index, Matrix> > &terms,
const Vector &b, const SharedDiagonal& model) :
GaussianFactorOrdered(GetKeys(terms.size(), terms.begin(), terms.end())),
model_(model), Ab_(matrix_)
{
// get number of measurements and variables involved in this factor
size_t m = b.size(), n = terms.size();
if(model->dim() != (size_t) m)
throw InvalidNoiseModel(b.size(), model->dim());
// Get the dimensions of each variable and copy to "dims" array, add 1 for RHS
size_t* dims = (size_t*)alloca(sizeof(size_t)*(n+1)); // FIXME: alloca is bad, just ask Google.
for(size_t j=0; j<n; ++j)
dims[j] = terms[j].second.cols();
dims[n] = 1;
// Frank is mystified why this is done this way, rather than just creating Ab_
Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+terms.size()+1, b.size()));
// Now copy the Jacobian matrices from the terms matrix
for(size_t j=0; j<n; ++j) {
assert(terms[j].second.rows()==m);
Ab_(j) = terms[j].second;
}
getb() = b;
assertInvariants();
}
/* ************************************************************************* */
JacobianFactorOrdered::JacobianFactorOrdered(const std::list<std::pair<Index, Matrix> > &terms,
const Vector &b, const SharedDiagonal& model) :
GaussianFactorOrdered(GetKeys(terms.size(), terms.begin(), terms.end())),
model_(model), Ab_(matrix_)
{
if(model->dim() != (size_t) b.size())
throw InvalidNoiseModel(b.size(), model->dim());
size_t* dims=(size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google.
size_t j=0;
std::list<std::pair<Index, Matrix> >::const_iterator term=terms.begin();
for(; term!=terms.end(); ++term,++j)
dims[j] = term->second.cols();
dims[j] = 1;
Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+terms.size()+1, b.size()));
j = 0;
for(term=terms.begin(); term!=terms.end(); ++term,++j)
Ab_(j) = term->second;
getb() = b;
assertInvariants();
}
/* ************************************************************************* */
JacobianFactorOrdered::JacobianFactorOrdered(const GaussianConditionalOrdered& cg) :
GaussianFactorOrdered(cg),
model_(noiseModel::Diagonal::Sigmas(cg.get_sigmas(), true)),
Ab_(matrix_) {
Ab_.assignNoalias(cg.rsd_);
assertInvariants();
}
/* ************************************************************************* */
JacobianFactorOrdered::JacobianFactorOrdered(const HessianFactorOrdered& factor) : Ab_(matrix_) {
keys_ = factor.keys_;
Ab_.assignNoalias(factor.info_);
// Do Cholesky to get a Jacobian
size_t maxrank;
bool success;
boost::tie(maxrank, success) = choleskyCareful(matrix_);
// Check for indefinite system
if(!success)
throw IndeterminantLinearSystemException(factor.keys().front());
// Zero out lower triangle
matrix_.topRows(maxrank).triangularView<Eigen::StrictlyLower>() =
Matrix::Zero(maxrank, matrix_.cols());
// FIXME: replace with triangular system
Ab_.rowEnd() = maxrank;
model_ = noiseModel::Unit::Create(maxrank);
assertInvariants();
}
/* ************************************************************************* */
JacobianFactorOrdered::JacobianFactorOrdered(const GaussianFactorGraphOrdered& gfg) : Ab_(matrix_) {
// Cast or convert to Jacobians
FactorGraphOrdered<JacobianFactorOrdered> jacobians;
BOOST_FOREACH(const GaussianFactorGraphOrdered::sharedFactor& factor, gfg) {
if(factor) {
if(JacobianFactorOrdered::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactorOrdered>(factor))
jacobians.push_back(jf);
else
jacobians.push_back(boost::make_shared<JacobianFactorOrdered>(*factor));
}
}
*this = *CombineJacobians(jacobians, VariableSlots(jacobians));
}
/* ************************************************************************* */
JacobianFactorOrdered& JacobianFactorOrdered::operator=(const JacobianFactorOrdered& rhs) {
this->Base::operator=(rhs); // Copy keys
model_ = rhs.model_; // Copy noise model
Ab_.assignNoalias(rhs.Ab_); // Copy matrix and block structure
assertInvariants();
return *this;
}
/* ************************************************************************* */
void JacobianFactorOrdered::print(const string& s, const IndexFormatter& formatter) const {
cout << s << "\n";
if (empty()) {
cout << " empty, keys: ";
BOOST_FOREACH(const Index& key, keys()) { cout << formatter(key) << " "; }
cout << endl;
} else {
for(const_iterator key=begin(); key!=end(); ++key)
cout << boost::format("A[%1%]=\n")%formatter(*key) << getA(key) << endl;
cout << "b=" << getb() << endl;
model_->print("model");
}
}
/* ************************************************************************* */
// Check if two linear factors are equal
bool JacobianFactorOrdered::equals(const GaussianFactorOrdered& f_, double tol) const {
if(!dynamic_cast<const JacobianFactorOrdered*>(&f_))
return false;
else {
const JacobianFactorOrdered& f(static_cast<const JacobianFactorOrdered&>(f_));
if (empty()) return (f.empty());
if(keys()!=f.keys() /*|| !model_->equals(lf->model_, tol)*/)
return false;
if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols()))
return false;
constABlock Ab1(Ab_.range(0, Ab_.nBlocks()));
constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks()));
for(size_t row=0; row< (size_t) Ab1.rows(); ++row)
if(!equal_with_abs_tol(Ab1.row(row), Ab2.row(row), tol) &&
!equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol))
return false;
return true;
}
}
/* ************************************************************************* */
Vector JacobianFactorOrdered::unweighted_error(const VectorValuesOrdered& c) const {
Vector e = -getb();
if (empty()) return e;
for(size_t pos=0; pos<size(); ++pos)
e += Ab_(pos) * c[keys_[pos]];
return e;
}
/* ************************************************************************* */
Vector JacobianFactorOrdered::error_vector(const VectorValuesOrdered& c) const {
if (empty()) return model_->whiten(-getb());
return model_->whiten(unweighted_error(c));
}
/* ************************************************************************* */
double JacobianFactorOrdered::error(const VectorValuesOrdered& c) const {
if (empty()) return 0;
Vector weighted = error_vector(c);
return 0.5 * weighted.dot(weighted);
}
/* ************************************************************************* */
Matrix JacobianFactorOrdered::augmentedInformation() const {
Matrix AbWhitened = Ab_.full();
model_->WhitenInPlace(AbWhitened);
return AbWhitened.transpose() * AbWhitened;
}
/* ************************************************************************* */
Matrix JacobianFactorOrdered::information() const {
Matrix AWhitened = this->getA();
model_->WhitenInPlace(AWhitened);
return AWhitened.transpose() * AWhitened;
}
/* ************************************************************************* */
Vector JacobianFactorOrdered::operator*(const VectorValuesOrdered& x) const {
Vector Ax = zero(Ab_.rows());
if (empty()) return Ax;
// Just iterate over all A matrices and multiply in correct config part
for(size_t pos=0; pos<size(); ++pos)
Ax += Ab_(pos) * x[keys_[pos]];
return model_->whiten(Ax);
}
/* ************************************************************************* */
void JacobianFactorOrdered::transposeMultiplyAdd(double alpha, const Vector& e,
VectorValuesOrdered& x) const {
Vector E = alpha * model_->whiten(e);
// Just iterate over all A matrices and insert Ai^e into VectorValues
for(size_t pos=0; pos<size(); ++pos)
gtsam::transposeMultiplyAdd(1.0, Ab_(pos), E, x[keys_[pos]]);
}
/* ************************************************************************* */
pair<Matrix,Vector> JacobianFactorOrdered::matrix(bool weight) const {
Matrix A(Ab_.range(0, size()));
Vector b(getb());
// divide in sigma so error is indeed 0.5*|Ax-b|
if (weight) model_->WhitenSystem(A,b);
return make_pair(A, b);
}
/* ************************************************************************* */
Matrix JacobianFactorOrdered::matrix_augmented(bool weight) const {
if (weight) { Matrix Ab(Ab_.range(0,Ab_.nBlocks())); model_->WhitenInPlace(Ab); return Ab; }
else return Ab_.range(0, Ab_.nBlocks());
}
/* ************************************************************************* */
std::vector<boost::tuple<size_t, size_t, double> >
JacobianFactorOrdered::sparse(const std::vector<size_t>& columnIndices) const {
std::vector<boost::tuple<size_t, size_t, double> > entries;
// iterate over all variables in the factor
for(const_iterator var=begin(); var<end(); ++var) {
Matrix whitenedA(model_->Whiten(getA(var)));
// find first column index for this key
size_t column_start = columnIndices[*var];
for (size_t i = 0; i < (size_t) whitenedA.rows(); i++)
for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) {
double s = whitenedA(i,j);
if (std::abs(s) > 1e-12) entries.push_back(
boost::make_tuple(i, column_start + j, s));
}
}
Vector whitenedb(model_->whiten(getb()));
size_t bcolumn = columnIndices.back();
for (size_t i = 0; i < (size_t) whitenedb.size(); i++)
entries.push_back(boost::make_tuple(i, bcolumn, whitenedb(i)));
// return the result
return entries;
}
/* ************************************************************************* */
JacobianFactorOrdered JacobianFactorOrdered::whiten() const {
JacobianFactorOrdered result(*this);
result.model_->WhitenInPlace(result.matrix_);
result.model_ = noiseModel::Unit::Create(result.model_->dim());
return result;
}
/* ************************************************************************* */
GaussianFactorOrdered::shared_ptr JacobianFactorOrdered::negate() const {
HessianFactorOrdered hessian(*this);
return hessian.negate();
}
/* ************************************************************************* */
GaussianConditionalOrdered::shared_ptr JacobianFactorOrdered::eliminateFirst() {
return this->eliminate(1);
}
/* ************************************************************************* */
GaussianConditionalOrdered::shared_ptr JacobianFactorOrdered::splitConditional(size_t nrFrontals) {
assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == (size_t) matrix_.rows() && Ab_.firstBlock() == 0);
assert(size() >= nrFrontals);
assertInvariants();
const bool debug = ISDEBUG("JacobianFactor::splitConditional");
if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl;
if(debug) this->print("Splitting JacobianFactor: ");
size_t frontalDim = Ab_.range(0,nrFrontals).cols();
// Check for singular factor
if(model_->dim() < frontalDim)
throw IndeterminantLinearSystemException(this->keys().front());
// Extract conditional
gttic(cond_Rd);
// Restrict the matrix to be in the first nrFrontals variables
Ab_.rowEnd() = Ab_.rowStart() + frontalDim;
const Eigen::VectorBlock<const Vector> sigmas = model_->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart());
GaussianConditionalOrdered::shared_ptr conditional(new GaussianConditionalOrdered(begin(), end(), nrFrontals, Ab_, sigmas));
if(debug) conditional->print("Extracted conditional: ");
Ab_.rowStart() += frontalDim;
Ab_.firstBlock() += nrFrontals;
gttoc(cond_Rd);
if(debug) conditional->print("Extracted conditional: ");
gttic(remaining_factor);
// Take lower-right block of Ab to get the new factor
Ab_.rowEnd() = model_->dim();
keys_.erase(begin(), begin() + nrFrontals);
// Set sigmas with the right model
if (model_->isConstrained())
model_ = noiseModel::Constrained::MixedSigmas(sub(model_->sigmas(), frontalDim, model_->dim()));
else
model_ = noiseModel::Diagonal::Sigmas(sub(model_->sigmas(), frontalDim, model_->dim()));
if(debug) this->print("Eliminated factor: ");
assert(Ab_.rows() <= Ab_.cols()-1);
gttoc(remaining_factor);
if(debug) print("Eliminated factor: ");
assertInvariants();
return conditional;
}
/* ************************************************************************* */
GaussianConditionalOrdered::shared_ptr JacobianFactorOrdered::eliminate(size_t nrFrontals) {
assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == (size_t) matrix_.rows() && Ab_.firstBlock() == 0);
assert(size() >= nrFrontals);
assertInvariants();
const bool debug = ISDEBUG("JacobianFactor::eliminate");
if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl;
if(debug) this->print("Eliminating JacobianFactor: ");
if(debug) gtsam::print(matrix_, "Augmented Ab: ");
size_t frontalDim = Ab_.range(0,nrFrontals).cols();
if(debug) cout << "frontalDim = " << frontalDim << endl;
// Use in-place QR dense Ab appropriate to NoiseModel
gttic(QR);
SharedDiagonal noiseModel = model_->QR(matrix_);
// In the new unordered code, empty noise model indicates unit noise model, and I already
// modified QR to return an empty noise model. This just creates a unit noise model in that
// case because this old code does not handle empty noise models.
if(!noiseModel)
noiseModel = noiseModel::Unit::Create(std::min(matrix_.rows(), matrix_.cols() - 1));
gttoc(QR);
// Zero the lower-left triangle. todo: not all of these entries actually
// need to be zeroed if we are careful to start copying rows after the last
// structural zero.
if(matrix_.rows() > 0)
for(size_t j=0; j<(size_t) matrix_.cols(); ++j)
for(size_t i=j+1; i<noiseModel->dim(); ++i)
matrix_(i,j) = 0.0;
if(debug) gtsam::print(matrix_, "QR result: ");
if(debug) noiseModel->print("QR result noise model: ");
// Start of next part
model_ = noiseModel;
return splitConditional(nrFrontals);
}
/* ************************************************************************* */
void JacobianFactorOrdered::allocate(const VariableSlots& variableSlots, vector<
size_t>& varDims, size_t m) {
keys_.resize(variableSlots.size());
std::transform(variableSlots.begin(), variableSlots.end(), begin(),
boost::bind(&VariableSlots::const_iterator::value_type::first, _1));
varDims.push_back(1);
Ab_.copyStructureFrom(BlockAb(matrix_, varDims.begin(), varDims.end(), m));
}
/* ************************************************************************* */
void JacobianFactorOrdered::setModel(bool anyConstrained, const Vector& sigmas) {
if((size_t) sigmas.size() != this->rows())
throw InvalidNoiseModel(this->rows(), sigmas.size());
if (anyConstrained)
model_ = noiseModel::Constrained::MixedSigmas(sigmas);
else
model_ = noiseModel::Diagonal::Sigmas(sigmas);
}
/* ************************************************************************* */
const char* JacobianFactorOrdered::InvalidNoiseModel::what() const throw() {
if(description_.empty())
description_ = (boost::format(
"A JacobianFactor was attempted to be constructed or modified to use a\n"
"noise model of incompatible dimension. The JacobianFactor has\n"
"dimensionality (i.e. length of error vector) %d but the provided noise\n"
"model has dimensionality %d.") % factorDims % noiseModelDims).str();
return description_.c_str();
}
}

335
gtsam/linear/JacobianFactorOrdered.h
View File

@ -1,335 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 JacobianFactor.h
* @author Richard Roberts
* @date Dec 8, 2010
*/
#pragma once
#include <gtsam/linear/GaussianFactorOrdered.h>
#include <gtsam/linear/GaussianConditionalOrdered.h>
#include <gtsam/linear/Errors.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/inference/FactorGraphOrdered.h>
#include <gtsam/base/blockMatrices.h>
#include <gtsam/global_includes.h>
#include <boost/tuple/tuple.hpp>
// Forward declarations of friend unit tests
class JacobianFactorOrderedCombine2Test;
class JacobianFactorOrderedeliminateFrontalsTest;
class JacobianFactorOrderedconstructor2Test;
namespace gtsam {
// Forward declarations
class HessianFactorOrdered;
class VariableSlots;
template<class C> class BayesNetOrdered;
class GaussianFactorGraphOrdered;
/**
* A Gaussian factor in the squared-error form.
*
* JacobianFactor implements a
* Gaussian, which has quadratic negative log-likelihood
* \f[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \f]
* where \f$ \Sigma \f$ is a \a diagonal covariance matrix. The
* matrix \f$ A \f$, r.h.s. vector \f$ b \f$, and diagonal noise model
* \f$ \Sigma \f$ are stored in this class.
*
* This factor represents the sum-of-squares error of a \a linear
* measurement function, and is created upon linearization of a NoiseModelFactor,
* which in turn is a sum-of-squares factor with a nonlinear measurement function.
*
* Here is an example of how this factor represents a sum-of-squares error:
*
* Letting \f$ h(x) \f$ be a \a linear measurement prediction function, \f$ z \f$ be
* the actual observed measurement, the residual is
* \f[ f(x) = h(x) - z . \f]
* If we expect noise with diagonal covariance matrix \f$ \Sigma \f$ on this
* measurement, then the negative log-likelihood of the Gaussian induced by this
* measurement model is
* \f[ E(x) = \frac{1}{2} (h(x) - z)^T \Sigma^{-1} (h(x) - z) . \f]
* Because \f$ h(x) \f$ is linear, we can write it as
* \f[ h(x) = Ax + e \f]
* and thus we have
* \f[ E(x) = \frac{1}{2} (Ax-b)^T \Sigma^{-1} (Ax-b) \f]
* where \f$ b = z - e \f$.
*
* This factor can involve an arbitrary number of variables, and in the
* above example \f$ x \f$ would almost always be only be a subset of the variables
* in the entire factor graph. There are special constructors for 1-, 2-, and 3-
* way JacobianFactors, and additional constructors for creating n-way JacobianFactors.
* The Jacobian matrix \f$ A \f$ is passed to these constructors in blocks,
* for example, for a 2-way factor, the constructor would accept \f$ A1 \f$ and \f$ A2 \f$,
* as well as the variable indices \f$ j1 \f$ and \f$ j2 \f$
* and the negative log-likelihood represented by this factor would be
* \f[ E(x) = \frac{1}{2} (A_1 x_{j1} + A_2 x_{j2} - b)^T \Sigma^{-1} (A_1 x_{j1} + A_2 x_{j2} - b) . \f]
*/
class GTSAM_EXPORT JacobianFactorOrdered : public GaussianFactorOrdered {
protected:
typedef Matrix AbMatrix;
typedef VerticalBlockView<AbMatrix> BlockAb;
noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
AbMatrix matrix_; // the full matrix corresponding to the factor
BlockAb Ab_; // the block view of the full matrix
typedef GaussianFactorOrdered Base; // typedef to base
public:
typedef boost::shared_ptr<JacobianFactorOrdered> shared_ptr;
typedef BlockAb::Block ABlock;
typedef BlockAb::constBlock constABlock;
typedef BlockAb::Column BVector;
typedef BlockAb::constColumn constBVector;
/** Copy constructor */
JacobianFactorOrdered(const JacobianFactorOrdered& gf);
/** Convert from other GaussianFactor */
JacobianFactorOrdered(const GaussianFactorOrdered& gf);
/** default constructor for I/O */
JacobianFactorOrdered();
/** Construct Null factor */
JacobianFactorOrdered(const Vector& b_in);
/** Construct unary factor */
JacobianFactorOrdered(Index i1, const Matrix& A1,
const Vector& b, const SharedDiagonal& model);
/** Construct binary factor */
JacobianFactorOrdered(Index i1, const Matrix& A1,
Index i2, const Matrix& A2,
const Vector& b, const SharedDiagonal& model);
/** Construct ternary factor */
JacobianFactorOrdered(Index i1, const Matrix& A1, Index i2,
const Matrix& A2, Index i3, const Matrix& A3,
const Vector& b, const SharedDiagonal& model);
/** Construct an n-ary factor */
JacobianFactorOrdered(const std::vector<std::pair<Index, Matrix> > &terms,
const Vector &b, const SharedDiagonal& model);
JacobianFactorOrdered(const std::list<std::pair<Index, Matrix> > &terms,
const Vector &b, const SharedDiagonal& model);
/** Construct from Conditional Gaussian */
JacobianFactorOrdered(const GaussianConditionalOrdered& cg);
/** Convert from a HessianFactor (does Cholesky) */
JacobianFactorOrdered(const HessianFactorOrdered& factor);
/** Build a dense joint factor from all the factors in a factor graph. */
JacobianFactorOrdered(const GaussianFactorGraphOrdered& gfg);
/** Virtual destructor */
virtual ~JacobianFactorOrdered() {}
/** Aassignment operator */
JacobianFactorOrdered& operator=(const JacobianFactorOrdered& rhs);
/** Clone this JacobianFactor */
virtual GaussianFactorOrdered::shared_ptr clone() const {
return boost::static_pointer_cast<GaussianFactorOrdered>(
shared_ptr(new JacobianFactorOrdered(*this)));
}
// Implementing Testable interface
virtual void print(const std::string& s = "",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
virtual bool equals(const GaussianFactorOrdered& lf, double tol = 1e-9) const;
Vector unweighted_error(const VectorValuesOrdered& c) const; /** (A*x-b) */
Vector error_vector(const VectorValuesOrdered& c) const; /** (A*x-b)/sigma */
virtual double error(const VectorValuesOrdered& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */
/** Return the augmented information matrix represented by this GaussianFactor.
* The augmented information matrix contains the information matrix with an
* additional column holding the information vector, and an additional row
* holding the transpose of the information vector. The lower-right entry
* contains the constant error term (when \f$ \delta x = 0 \f$). The
* augmented information matrix is described in more detail in HessianFactor,
* which in fact stores an augmented information matrix.
*/
virtual Matrix augmentedInformation() const;
/** Return the non-augmented information matrix represented by this
* GaussianFactor.
*/
virtual Matrix information() const;
/**
* Construct the corresponding anti-factor to negate information
* stored stored in this factor.
* @return a HessianFactor with negated Hessian matrices
*/
virtual GaussianFactorOrdered::shared_ptr negate() const;
/** Check if the factor contains no information, i.e. zero rows. This does
* not necessarily mean that the factor involves no variables (to check for
* involving no variables use keys().empty()).
*/
bool empty() const { return Ab_.rows() == 0;}
/** is noise model constrained ? */
bool isConstrained() const { return model_->isConstrained();}
/** Return the dimension of the variable pointed to by the given key iterator
* todo: Remove this in favor of keeping track of dimensions with variables?
*/
virtual size_t getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); }
/**
* return the number of rows in the corresponding linear system
*/
size_t rows() const { return Ab_.rows(); }
/**
* return the number of rows in the corresponding linear system
*/
size_t numberOfRows() const { return rows(); }
/**
* return the number of columns in the corresponding linear system
*/
size_t cols() const { return Ab_.cols(); }
/** get a copy of model */
const SharedDiagonal& get_model() const { return model_; }
/** get a copy of model (non-const version) */
SharedDiagonal& get_model() { return model_; }
/** Get a view of the r.h.s. vector b */
const constBVector getb() const { return Ab_.column(size(), 0); }
/** Get a view of the A matrix for the variable pointed to by the given key iterator */
constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); }
/** Get a view of the A matrix */
constABlock getA() const { return Ab_.range(0, size()); }
/** Get a view of the r.h.s. vector b (non-const version) */
BVector getb() { return Ab_.column(size(), 0); }
/** Get a view of the A matrix for the variable pointed to by the given key iterator (non-const version) */
ABlock getA(iterator variable) { return Ab_(variable - begin()); }
/** Get a view of the A matrix */
ABlock getA() { return Ab_.range(0, size()); }
/** Return A*x */
Vector operator*(const VectorValuesOrdered& x) const;
/** x += A'*e */
void transposeMultiplyAdd(double alpha, const Vector& e, VectorValuesOrdered& x) const;
/**
* Return (dense) matrix associated with factor
* @param ordering of variables needed for matrix column order
* @param set weight to true to bake in the weights
*/
std::pair<Matrix, Vector> matrix(bool weight = true) const;
/**
* Return (dense) matrix associated with factor
* The returned system is an augmented matrix: [A b]
* @param set weight to use whitening to bake in weights
*/
Matrix matrix_augmented(bool weight = true) const;
/**
* Return vector of i, j, and s to generate an m-by-n sparse matrix
* such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse.
* As above, the standard deviations are baked into A and b
* @param columnIndices First column index for each variable.
*/
std::vector<boost::tuple<size_t, size_t, double> >
sparse(const std::vector<size_t>& columnIndices) const;
/**
* Return a whitened version of the factor, i.e. with unit diagonal noise
* model. */
JacobianFactorOrdered whiten() const;
/** Eliminate the first variable, modifying the factor in place to contain the remaining marginal. */
boost::shared_ptr<GaussianConditionalOrdered> eliminateFirst();
/** Eliminate the requested number of frontal variables, modifying the factor in place to contain the remaining marginal. */
boost::shared_ptr<GaussianConditionalOrdered> eliminate(size_t nrFrontals = 1);
/**
* splits a pre-factorized factor into a conditional, and changes the current
* factor to be the remaining component. Performs same operation as eliminate(),
* but without running QR.
*/
boost::shared_ptr<GaussianConditionalOrdered> splitConditional(size_t nrFrontals = 1);
// following methods all used in CombineJacobians:
// Many imperative, perhaps all need to be combined in constructor
/** allocate space */
void allocate(const VariableSlots& variableSlots,
std::vector<size_t>& varDims, size_t m);
/** set noiseModel correctly */
void setModel(bool anyConstrained, const Vector& sigmas);
/** Assert invariants after elimination */
void assertInvariants() const;
/** An exception indicating that the noise model dimension passed into the
* JacobianFactor has a different dimensionality than the factor. */
class InvalidNoiseModel : public std::exception {
public:
const size_t factorDims; ///< The dimensionality of the factor
const size_t noiseModelDims; ///< The dimensionality of the noise model
InvalidNoiseModel(size_t factorDims, size_t noiseModelDims) :
factorDims(factorDims), noiseModelDims(noiseModelDims) {}
virtual ~InvalidNoiseModel() throw() {}
virtual const char* what() const throw();
private:
mutable std::string description_;
};
private:
// Friend HessianFactor to facilitate conversion constructors
friend class HessianFactorOrdered;
// Friend unit tests (see also forward declarations above)
friend class ::JacobianFactorOrderedCombine2Test;
friend class ::JacobianFactorOrderedeliminateFrontalsTest;
friend class ::JacobianFactorOrderedconstructor2Test;
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactorOrdered);
ar & BOOST_SERIALIZATION_NVP(Ab_);
ar & BOOST_SERIALIZATION_NVP(model_);
ar & BOOST_SERIALIZATION_NVP(matrix_);
}
}; // JacobianFactor
} // gtsam

237
gtsam/linear/VectorValuesOrdered.cpp
View File

@ -1,237 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 VectorValues.cpp
* @brief Implementations for VectorValues
* @author Richard Roberts
* @author Alex Cunningham
*/
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/PermutationOrdered.h>
#include <gtsam/linear/VectorValuesOrdered.h>
#include <boost/iterator/counting_iterator.hpp>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
VectorValuesOrdered VectorValuesOrdered::Zero(const VectorValuesOrdered& x) {
VectorValuesOrdered result;
result.values_.resize(x.size());
for(size_t j=0; j<x.size(); ++j)
result.values_[j] = Vector::Zero(x.values_[j].size());
return result;
}
/* ************************************************************************* */
vector<size_t> VectorValuesOrdered::dims() const {
std::vector<size_t> result(this->size());
for(Index j = 0; j < this->size(); ++j)
result[j] = this->dim(j);
return result;
}
/* ************************************************************************* */
void VectorValuesOrdered::insert(Index j, const Vector& value) {
// Make sure j does not already exist
if(exists(j))
throw invalid_argument("VectorValues: requested variable index to insert already exists.");
// If this adds variables at the end, insert zero-length entries up to j
if(j >= size())
values_.resize(j+1);
// Assign value
values_[j] = value;
}
/* ************************************************************************* */
void VectorValuesOrdered::print(const std::string& str, const IndexFormatter& formatter) const {
std::cout << str << ": " << size() << " elements\n";
for (Index var = 0; var < size(); ++var)
std::cout << " " << formatter(var) << ": " << (*this)[var].transpose() << "\n";
std::cout.flush();
}
/* ************************************************************************* */
bool VectorValuesOrdered::equals(const VectorValuesOrdered& x, double tol) const {
if(this->size() != x.size())
return false;
for(Index j=0; j < size(); ++j)
if(!equal_with_abs_tol(values_[j], x.values_[j], tol))
return false;
return true;
}
/* ************************************************************************* */
void VectorValuesOrdered::resize(Index nVars, size_t varDim) {
values_.resize(nVars);
for(Index j = 0; j < nVars; ++j)
values_[j] = Vector(varDim);
}
/* ************************************************************************* */
void VectorValuesOrdered::resizeLike(const VectorValuesOrdered& other) {
values_.resize(other.size());
for(Index j = 0; j < other.size(); ++j)
values_[j].resize(other.values_[j].size());
}
/* ************************************************************************* */
VectorValuesOrdered VectorValuesOrdered::SameStructure(const VectorValuesOrdered& other) {
VectorValuesOrdered ret;
ret.resizeLike(other);
return ret;
}
/* ************************************************************************* */
VectorValuesOrdered VectorValuesOrdered::Zero(Index nVars, size_t varDim) {
VectorValuesOrdered ret(nVars, varDim);
ret.setZero();
return ret;
}
/* ************************************************************************* */
void VectorValuesOrdered::setZero() {
BOOST_FOREACH(Vector& v, *this) {
v.setZero();
}
}
/* ************************************************************************* */
const Vector VectorValuesOrdered::asVector() const {
return internal::extractVectorValuesSlices(*this,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(this->size()), true);
}
/* ************************************************************************* */
const Vector VectorValuesOrdered::vector(const std::vector<Index>& indices) const {
return internal::extractVectorValuesSlices(*this, indices.begin(), indices.end());
}
/* ************************************************************************* */
bool VectorValuesOrdered::hasSameStructure(const VectorValuesOrdered& other) const {
if(this->size() != other.size())
return false;
for(size_t j = 0; j < size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->values_[j].rows() != other.values_[j].rows())
return false;
return true;
}
/* ************************************************************************* */
void VectorValuesOrdered::permuteInPlace(const Permutation& permutation) {
// Allocate new values
Values newValues(this->size());
// Swap values from this VectorValues to the permuted VectorValues
for(size_t i = 0; i < this->size(); ++i)
newValues[i].swap(this->at(permutation[i]));
// Swap the values into this VectorValues
values_.swap(newValues);
}
/* ************************************************************************* */
void VectorValuesOrdered::permuteInPlace(const Permutation& selector, const Permutation& permutation) {
if(selector.size() != permutation.size())
throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes.");
// Create new index the size of the permuted entries
Values reorderedEntries(selector.size());
// Permute the affected entries into the new index
for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot)
reorderedEntries[dstSlot].swap(values_[selector[permutation[dstSlot]]]);
// Put the affected entries back in the new order
for(size_t slot = 0; slot < selector.size(); ++slot)
values_[selector[slot]].swap(reorderedEntries[slot]);
}
/* ************************************************************************* */
void VectorValuesOrdered::swap(VectorValuesOrdered& other) {
this->values_.swap(other.values_);
}
/* ************************************************************************* */
double VectorValuesOrdered::dot(const VectorValuesOrdered& v) const {
double result = 0.0;
if(this->size() != v.size())
throw invalid_argument("VectorValues::dot called with different vector sizes");
for(Index j = 0; j < this->size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->values_[j].size() == v.values_[j].size())
result += this->values_[j].dot(v.values_[j]);
else
throw invalid_argument("VectorValues::dot called with different vector sizes");
return result;
}
/* ************************************************************************* */
double VectorValuesOrdered::norm() const {
return std::sqrt(this->squaredNorm());
}
/* ************************************************************************* */
double VectorValuesOrdered::squaredNorm() const {
double sumSquares = 0.0;
for(Index j = 0; j < this->size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
sumSquares += this->values_[j].squaredNorm();
return sumSquares;
}
/* ************************************************************************* */
VectorValuesOrdered VectorValuesOrdered::operator+(const VectorValuesOrdered& c) const {
VectorValuesOrdered result = SameStructure(*this);
if(this->size() != c.size())
throw invalid_argument("VectorValues::operator+ called with different vector sizes");
for(Index j = 0; j < this->size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->values_[j].size() == c.values_[j].size())
result.values_[j] = this->values_[j] + c.values_[j];
else
throw invalid_argument("VectorValues::operator- called with different vector sizes");
return result;
}
/* ************************************************************************* */
VectorValuesOrdered VectorValuesOrdered::operator-(const VectorValuesOrdered& c) const {
VectorValuesOrdered result = SameStructure(*this);
if(this->size() != c.size())
throw invalid_argument("VectorValues::operator- called with different vector sizes");
for(Index j = 0; j < this->size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->values_[j].size() == c.values_[j].size())
result.values_[j] = this->values_[j] - c.values_[j];
else
throw invalid_argument("VectorValues::operator- called with different vector sizes");
return result;
}
/* ************************************************************************* */
void VectorValuesOrdered::operator+=(const VectorValuesOrdered& c) {
if(this->size() != c.size())
throw invalid_argument("VectorValues::operator+= called with different vector sizes");
for(Index j = 0; j < this->size(); ++j)
// Directly accessing maps instead of using VV::dim in case some values are empty
if(this->values_[j].size() == c.values_[j].size())
this->values_[j] += c.values_[j];
else
throw invalid_argument("VectorValues::operator+= called with different vector sizes");
}
/* ************************************************************************* */
} // \namespace gtsam

468
gtsam/linear/VectorValuesOrdered.h
View File

@ -1,468 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 VectorValues.h
* @brief Factor Graph Values
* @author Richard Roberts
*/
#pragma once
#include <gtsam/base/Vector.h>
#include <gtsam/global_includes.h>
#include <boost/format.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <numeric>
#include <stdexcept>
namespace gtsam {
// Forward declarations
class Permutation;
/**
* This class represents a collection of vector-valued variables associated
* each with a unique integer index. It is typically used to store the variables
* of a GaussianFactorGraph. Optimizing a GaussianFactorGraph or GaussianBayesNet
* returns this class.
*
* For basic usage, such as receiving a linear solution from gtsam solving functions,
* or creating this class in unit tests and examples where speed is not important,
* you can use a simple interface:
* - The default constructor VectorValues() to create this class
* - insert(Index, const Vector&) to add vector variables
* - operator[](Index) for read and write access to stored variables
* - \ref exists (Index) to check if a variable is present
* - Other facilities like iterators, size(), dim(), etc.
*
* Indices can be non-consecutive and inserted out-of-order, but you should not
* use indices that are larger than a reasonable array size because the indices
* correspond to positions in an internal array.
*
* Example:
* \code
VectorValues values;
values.insert(3, Vector_(3, 1.0, 2.0, 3.0));
values.insert(4, Vector_(2, 4.0, 5.0));
values.insert(0, Vector_(4, 6.0, 7.0, 8.0, 9.0));
// Prints [ 3.0 4.0 ]
gtsam::print(values[1]);
// Prints [ 8.0 9.0 ]
values[1] = Vector_(2, 8.0, 9.0);
gtsam::print(values[1]);
\endcode
*
* <h2>Advanced Interface and Performance Information</h2>
*
* Internally, all vector values are stored as part of one large vector. In
* gtsam this vector is always pre-allocated for efficiency, using the
* advanced interface described below. Accessing and modifying already-allocated
* values is \f$ O(1) \f$. Using the insert() function of the standard interface
* is slow because it requires re-allocating the internal vector.
*
* For advanced usage, or where speed is important:
* - Allocate space ahead of time using a pre-allocating constructor
* (\ref AdvancedConstructors "Advanced Constructors"), Zero(),
* SameStructure(), resize(), or append(). Do not use
* insert(Index, const Vector&), which always has to re-allocate the
* internal vector.
* - The vector() function permits access to the underlying Vector, for
* doing mathematical or other operations that require all values.
* - operator[]() returns a SubVector view of the underlying Vector,
* without copying any data.
*
* Access is through the variable index j, and returns a SubVector,
* which is a view on the underlying data structure.
*
* This class is additionally used in gradient descent and dog leg to store the gradient.
* \nosubgrouping
*/
class GTSAM_EXPORT VectorValuesOrdered {
protected:
typedef std::vector<Vector> Values; ///< Typedef for the collection of Vectors making up a VectorValues
Values values_; ///< Collection of Vectors making up this VectorValues
public:
typedef Values::iterator iterator; ///< Iterator over vector values
typedef Values::const_iterator const_iterator; ///< Const iterator over vector values
typedef Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values
typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values
typedef boost::shared_ptr<VectorValuesOrdered> shared_ptr; ///< shared_ptr to this class
/// @name Standard Constructors
/// @{
/**
* Default constructor creates an empty VectorValues.
*/
VectorValuesOrdered() {}
/** Named constructor to create a VectorValues of the same structure of the
* specified one, but filled with zeros.
* @return
*/
static VectorValuesOrdered Zero(const VectorValuesOrdered& model);
/// @}
/// @name Standard Interface
/// @{
/** Number of variables stored, always 1 more than the highest variable index,
* even if some variables with lower indices are not present. */
Index size() const { return values_.size(); }
/** Return the dimension of variable \c j. */
size_t dim(Index j) const { checkExists(j); return (*this)[j].rows(); }
/** Return the dimension of each vector in this container */
std::vector<size_t> dims() const;
/** Check whether a variable with index \c j exists. */
bool exists(Index j) const { return j < size() && values_[j].rows() > 0; }
/** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Index). */
Vector& at(Index j) { checkExists(j); return values_[j]; }
/** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Index). */
const Vector& at(Index j) const { checkExists(j); return values_[j]; }
/** Read/write access to the vector value with index \c j, throws std::out_of_range if \c j does not exist, identical to at(Index). */
Vector& operator[](Index j) { return at(j); }
/** Access the vector value with index \c j (const version), throws std::out_of_range if \c j does not exist, identical to at(Index). */
const Vector& operator[](Index j) const { return at(j); }
/** Insert a vector \c value with index \c j.
* Causes reallocation, but can insert values in any order.
* Throws an invalid_argument exception if the index \c j is already used.
* @param value The vector to be inserted.
* @param j The index with which the value will be associated.
*/
void insert(Index j, const Vector& value);
iterator begin() { return values_.begin(); } ///< Iterator over variables
const_iterator begin() const { return values_.begin(); } ///< Iterator over variables
iterator end() { return values_.end(); } ///< Iterator over variables
const_iterator end() const { return values_.end(); } ///< Iterator over variables
reverse_iterator rbegin() { return values_.rbegin(); } ///< Reverse iterator over variables
const_reverse_iterator rbegin() const { return values_.rbegin(); } ///< Reverse iterator over variables
reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables
const_reverse_iterator rend() const { return values_.rend(); } ///< Reverse iterator over variables
/** print required by Testable for unit testing */
void print(const std::string& str = "VectorValues: ",
const IndexFormatter& formatter = DefaultIndexFormatter) const;
/** equals required by Testable for unit testing */
bool equals(const VectorValuesOrdered& x, double tol = 1e-9) const;
/// @{
/// \anchor AdvancedConstructors
/// @name Advanced Constructors
/// @}
/** Construct from a container of variable dimensions (in variable order), without initializing any values. */
template<class CONTAINER>
explicit VectorValuesOrdered(const CONTAINER& dimensions) { this->append(dimensions); }
/** Construct to hold nVars vectors of varDim dimension each. */
VectorValuesOrdered(Index nVars, size_t varDim) { this->resize(nVars, varDim); }
/** Named constructor to create a VectorValues that matches the structure of
* the specified VectorValues, but do not initialize the new values. */
static VectorValuesOrdered SameStructure(const VectorValuesOrdered& other);
/** Named constructor to create a VectorValues from a container of variable
* dimensions that is filled with zeros.
* @param dimensions A container of the dimension of each variable to create.
*/
template<class CONTAINER>
static VectorValuesOrdered Zero(const CONTAINER& dimensions);
/** Named constructor to create a VectorValues filled with zeros that has
* \c nVars variables, each of dimension \c varDim
* @param nVars The number of variables to create
* @param varDim The dimension of each variable
* @return The new VectorValues
*/
static VectorValuesOrdered Zero(Index nVars, size_t varDim);
/// @}
/// @name Advanced Interface
/// @{
/** Resize this VectorValues to have identical structure to other, leaving
* this VectorValues with uninitialized values.
* @param other The VectorValues whose structure to copy
*/
void resizeLike(const VectorValuesOrdered& other);
/** Resize the VectorValues to hold \c nVars variables, each of dimension
* \c varDim. Any individual vectors that do not change size will keep
* their values, but any new or resized vectors will be uninitialized.
* @param nVars The number of variables to create
* @param varDim The dimension of each variable
*/
void resize(Index nVars, size_t varDim);
/** Resize the VectorValues to contain variables of the dimensions stored
* in \c dimensions. Any individual vectors that do not change size will keep
* their values, but any new or resized vectors will be uninitialized.
* @param dimensions A container of the dimension of each variable to create.
*/
template<class CONTAINER>
void resize(const CONTAINER& dimensions);
/** Append to the VectorValues to additionally contain variables of the
* dimensions stored in \c dimensions. The new variables are uninitialized,
* but this function is used to pre-allocate space for performance. This
* function preserves the original data, so all previously-existing variables
* are left unchanged.
* @param dimensions A container of the dimension of each variable to create.
*/
template<class CONTAINER>
void append(const CONTAINER& dimensions);
/** Removes the last subvector from the VectorValues */
void pop_back() { values_.pop_back(); };
/** Set all entries to zero, does not modify the size. */
void setZero();
/** Retrieve the entire solution as a single vector */
const Vector asVector() const;
/** Access a vector that is a subset of relevant indices */
const Vector vector(const std::vector<Index>& indices) const;
/** Check whether this VectorValues has the same structure, meaning has the
* same number of variables and that all variables are of the same dimension,
* as another VectorValues
* @param other The other VectorValues with which to compare structure
* @return \c true if the structure is the same, \c false if not.
*/
bool hasSameStructure(const VectorValuesOrdered& other) const;
/**
* Permute the variables in the VariableIndex according to the given partial permutation
*/
void permuteInPlace(const Permutation& selector, const Permutation& permutation);
/**
* Permute the entries of this VectorValues in place
*/
void permuteInPlace(const Permutation& permutation);
/**
* Swap the data in this VectorValues with another.
*/
void swap(VectorValuesOrdered& other);
/// @}
/// @name Linear algebra operations
/// @{
/** Dot product with another VectorValues, interpreting both as vectors of
* their concatenated values. */
double dot(const VectorValuesOrdered& v) const;
/** Vector L2 norm */
double norm() const;
/** Squared vector L2 norm */
double squaredNorm() const;
/**
* + operator does element-wise addition. Both VectorValues must have the
* same structure (checked when NDEBUG is not defined).
*/
VectorValuesOrdered operator+(const VectorValuesOrdered& c) const;
/**
* + operator does element-wise subtraction. Both VectorValues must have the
* same structure (checked when NDEBUG is not defined).
*/
VectorValuesOrdered operator-(const VectorValuesOrdered& c) const;
/**
* += operator does element-wise addition. Both VectorValues must have the
* same structure (checked when NDEBUG is not defined).
*/
void operator+=(const VectorValuesOrdered& c);
/// @}
/// @}
/// @name Matlab syntactic sugar for linear algebra operations
/// @{
inline VectorValuesOrdered add(const VectorValuesOrdered& c) const { return *this + c; }
inline VectorValuesOrdered scale(const double a, const VectorValuesOrdered& c) const { return a * (*this); }
/// @}
private:
// Throw an exception if j does not exist
void checkExists(Index j) const {
if(!exists(j)) {
const std::string msg =
(boost::format("VectorValues: requested variable index j=%1% is not in this VectorValues.") % j).str();
throw std::out_of_range(msg);
}
}
public:
/**
* scale a vector by a scalar
*/
friend VectorValuesOrdered operator*(const double a, const VectorValuesOrdered &v) {
VectorValuesOrdered result = VectorValuesOrdered::SameStructure(v);
for(Index j = 0; j < v.size(); ++j)
result.values_[j] = a * v.values_[j];
return result;
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void scal(double alpha, VectorValuesOrdered& x) {
for(Index j = 0; j < x.size(); ++j)
x.values_[j] *= alpha;
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void axpy(double alpha, const VectorValuesOrdered& x, VectorValuesOrdered& y) {
if(x.size() != y.size())
throw std::invalid_argument("axpy(VectorValues) called with different vector sizes");
for(Index j = 0; j < x.size(); ++j)
if(x.values_[j].size() == y.values_[j].size())
y.values_[j] += alpha * x.values_[j];
else
throw std::invalid_argument("axpy(VectorValues) called with different vector sizes");
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void sqrt(VectorValuesOrdered &x) {
for(Index j = 0; j < x.size(); ++j)
x.values_[j] = x.values_[j].cwiseSqrt();
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void ediv(const VectorValuesOrdered& numerator, const VectorValuesOrdered& denominator, VectorValuesOrdered &result) {
if(numerator.size() != denominator.size() || numerator.size() != result.size())
throw std::invalid_argument("ediv(VectorValues) called with different vector sizes");
for(Index j = 0; j < numerator.size(); ++j)
if(numerator.values_[j].size() == denominator.values_[j].size() && numerator.values_[j].size() == result.values_[j].size())
result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]);
else
throw std::invalid_argument("ediv(VectorValues) called with different vector sizes");
}
/// TODO: linear algebra interface seems to have been added for SPCG.
friend void edivInPlace(VectorValuesOrdered& x, const VectorValuesOrdered& y) {
if(x.size() != y.size())
throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
for(Index j = 0; j < x.size(); ++j)
if(x.values_[j].size() == y.values_[j].size())
x.values_[j].array() /= y.values_[j].array();
else
throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(values_);
}
}; // VectorValues definition
// Implementations of template and inline functions
/* ************************************************************************* */
template<class CONTAINER>
void VectorValuesOrdered::resize(const CONTAINER& dimensions) {
values_.clear();
append(dimensions);
}
/* ************************************************************************* */
template<class CONTAINER>
void VectorValuesOrdered::append(const CONTAINER& dimensions) {
size_t i = size();
values_.resize(size() + dimensions.size());
BOOST_FOREACH(size_t dim, dimensions) {
values_[i] = Vector(dim);
++ i;
}
}
/* ************************************************************************* */
template<class CONTAINER>
VectorValuesOrdered VectorValuesOrdered::Zero(const CONTAINER& dimensions) {
VectorValuesOrdered ret;
ret.values_.resize(dimensions.size());
size_t i = 0;
BOOST_FOREACH(size_t dim, dimensions) {
ret.values_[i] = Vector::Zero(dim);
++ i;
}
return ret;
}
namespace internal {
/* ************************************************************************* */
// Helper function, extracts vectors with variable indices
// in the first and last iterators, and concatenates them in that order into the
// output.
template<typename ITERATOR>
const Vector extractVectorValuesSlices(const VectorValuesOrdered& values, ITERATOR first, ITERATOR last, bool allowNonexistant = false) {
// Find total dimensionality
size_t dim = 0;
for(ITERATOR j = first; j != last; ++j)
// If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent)
if(!allowNonexistant || values.exists(*j))
dim += values.dim(*j);
// Copy vectors
Vector ret(dim);
size_t varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
// If allowNonexistant is true, skip nonexistent indices (otherwise dim will throw an error on nonexistent)
if(!allowNonexistant || values.exists(*j)) {
ret.segment(varStart, values.dim(*j)) = values[*j];
varStart += values.dim(*j);
}
}
return ret;
}
/* ************************************************************************* */
// Helper function, writes to the variables in values
// with indices iterated over by first and last, interpreting vector as the
// concatenated vectors to write.
template<class VECTOR, typename ITERATOR>
void writeVectorValuesSlices(const VECTOR& vector, VectorValuesOrdered& values, ITERATOR first, ITERATOR last) {
// Copy vectors
size_t varStart = 0;
for(ITERATOR j = first; j != last; ++j) {
values[*j] = vector.segment(varStart, values[*j].rows());
varStart += values[*j].rows();
}
assert(varStart == vector.rows());
}
}
} // \namespace gtsam

354
gtsam/linear/tests/testGaussianConditionalObsolete.cpp
View File

@ -1,354 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testGaussianConditional.cpp
* @brief Unit tests for Conditional gaussian
* @author Christian Potthast
**/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/linear/JacobianFactorOrdered.h>
#include <gtsam/linear/GaussianConditionalOrdered.h>
#include <gtsam/linear/GaussianBayesNetOrdered.h>
#include <iostream>
#include <sstream>
#include <vector>
#include <boost/assign/std/list.hpp>
#include <boost/assign/std/vector.hpp>
using namespace gtsam;
using namespace std;
using namespace boost::assign;
static const double tol = 1e-5;
static const Index _x_=0, _x1_=1, _l1_=2;
Matrix R = Matrix_(2,2,
-12.1244, -5.1962,
0., 4.6904);
/* ************************************************************************* */
TEST(GaussianConditionalOrdered, constructor)
{
Matrix S1 = Matrix_(2,2,
-5.2786, -8.6603,
5.0254, 5.5432);
Matrix S2 = Matrix_(2,2,
-10.5573, -5.9385,
5.5737, 3.0153);
Matrix S3 = Matrix_(2,2,
-11.3820, -7.2581,
-3.0153, -3.5635);
Vector d = Vector_(2, 1.0, 2.0);
Vector s = Vector_(2, 3.0, 4.0);
list<pair<Index, Matrix> > terms;
terms +=
make_pair(3, S1),
make_pair(5, S2),
make_pair(7, S3);
GaussianConditionalOrdered actual(1, d, R, terms, s);
GaussianConditionalOrdered::const_iterator it = actual.beginFrontals();
EXPECT(assert_equal(Index(1), *it));
EXPECT(assert_equal(R, actual.get_R()));
++ it;
EXPECT(it == actual.endFrontals());
it = actual.beginParents();
EXPECT(assert_equal(Index(3), *it));
EXPECT(assert_equal(S1, actual.get_S(it)));
++ it;
EXPECT(assert_equal(Index(5), *it));
EXPECT(assert_equal(S2, actual.get_S(it)));
++ it;
EXPECT(assert_equal(Index(7), *it));
EXPECT(assert_equal(S3, actual.get_S(it)));
++it;
EXPECT(it == actual.endParents());
EXPECT(assert_equal(d, actual.get_d()));
EXPECT(assert_equal(s, actual.get_sigmas()));
// test copy constructor
GaussianConditionalOrdered copied(actual);
EXPECT(assert_equal(d, copied.get_d()));
EXPECT(assert_equal(s, copied.get_sigmas()));
EXPECT(assert_equal(R, copied.get_R()));
}
/* ************************************************************************* */
GaussianConditionalOrdered construct() {
Vector d = Vector_(2, 1.0, 2.0);
Vector s = Vector_(2, 3.0, 4.0);
GaussianConditionalOrdered::shared_ptr shared(new GaussianConditionalOrdered(1, d, R, s));
return *shared;
}
TEST(GaussianConditionalOrdered, return_value)
{
Vector d = Vector_(2, 1.0, 2.0);
Vector s = Vector_(2, 3.0, 4.0);
GaussianConditionalOrdered copied = construct();
EXPECT(assert_equal(d, copied.get_d()));
EXPECT(assert_equal(s, copied.get_sigmas()));
EXPECT(assert_equal(R, copied.get_R()));
}
/* ************************************************************************* */
TEST( GaussianConditionalOrdered, equals )
{
// create a conditional gaussian node
Matrix A1(2,2);
A1(0,0) = 1 ; A1(1,0) = 2;
A1(0,1) = 3 ; A1(1,1) = 4;
Matrix A2(2,2);
A2(0,0) = 6 ; A2(1,0) = 0.2;
A2(0,1) = 8 ; A2(1,1) = 0.4;
Matrix R(2,2);
R(0,0) = 0.1 ; R(1,0) = 0.3;
R(0,1) = 0.0 ; R(1,1) = 0.34;
Vector tau(2);
tau(0) = 1.0;
tau(1) = 0.34;
Vector d(2);
d(0) = 0.2; d(1) = 0.5;
GaussianConditionalOrdered
expected(_x_,d, R, _x1_, A1, _l1_, A2, tau),
actual(_x_,d, R, _x1_, A1, _l1_, A2, tau);
EXPECT( expected.equals(actual) );
}
/* ************************************************************************* */
TEST( GaussianConditionalOrdered, solve )
{
//expected solution
Vector expectedX(2);
expectedX(0) = 20-3-11 ; expectedX(1) = 40-7-15;
// create a conditional Gaussian node
Matrix R = Matrix_(2,2, 1., 0.,
0., 1.);
Matrix A1 = Matrix_(2,2, 1., 2.,
3., 4.);
Matrix A2 = Matrix_(2,2, 5., 6.,
7., 8.);
Vector d(2);
d(0) = 20.0; d(1) = 40.0;
Vector tau = ones(2);
GaussianConditionalOrdered cg(_x_, d, R, _x1_, A1, _l1_, A2, tau);
Vector sx1(2);
sx1(0) = 1.0; sx1(1) = 1.0;
Vector sl1(2);
sl1(0) = 1.0; sl1(1) = 1.0;
VectorValuesOrdered solution(vector<size_t>(3, 2));
solution[_x_] = d;
solution[_x1_] = sx1; // parents
solution[_l1_] = sl1;
VectorValuesOrdered expected(vector<size_t>(3, 2));
expected[_x_] = expectedX;
expected[_x1_] = sx1;
expected[_l1_] = sl1;
cg.solveInPlace(solution);
EXPECT(assert_equal(expected, solution, 0.0001));
}
/* ************************************************************************* */
TEST( GaussianConditionalOrdered, solve_simple )
{
Matrix full_matrix = Matrix_(4, 7,
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3,
0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4);
// solve system as a non-multifrontal version first
// 2 variables, frontal has dim=4
vector<size_t> dims; dims += 4, 2, 1;
GaussianConditionalOrdered::rsd_type matrices(full_matrix, dims.begin(), dims.end());
Vector sigmas = ones(4);
vector<size_t> cgdims; cgdims += _x_, _x1_;
GaussianConditionalOrdered cg(cgdims.begin(), cgdims.end(), 1, matrices, sigmas);
// partial solution
Vector sx1 = Vector_(2, 9.0, 10.0);
// elimination order; _x_, _x1_
vector<size_t> vdim; vdim += 4, 2;
VectorValuesOrdered actual(vdim);
actual[_x1_] = sx1; // parent
VectorValuesOrdered expected(vdim);
expected[_x1_] = sx1;
expected[_x_] = Vector_(4, -3.1,-3.4,-11.9,-13.2);
// verify indices/size
EXPECT_LONGS_EQUAL(2, cg.size());
EXPECT_LONGS_EQUAL(4, cg.dim());
// solve and verify
cg.solveInPlace(actual);
EXPECT(assert_equal(expected, actual, tol));
}
/* ************************************************************************* */
TEST( GaussianConditionalOrdered, solve_multifrontal )
{
// create full system, 3 variables, 2 frontals, all 2 dim
Matrix full_matrix = Matrix_(4, 7,
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3,
0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4);
// 3 variables, all dim=2
vector<size_t> dims; dims += 2, 2, 2, 1;
GaussianConditionalOrdered::rsd_type matrices(full_matrix, dims.begin(), dims.end());
Vector sigmas = ones(4);
vector<size_t> cgdims; cgdims += _x_, _x1_, _l1_;
GaussianConditionalOrdered cg(cgdims.begin(), cgdims.end(), 2, matrices, sigmas);
EXPECT(assert_equal(Vector_(4, 0.1, 0.2, 0.3, 0.4), cg.get_d()));
// partial solution
Vector sl1 = Vector_(2, 9.0, 10.0);
// elimination order; _x_, _x1_, _l1_
VectorValuesOrdered actual(vector<size_t>(3, 2));
actual[_l1_] = sl1; // parent
VectorValuesOrdered expected(vector<size_t>(3, 2));
expected[_x_] = Vector_(2, -3.1,-3.4);
expected[_x1_] = Vector_(2, -11.9,-13.2);
expected[_l1_] = sl1;
// verify indices/size
EXPECT_LONGS_EQUAL(3, cg.size());
EXPECT_LONGS_EQUAL(4, cg.dim());
// solve and verify
cg.solveInPlace(actual);
EXPECT(assert_equal(expected, actual, tol));
}
/* ************************************************************************* */
TEST( GaussianConditionalOrdered, solveTranspose ) {
static const Index _y_=1;
/** create small Chordal Bayes Net x <- y
* x y d
* 1 1 9
* 1 5
*/
Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0);
Matrix R22 = Matrix_(1, 1, 1.0);
Vector d1(1), d2(1);
d1(0) = 9;
d2(0) = 5;
Vector tau(1);
tau(0) = 1.0;
// define nodes and specify in reverse topological sort (i.e. parents last)
GaussianConditionalOrdered::shared_ptr Px_y(new GaussianConditionalOrdered(_x_, d1, R11, _y_, S12, tau));
GaussianConditionalOrdered::shared_ptr Py(new GaussianConditionalOrdered(_y_, d2, R22, tau));
GaussianBayesNetOrdered cbn;
cbn.push_back(Px_y);
cbn.push_back(Py);
// x=R'*y, y=inv(R')*x
// 2 = 1 2
// 5 1 1 3
VectorValuesOrdered y(vector<size_t>(2,1)), x(vector<size_t>(2,1));
x[_x_] = Vector_(1,2.);
x[_y_] = Vector_(1,5.);
y[_x_] = Vector_(1,2.);
y[_y_] = Vector_(1,3.);
// test functional version
VectorValuesOrdered actual = backSubstituteTranspose(cbn,x);
CHECK(assert_equal(y,actual));
}
/* ************************************************************************* */
TEST( GaussianConditionalOrdered, information ) {
// Create R matrix
Matrix R = (Matrix(4,4) <<
1, 2, 3, 4,
0, 5, 6, 7,
0, 0, 8, 9,
0, 0, 0, 10).finished();
// Create conditional
GaussianConditionalOrdered conditional(0, Vector::Zero(4), R, Vector::Constant(4, 1.0));
// Expected information matrix (using permuted R)
Matrix IExpected = R.transpose() * R;
// Actual information matrix (conditional should permute R)
Matrix IActual = conditional.information();
EXPECT(assert_equal(IExpected, IActual));
}
/* ************************************************************************* */
TEST( GaussianConditionalOrdered, isGaussianFactor ) {
// Create R matrix
Matrix R = (Matrix(4,4) <<
1, 2, 3, 4,
0, 5, 6, 7,
0, 0, 8, 9,
0, 0, 0, 10).finished();
// Create a conditional
GaussianConditionalOrdered conditional(0, Vector::Zero(4), R, Vector::Constant(4, 1.0));
// Expected information matrix computed by conditional
Matrix IExpected = conditional.information();
// Expected information matrix computed by a factor
JacobianFactorOrdered jf = *conditional.toFactor();
Matrix IActual = jf.getA(jf.begin()).transpose() * jf.getA(jf.begin());
EXPECT(assert_equal(IExpected, IActual));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

537
gtsam/linear/tests/testGaussianFactorGraphObsolete.cpp
View File

@ -1,537 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testGaussianFactor.cpp
* @brief Unit tests for Linear Factor
* @author Christian Potthast
* @author Frank Dellaert
**/
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/debug.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/VariableIndexOrdered.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
using namespace std;
using namespace gtsam;
using namespace boost;
static SharedDiagonal
sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2),
constraintModel = noiseModel::Constrained::All(2);
static GaussianFactorGraphOrdered createSimpleGaussianFactorGraph() {
GaussianFactorGraphOrdered fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
fg.add(2, 10*eye(2), -1.0*ones(2), unit2);
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
fg.add(2, -10*eye(2), 0, 10*eye(2), Vector_(2, 2.0, -1.0), unit2);
// measurement between x1 and l1: l1-x1=[0.0;0.2]
fg.add(2, -5*eye(2), 1, 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
fg.add(0, -5*eye(2), 1, 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
return fg;
}
/* ************************************************************************* */
TEST(GaussianFactorGraphOrdered, initialization) {
// Create empty graph
GaussianFactorGraphOrdered fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
fg.add(0, 10*eye(2), -1.0*ones(2), unit2);
fg.add(0, -10*eye(2),1, 10*eye(2), Vector_(2, 2.0, -1.0), unit2);
fg.add(0, -5*eye(2), 2, 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
fg.add(1, -5*eye(2), 2, 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
EXPECT_LONGS_EQUAL(4, fg.size());
JacobianFactorOrdered factor = *boost::dynamic_pointer_cast<JacobianFactorOrdered>(fg[0]);
// Test sparse, which takes a vector and returns a matrix, used in MATLAB
// Note that this the augmented vector and the RHS is in column 7
Matrix expectedIJS = Matrix_(3,22,
1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 5., 6., 7., 8., 7., 8., 7., 8.,
1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 7., 3., 4., 5., 6., 7., 7.,
10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5., 0., 1., -5., -5., 5., 5., -1., 1.5
);
Matrix actualIJS = fg.sparseJacobian_();
EQUALITY(expectedIJS, actualIJS);
}
/* ************************************************************************* */
TEST(GaussianFactorGraphOrdered, CombineJacobians)
{
Matrix A01 = Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b0 = Vector_(3, 1.5, 1.5, 1.5);
Vector s0 = Vector_(3, 1.6, 1.6, 1.6);
Matrix A10 = Matrix_(3,3,
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 2.0);
Matrix A11 = Matrix_(3,3,
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0);
Vector b1 = Vector_(3, 2.5, 2.5, 2.5);
Vector s1 = Vector_(3, 2.6, 2.6, 2.6);
Matrix A21 = Matrix_(3,3,
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0);
Vector b2 = Vector_(3, 3.5, 3.5, 3.5);
Vector s2 = Vector_(3, 3.6, 3.6, 3.6);
GaussianFactorGraphOrdered gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
// Convert to Jacobians (inefficient copy of all factors instead of selectively converting only Hessians)
FactorGraphOrdered<JacobianFactorOrdered> jacobians;
BOOST_FOREACH(const GaussianFactorGraphOrdered::sharedFactor& factor, gfg) {
jacobians.push_back(boost::make_shared<JacobianFactorOrdered>(*factor));
}
// Combine Jacobians into a single dense factor
JacobianFactorOrdered actual = *CombineJacobians(jacobians, VariableSlots(gfg));
Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &zero3x3, &A10, &zero3x3);
Matrix A1 = gtsam::stack(3, &A01, &A11, &A21);
Vector b = gtsam::concatVectors(3, &b0, &b1, &b2);
Vector sigmas = gtsam::concatVectors(3, &s0, &s1, &s2);
JacobianFactorOrdered expected(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(GaussianFactorOrdered, CombineAndEliminate)
{
Matrix A01 = Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b0 = Vector_(3, 1.5, 1.5, 1.5);
Vector s0 = Vector_(3, 1.6, 1.6, 1.6);
Matrix A10 = Matrix_(3,3,
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 2.0);
Matrix A11 = Matrix_(3,3,
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0);
Vector b1 = Vector_(3, 2.5, 2.5, 2.5);
Vector s1 = Vector_(3, 2.6, 2.6, 2.6);
Matrix A21 = Matrix_(3,3,
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0);
Vector b2 = Vector_(3, 3.5, 3.5, 3.5);
Vector s2 = Vector_(3, 3.6, 3.6, 3.6);
GaussianFactorGraphOrdered gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Vector b = gtsam::concatVectors(3, &b1, &b0, &b2);
Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2);
JacobianFactorOrdered expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
GaussianConditionalOrdered::shared_ptr expectedBN = expectedFactor.eliminate(1);
GaussianConditionalOrdered::shared_ptr actualBN;
GaussianFactorOrdered::shared_ptr actualFactor;
boost::tie(actualBN, actualFactor) = EliminateQROrdered(gfg, 1);
JacobianFactorOrdered::shared_ptr actualJacobian = boost::dynamic_pointer_cast<
JacobianFactorOrdered>(actualFactor);
EXPECT(assert_equal(*expectedBN, *actualBN));
EXPECT(assert_equal(expectedFactor, *actualJacobian));
}
/* ************************************************************************* */
TEST(GaussianFactorOrdered, eliminateFrontals)
{
// Augmented Ab test case for whole factor graph
Matrix Ab = Matrix_(14,11,
4., 0., 1., 4., 1., 0., 3., 6., 8., 8., 1.,
9., 2., 0., 1., 6., 3., 9., 6., 6., 9., 4.,
5., 3., 7., 9., 5., 5., 9., 1., 3., 7., 0.,
5., 6., 5., 7., 9., 4., 0., 1., 1., 3., 5.,
0., 0., 4., 5., 6., 6., 7., 9., 4., 5., 4.,
0., 0., 9., 4., 8., 6., 2., 1., 4., 1., 6.,
0., 0., 6., 0., 4., 2., 4., 0., 1., 9., 6.,
0., 0., 6., 6., 4., 4., 5., 5., 5., 8., 6.,
0., 0., 0., 0., 8., 0., 9., 8., 2., 8., 0.,
0., 0., 0., 0., 0., 9., 4., 6., 3., 2., 0.,
0., 0., 0., 0., 1., 1., 9., 1., 5., 5., 3.,
0., 0., 0., 0., 1., 1., 3., 3., 2., 0., 5.,
0., 0., 0., 0., 0., 0., 0., 0., 2., 4., 6.,
0., 0., 0., 0., 0., 0., 0., 0., 6., 3., 4.);
// Create first factor (from pieces of Ab)
list<pair<Index, Matrix> > terms1;
terms1 +=
make_pair( 3, Matrix(Ab.block(0, 0, 4, 2))),
make_pair( 5, Matrix(Ab.block(0, 2, 4, 2))),
make_pair( 7, Matrix(Ab.block(0, 4, 4, 2))),
make_pair( 9, Matrix(Ab.block(0, 6, 4, 2))),
make_pair(11, Matrix(Ab.block(0, 8, 4, 2)));
Vector b1 = Ab.col(10).segment(0, 4);
JacobianFactorOrdered::shared_ptr factor1(new JacobianFactorOrdered(terms1, b1, noiseModel::Isotropic::Sigma(4, 0.5)));
// Create second factor
list<pair<Index, Matrix> > terms2;
terms2 +=
make_pair(5, Matrix(Ab.block(4, 2, 4, 2))),
make_pair(7, Matrix(Ab.block(4, 4, 4, 2))),
make_pair(9, Matrix(Ab.block(4, 6, 4, 2))),
make_pair(11,Matrix(Ab.block(4, 8, 4, 2)));
Vector b2 = Ab.col(10).segment(4, 4);
JacobianFactorOrdered::shared_ptr factor2(new JacobianFactorOrdered(terms2, b2, noiseModel::Isotropic::Sigma(4, 0.5)));
// Create third factor
list<pair<Index, Matrix> > terms3;
terms3 +=
make_pair(7, Matrix(Ab.block(8, 4, 4, 2))),
make_pair(9, Matrix(Ab.block(8, 6, 4, 2))),
make_pair(11,Matrix(Ab.block(8, 8, 4, 2)));
Vector b3 = Ab.col(10).segment(8, 4);
JacobianFactorOrdered::shared_ptr factor3(new JacobianFactorOrdered(terms3, b3, noiseModel::Isotropic::Sigma(4, 0.5)));
// Create fourth factor
list<pair<Index, Matrix> > terms4;
terms4 +=
make_pair(11, Matrix(Ab.block(12, 8, 2, 2)));
Vector b4 = Ab.col(10).segment(12, 2);
JacobianFactorOrdered::shared_ptr factor4(new JacobianFactorOrdered(terms4, b4, noiseModel::Isotropic::Sigma(2, 0.5)));
// Create factor graph
GaussianFactorGraphOrdered factors;
factors.push_back(factor1);
factors.push_back(factor2);
factors.push_back(factor3);
factors.push_back(factor4);
// extract the dense matrix for the graph
Matrix actualDense = factors.augmentedJacobian();
EXPECT(assert_equal(2.0 * Ab, actualDense));
// Convert to Jacobians, inefficient copy of all factors instead of selectively converting only Hessians
FactorGraphOrdered<JacobianFactorOrdered> jacobians;
BOOST_FOREACH(const GaussianFactorGraphOrdered::sharedFactor& factor, factors) {
jacobians.push_back(boost::make_shared<JacobianFactorOrdered>(*factor));
}
// Create combined factor
JacobianFactorOrdered combined(*CombineJacobians(jacobians, VariableSlots(factors)));
// Copies factors as they will be eliminated in place
JacobianFactorOrdered actualFactor_QR = combined;
JacobianFactorOrdered actualFactor_Chol = combined;
// Expected augmented matrix, both GaussianConditional (first 6 rows) and remaining factor (next 4 rows)
Matrix R = 2.0*Matrix_(11,11,
-12.1244, -5.1962, -5.2786, -8.6603, -10.5573, -5.9385, -11.3820, -7.2581, -8.7427, -13.4440, -5.3611,
0., 4.6904, 5.0254, 5.5432, 5.5737, 3.0153, -3.0153, -3.5635, -3.9290, -2.7412, 2.1625,
0., 0., -13.8160, -8.7166, -10.2245, -8.8666, -8.7632, -5.2544, -6.9192, -10.5537, -9.3250,
0., 0., 0., 6.5033, -1.1453, 1.3179, 2.5768, 5.5503, 3.6524, 1.3491, -2.5676,
0., 0., 0., 0., -9.6242, -2.1148, -9.3509, -10.5846, -3.5366, -6.8561, -3.2277,
0., 0., 0., 0., 0., 9.7887, 4.3551, 5.7572, 2.7876, 0.1611, 1.1769,
0., 0., 0., 0., 0., 0., -11.1139, -0.6521, -2.1943, -7.5529, -0.9081,
0., 0., 0., 0., 0., 0., 0., -4.6479, -1.9367, -6.5170, -3.7685,
0., 0., 0., 0., 0., 0., 0., 0., 8.2503, 3.3757, 6.8476,
0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -7.1635);
// Expected conditional on first variable from first 2 rows of R
Matrix R1 = sub(R, 0,2, 0,2);
list<pair<Index, Matrix> > cterms1;
cterms1 +=
make_pair(5, sub(R, 0,2, 2,4 )),
make_pair(7, sub(R, 0,2, 4,6 )),
make_pair(9, sub(R, 0,2, 6,8 )),
make_pair(11,sub(R, 0,2, 8,10));
Vector d1 = R.col(10).segment(0,2);
GaussianConditionalOrdered::shared_ptr cond1(new GaussianConditionalOrdered(3, d1, R1, cterms1, ones(2)));
// Expected conditional on second variable from next 2 rows of R
Matrix R2 = sub(R, 2,4, 2,4);
list<pair<Index, Matrix> > cterms2;
cterms2 +=
make_pair(7, sub(R, 2,4, 4,6)),
make_pair(9, sub(R, 2,4, 6,8)),
make_pair(11,sub(R, 2,4, 8,10));
Vector d2 = R.col(10).segment(2,2);
GaussianConditionalOrdered::shared_ptr cond2(new GaussianConditionalOrdered(5, d2, R2, cterms2, ones(2)));
// Expected conditional on third variable from next 2 rows of R
Matrix R3 = sub(R, 4,6, 4,6);
list<pair<Index, Matrix> > cterms3;
cterms3 +=
make_pair(9, sub(R, 4,6, 6,8)),
make_pair(11, sub(R, 4,6, 8,10));
Vector d3 = R.col(10).segment(4,2);
GaussianConditionalOrdered::shared_ptr cond3(new GaussianConditionalOrdered(7, d3, R3, cterms3, ones(2)));
// Create expected Bayes net fragment from three conditionals above
// GaussianBayesNetOrdered expectedFragment;
// expectedFragment.push_back(cond1);
// expectedFragment.push_back(cond2);
// expectedFragment.push_back(cond3);
Index ikeys[] = {3,5,7,9,11};
std::vector<Index> keys(ikeys, ikeys + sizeof(ikeys)/sizeof(Index));
size_t dims[] = { 2,2,2,2,2,1 };
size_t height = 11;
VerticalBlockView<Matrix> Rblock(R, dims, dims+6, height);
GaussianConditionalOrdered::shared_ptr expectedFragment( new GaussianConditionalOrdered(keys.begin(), keys.end(), 3,
Rblock, ones(6)) );
// Get expected matrices for remaining factor
Matrix Ae1 = sub(R, 6,10, 6,8);
Matrix Ae2 = sub(R, 6,10, 8,10);
Vector be = R.col(10).segment(6, 4);
// Eliminate (3 frontal variables, 6 scalar columns) using QR !!!!
GaussianConditionalOrdered::shared_ptr actualFragment_QR = actualFactor_QR.eliminate(3);
EXPECT(assert_equal(*expectedFragment, *actualFragment_QR, 0.001));
EXPECT(assert_equal(size_t(2), actualFactor_QR.keys().size()));
EXPECT(assert_equal(Index(9), actualFactor_QR.keys()[0]));
EXPECT(assert_equal(Index(11), actualFactor_QR.keys()[1]));
EXPECT(assert_equal(Ae1, actualFactor_QR.getA(actualFactor_QR.begin()), 0.001));
EXPECT(assert_equal(Ae2, actualFactor_QR.getA(actualFactor_QR.begin()+1), 0.001));
EXPECT(assert_equal(be, actualFactor_QR.getb(), 0.001));
EXPECT(assert_equal(ones(4), actualFactor_QR.get_model()->sigmas(), 0.001));
// Eliminate (3 frontal variables, 6 scalar columns) using Cholesky !!!!
#ifdef BROKEN
GaussianBayesNetOrdered actualFragment_Chol = *actualFactor_Chol.eliminate(3, JacobianFactorOrdered::SOLVE_CHOLESKY);
EXPECT(assert_equal(expectedFragment, actualFragment_Chol, 0.001));
EXPECT(assert_equal(size_t(2), actualFactor_Chol.keys().size()));
EXPECT(assert_equal(Index(9), actualFactor_Chol.keys()[0]));
EXPECT(assert_equal(Index(11), actualFactor_Chol.keys()[1]));
EXPECT(assert_equal(Ae1, actualFactor_Chol.getA(actualFactor_Chol.begin()), 0.001)); ////
EXPECT(linear_dependent(Ae2, actualFactor_Chol.getA(actualFactor_Chol.begin()+1), 0.001));
EXPECT(assert_equal(be, actualFactor_Chol.getb(), 0.001)); ////
EXPECT(assert_equal(ones(4), actualFactor_Chol.get_sigmas(), 0.001));
#endif
}
/* ************************************************************************* */
TEST(GaussianFactorOrdered, permuteWithInverse)
{
Matrix A1 = Matrix_(2,2,
1.0, 2.0,
3.0, 4.0);
Matrix A2 = Matrix_(2,1,
5.0,
6.0);
Matrix A3 = Matrix_(2,3,
7.0, 8.0, 9.0,
10.0, 11.0, 12.0);
Vector b = Vector_(2, 13.0, 14.0);
Permutation inversePermutation(6);
inversePermutation[0] = 5;
inversePermutation[1] = 4;
inversePermutation[2] = 3;
inversePermutation[3] = 2;
inversePermutation[4] = 1;
inversePermutation[5] = 0;
JacobianFactorOrdered actual(1, A1, 3, A2, 5, A3, b, noiseModel::Isotropic::Sigma(2, 1.0));
GaussianFactorGraphOrdered actualFG; actualFG.push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(actual)));
VariableIndexOrdered actualIndex(actualFG);
actual.permuteWithInverse(inversePermutation);
// actualIndex.permute(*inversePermutation.inverse());
JacobianFactorOrdered expected(4, A1, 2, A2, 0, A3, b, noiseModel::Isotropic::Sigma(2, 1.0));
GaussianFactorGraphOrdered expectedFG; expectedFG.push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(expected)));
// GaussianVariableIndex expectedIndex(expectedFG);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(GaussianFactorGraphOrdered, sparseJacobian) {
// Create factor graph:
// x1 x2 x3 x4 x5 b
// 1 2 3 0 0 4
// 5 6 7 0 0 8
// 9 10 0 11 12 13
// 0 0 0 14 15 16
// Expected - NOTE that we transpose this!
Matrix expected = Matrix_(16,3,
1., 1., 2.,
1., 2., 4.,
1., 3., 6.,
2., 1.,10.,
2., 2.,12.,
2., 3.,14.,
1., 6., 8.,
2., 6.,16.,
3., 1.,18.,
3., 2.,20.,
3., 4.,22.,
3., 5.,24.,
4., 4.,28.,
4., 5.,30.,
3., 6.,26.,
4., 6.,32.).transpose();
GaussianFactorGraphOrdered gfg;
SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), Vector_(2, 4., 8.), model);
gfg.add(0, Matrix_(2,3, 9.,10., 0., 0., 0., 0.), 1, Matrix_(2,2, 11., 12., 14., 15.), Vector_(2, 13.,16.), model);
Matrix actual = gfg.sparseJacobian_();
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(GaussianFactorGraphOrdered, matrices) {
// Create factor graph:
// x1 x2 x3 x4 x5 b
// 1 2 3 0 0 4
// 5 6 7 0 0 8
// 9 10 0 11 12 13
// 0 0 0 14 15 16
GaussianFactorGraphOrdered gfg;
SharedDiagonal model = noiseModel::Unit::Create(2);
gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), Vector_(2, 4., 8.), model);
gfg.add(0, Matrix_(2,3, 9.,10., 0., 0., 0., 0.), 1, Matrix_(2,2, 11., 12., 14., 15.), Vector_(2, 13.,16.), model);
Matrix jacobian(4,6);
jacobian <<
1, 2, 3, 0, 0, 4,
5, 6, 7, 0, 0, 8,
9,10, 0,11,12,13,
0, 0, 0,14,15,16;
Matrix expectedJacobian = jacobian;
Matrix expectedHessian = jacobian.transpose() * jacobian;
Matrix expectedA = jacobian.leftCols(jacobian.cols()-1);
Vector expectedb = jacobian.col(jacobian.cols()-1);
Matrix expectedL = expectedA.transpose() * expectedA;
Vector expectedeta = expectedA.transpose() * expectedb;
Matrix actualJacobian = gfg.augmentedJacobian();
Matrix actualHessian = gfg.augmentedHessian();
Matrix actualA; Vector actualb; boost::tie(actualA,actualb) = gfg.jacobian();
Matrix actualL; Vector actualeta; boost::tie(actualL,actualeta) = gfg.hessian();
EXPECT(assert_equal(expectedJacobian, actualJacobian));
EXPECT(assert_equal(expectedHessian, actualHessian));
EXPECT(assert_equal(expectedA, actualA));
EXPECT(assert_equal(expectedb, actualb));
EXPECT(assert_equal(expectedL, actualL));
EXPECT(assert_equal(expectedeta, actualeta));
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, gradient )
{
GaussianFactorGraphOrdered fg = createSimpleGaussianFactorGraph();
// Construct expected gradient
VectorValuesOrdered expected;
// 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2
// worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5]
expected.insert(1,Vector_(2, 5.0,-12.5));
expected.insert(2,Vector_(2, 30.0, 5.0));
expected.insert(0,Vector_(2,-25.0, 17.5));
// Check the gradient at delta=0
VectorValuesOrdered zero = VectorValuesOrdered::Zero(expected);
VectorValuesOrdered actual = gradient(fg, zero);
EXPECT(assert_equal(expected,actual));
// Check the gradient at the solution (should be zero)
VectorValuesOrdered solution = *GaussianSequentialSolver(fg).optimize();
VectorValuesOrdered actual2 = gradient(fg, solution);
EXPECT(assert_equal(VectorValuesOrdered::Zero(solution), actual2));
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, transposeMultiplication )
{
GaussianFactorGraphOrdered A = createSimpleGaussianFactorGraph();
VectorValuesOrdered e;
e.insert(0, Vector_(2, 0.0, 0.0));
e.insert(1, Vector_(2,15.0, 0.0));
e.insert(2, Vector_(2, 0.0,-5.0));
e.insert(3, Vector_(2,-7.5,-5.0));
VectorValuesOrdered expected;
expected.insert(1, Vector_(2, -37.5,-50.0));
expected.insert(2, Vector_(2,-150.0, 25.0));
expected.insert(0, Vector_(2, 187.5, 25.0));
VectorValuesOrdered actual = VectorValuesOrdered::SameStructure(expected);
transposeMultiply(A, e, actual);
EXPECT(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST(GaussianFactorGraphOrdered, eliminate_empty )
{
// eliminate an empty factor
GaussianFactorGraphOrdered gfg;
gfg.push_back(boost::make_shared<JacobianFactorOrdered>());
GaussianConditionalOrdered::shared_ptr actualCG;
GaussianFactorGraphOrdered remainingGFG;
boost::tie(actualCG, remainingGFG) = gfg.eliminateOne(0);
// expected Conditional Gaussian is just a parent-less node with P(x)=1
GaussianConditionalOrdered expectedCG(0, Vector(), Matrix(), Vector());
// expected remaining graph should be the same as the original, still empty :-)
GaussianFactorGraphOrdered expectedLF = gfg;
// check if the result matches
EXPECT(actualCG->equals(expectedCG));
EXPECT(remainingGFG.equals(expectedLF));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

309
gtsam/linear/tests/testGaussianJunctionTreeObsolete.cpp
View File

@ -1,309 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testGaussianJunctionTree.cpp
* @date Jul 8, 2010
* @author Kai Ni
*/
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/list_of.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
using namespace boost::assign;
#include <gtsam/base/debug.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/linear/GaussianJunctionTreeOrdered.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/inference/BayesTreeOrdered.h>
using namespace std;
using namespace gtsam;
static const Index x2=0, x1=1, x3=2, x4=3;
static GaussianFactorGraphOrdered createChain() {
typedef GaussianFactorGraphOrdered::sharedFactor Factor;
SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 0.5);
Factor factor1(new JacobianFactorOrdered(x2, Matrix_(1,1,1.), x1, Matrix_(1,1,1.), Vector_(1,1.), model));
Factor factor2(new JacobianFactorOrdered(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), Vector_(1,1.), model));
Factor factor3(new JacobianFactorOrdered(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), Vector_(1,1.), model));
Factor factor4(new JacobianFactorOrdered(x4, Matrix_(1,1,1.), Vector_(1,1.), model));
GaussianFactorGraphOrdered fg;
fg.push_back(factor1);
fg.push_back(factor2);
fg.push_back(factor3);
fg.push_back(factor4);
return fg;
}
/* ************************************************************************* */
/**
* x1 - x2 - x3 - x4
* x3 x4
* x2 x1 : x3
*
* x2 x1 x3 x4 b
* 1 1 1
* 1 1 1
* 1 1 1
* 1 1
*
* 1 0 0 1
*/
TEST( GaussianJunctionTreeOrdered, eliminate )
{
GaussianFactorGraphOrdered fg = createChain();
GaussianJunctionTreeOrdered junctionTree(fg);
BayesTreeOrdered<GaussianConditionalOrdered>::sharedClique rootClique = junctionTree.eliminate(&EliminateQROrdered);
typedef BayesTreeOrdered<GaussianConditionalOrdered>::sharedConditional sharedConditional;
Matrix two = Matrix_(1,1,2.);
Matrix one = Matrix_(1,1,1.);
BayesTreeOrdered<GaussianConditionalOrdered> bayesTree_expected;
Index keys_root[] = {x3,x4};
Matrix rsd_root = Matrix_(2,3, 2., 2., 2., 0., 2., 2.);
size_t dim_root[] = {1, 1, 1};
sharedConditional root_expected(new GaussianConditionalOrdered(keys_root, keys_root+2, 2,
VerticalBlockView<Matrix>(rsd_root, dim_root, dim_root+3, 2), ones(2)));
BayesTreeOrdered<GaussianConditionalOrdered>::sharedClique rootClique_expected(new BayesTreeOrdered<GaussianConditionalOrdered>::Clique(root_expected));
Index keys_child[] = {x2,x1,x3};
Matrix rsd_child = Matrix_(2,4, 2., 1., 1., 2., 0., -1., 1., 0.);
size_t dim_child[] = {1, 1, 1, 1};
sharedConditional child_expected(new GaussianConditionalOrdered(keys_child, keys_child+3, 2,
VerticalBlockView<Matrix>(rsd_child, dim_child, dim_child+4, 2), ones(2)));
BayesTreeOrdered<GaussianConditionalOrdered>::sharedClique childClique_expected(new BayesTreeOrdered<GaussianConditionalOrdered>::Clique(child_expected));
bayesTree_expected.insert(rootClique_expected);
bayesTree_expected.insert(childClique_expected);
// bayesTree_expected.insert(sharedConditional(new GaussianConditionalOrdered(x4, Vector_(1,2.), two, Vector_(1,1.))));
// bayesTree_expected.insert(sharedConditional(new GaussianConditionalOrdered(x3, Vector_(1,2.), two, x4, two, Vector_(1,1.))));
// bayesTree_expected.insert(sharedConditional(new GaussianConditionalOrdered(x1, Vector_(1,0.), one*(-1), x3, one, Vector_(1,1.))));
// bayesTree_expected.insert(sharedConditional(new GaussianConditionalOrdered(x2, Vector_(1,2.), two, x1, one, x3, one, Vector_(1,1.))));
CHECK(assert_equal(*bayesTree_expected.root(), *rootClique));
EXPECT(assert_equal(*(bayesTree_expected.root()->children().front()), *(rootClique->children().front())));
}
/* ************************************************************************* */
TEST( GaussianJunctionTreeOrdered, GBNConstructor )
{
GaussianFactorGraphOrdered fg = createChain();
GaussianJunctionTreeOrdered jt(fg);
BayesTreeOrdered<GaussianConditionalOrdered>::sharedClique root = jt.eliminate(&EliminateQROrdered);
BayesTreeOrdered<GaussianConditionalOrdered> expected;
expected.insert(root);
GaussianBayesNetOrdered bn(*GaussianSequentialSolver(fg).eliminate());
BayesTreeOrdered<GaussianConditionalOrdered> actual(bn);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( GaussianJunctionTreeOrdered, optimizeMultiFrontal )
{
GaussianFactorGraphOrdered fg = createChain();
GaussianJunctionTreeOrdered tree(fg);
VectorValuesOrdered actual = tree.optimize(&EliminateQROrdered);
VectorValuesOrdered expected(vector<size_t>(4,1));
expected[x1] = Vector_(1, 0.);
expected[x2] = Vector_(1, 1.);
expected[x3] = Vector_(1, 0.);
expected[x4] = Vector_(1, 1.);
EXPECT(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST(GaussianJunctionTreeOrdered, complicatedMarginal) {
// Create the conditionals to go in the BayesTree
GaussianConditionalOrdered::shared_ptr R_1_2(new GaussianConditionalOrdered(
pair_list_of
(1, (Matrix(3,1) <<
0.2630,
0,
0).finished())
(2, (Matrix(3,2) <<
0.7482, 0.2290,
0.4505, 0.9133,
0, 0.1524).finished())
(5, (Matrix(3,1) <<
0.8258,
0.5383,
0.9961).finished()),
2, (Vector(3) << 0.0782, 0.4427, 0.1067).finished(), ones(3)));
GaussianConditionalOrdered::shared_ptr R_3_4(new GaussianConditionalOrdered(
pair_list_of
(3, (Matrix(3,1) <<
0.0540,
0,
0).finished())
(4, (Matrix(3,2) <<
0.9340, 0.4694,
0.1299, 0.0119,
0, 0.3371).finished())
(6, (Matrix(3,2) <<
0.1622, 0.5285,
0.7943, 0.1656,
0.3112, 0.6020).finished()),
2, (Vector(3) << 0.9619, 0.0046, 0.7749).finished(), ones(3)));
// GaussianConditionalOrdered::shared_ptr R_5_6(new GaussianConditionalOrdered(
// pair_list_of
// (5, (Matrix(3,1) <<
// 0.2435,
// 0,
// 0).finished())
// (6, (Matrix(3,2) <<
// 0.4733, 0.1966,
// 0.9022, 0.0979,
// 0.0, 0.2312).finished()) // Attempted to recreate without permutation
// (7, (Matrix(3,1) <<
// 0.5853,
// 1.0589,
// 0.1487).finished())
// (8, (Matrix(3,2) <<
// 0.2858, 0.3804,
// 0.9893, 0.2912,
// 0.4035, 0.4933).finished()),
// 2, (Vector(3) << 0.8173, 0.4164, 0.7671).finished(), ones(3)));
GaussianConditionalOrdered::shared_ptr R_5_6(new GaussianConditionalOrdered(
pair_list_of
(5, (Matrix(3,1) <<
0.2435,
0,
0).finished())
(6, (Matrix(3,2) <<
0.4733, 0.1966,
0.3517, 0.2511,
0.8308, 0.0).finished()) // NOTE the non-upper-triangular form
// here since this test was written when we had column permutations
// from LDL. The code still works currently (does not enfore
// upper-triangularity in this case) but this test will need to be
// redone if this stops working in the future
(7, (Matrix(3,1) <<
0.5853,
0.5497,
0.9172).finished())
(8, (Matrix(3,2) <<
0.2858, 0.3804,
0.7572, 0.5678,
0.7537, 0.0759).finished()),
2, (Vector(3) << 0.8173, 0.8687, 0.0844).finished(), ones(3)));
GaussianConditionalOrdered::shared_ptr R_7_8(new GaussianConditionalOrdered(
pair_list_of
(7, (Matrix(3,1) <<
0.2551,
0,
0).finished())
(8, (Matrix(3,2) <<
0.8909, 0.1386,
0.9593, 0.1493,
0, 0.2575).finished())
(11, (Matrix(3,1) <<
0.8407,
0.2543,
0.8143).finished()),
2, (Vector(3) << 0.3998, 0.2599, 0.8001).finished(), ones(3)));
GaussianConditionalOrdered::shared_ptr R_9_10(new GaussianConditionalOrdered(
pair_list_of
(9, (Matrix(3,1) <<
0.7952,
0,
0).finished())
(10, (Matrix(3,2) <<
0.4456, 0.7547,
0.6463, 0.2760,
0, 0.6797).finished())
(11, (Matrix(3,1) <<
0.6551,
0.1626,
0.1190).finished())
(12, (Matrix(3,2) <<
0.4984, 0.5853,
0.9597, 0.2238,
0.3404, 0.7513).finished()),
2, (Vector(3) << 0.4314, 0.9106, 0.1818).finished(), ones(3)));
GaussianConditionalOrdered::shared_ptr R_11_12(new GaussianConditionalOrdered(
pair_list_of
(11, (Matrix(3,1) <<
0.0971,
0,
0).finished())
(12, (Matrix(3,2) <<
0.3171, 0.4387,
0.9502, 0.3816,
0, 0.7655).finished()),
2, (Vector(3) << 0.2638, 0.1455, 0.1361).finished(), ones(3)));
// Gaussian Bayes Tree
typedef BayesTreeOrdered<GaussianConditionalOrdered> GaussianBayesTree;
typedef GaussianBayesTree::Clique Clique;
typedef GaussianBayesTree::sharedClique sharedClique;
// Create Bayes Tree
GaussianBayesTree bt;
bt.insert(sharedClique(new Clique(R_11_12)));
bt.insert(sharedClique(new Clique(R_9_10)));
bt.insert(sharedClique(new Clique(R_7_8)));
bt.insert(sharedClique(new Clique(R_5_6)));
bt.insert(sharedClique(new Clique(R_3_4)));
bt.insert(sharedClique(new Clique(R_1_2)));
// Marginal on 5
Matrix expectedCov = (Matrix(1,1) << 236.5166).finished();
JacobianFactorOrdered::shared_ptr actualJacobianChol= boost::dynamic_pointer_cast<JacobianFactorOrdered>(
bt.marginalFactor(5, EliminateCholeskyOrdered));
JacobianFactorOrdered::shared_ptr actualJacobianQR = boost::dynamic_pointer_cast<JacobianFactorOrdered>(
bt.marginalFactor(5, EliminateQROrdered));
CHECK(assert_equal(*actualJacobianChol, *actualJacobianQR)); // Check that Chol and QR obtained marginals are the same
LONGS_EQUAL(1, actualJacobianChol->rows());
LONGS_EQUAL(1, actualJacobianChol->size());
LONGS_EQUAL(5, actualJacobianChol->keys()[0]);
Matrix actualA = actualJacobianChol->getA(actualJacobianChol->begin());
Matrix actualCov = inverse(actualA.transpose() * actualA);
EXPECT(assert_equal(expectedCov, actualCov, 1e-1));
// Marginal on 6
// expectedCov = (Matrix(2,2) <<
// 8471.2, 2886.2,
// 2886.2, 1015.8).finished();
expectedCov = (Matrix(2,2) <<
1015.8, 2886.2,
2886.2, 8471.2).finished();
actualJacobianChol = boost::dynamic_pointer_cast<JacobianFactorOrdered>(
bt.marginalFactor(6, EliminateCholeskyOrdered));
actualJacobianQR = boost::dynamic_pointer_cast<JacobianFactorOrdered>(
bt.marginalFactor(6, EliminateQROrdered));
CHECK(assert_equal(*actualJacobianChol, *actualJacobianQR)); // Check that Chol and QR obtained marginals are the same
LONGS_EQUAL(2, actualJacobianChol->rows());
LONGS_EQUAL(1, actualJacobianChol->size());
LONGS_EQUAL(6, actualJacobianChol->keys()[0]);
actualA = actualJacobianChol->getA(actualJacobianChol->begin());
actualCov = inverse(actualA.transpose() * actualA);
EXPECT(assert_equal(expectedCov, actualCov, 1e1));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

635
gtsam/linear/tests/testHessianFactorObsolete.cpp
View File

@ -1,635 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testCholeskyFactor.cpp
* @author Richard Roberts
* @date Dec 15, 2010
*/
#include <vector>
#include <utility>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std/map.hpp>
#include <gtsam/base/debug.h>
#include <gtsam/linear/HessianFactorOrdered.h>
#include <gtsam/linear/JacobianFactorOrdered.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace boost::assign;
using namespace gtsam;
const double tol = 1e-5;
/* ************************************************************************* */
TEST(HessianFactorOrdered, emptyConstructor) {
HessianFactorOrdered f;
DOUBLES_EQUAL(0.0, f.constantTerm(), 1e-9); // Constant term should be zero
EXPECT(assert_equal(Vector(), f.linearTerm())); // Linear term should be empty
EXPECT(assert_equal(zeros(1,1), f.info())); // Full matrix should be 1-by-1 zero matrix
DOUBLES_EQUAL(0.0, f.error(VectorValuesOrdered()), 1e-9); // Should have zero error
}
/* ************************************************************************* */
TEST(HessianFactorOrdered, ConversionConstructor) {
HessianFactorOrdered expected;
expected.keys_.push_back(0);
expected.keys_.push_back(1);
size_t dims[] = { 2, 4, 1 };
expected.info_.resize(dims, dims+3, false);
expected.matrix_ = Matrix_(7,7,
125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000,
0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000,
-25.0000, 0.0, 25.0000, 0.0, 0.0, 0.0, -5.0000,
0.0, -25.0000, 0.0, 25.0000, 0.0, 0.0, 7.5000,
-100.0000, 0.0, 0.0, 0.0, 100.0000, 0.0, -20.0000,
0.0, -100.0000, 0.0, 0.0, 0.0, 100.0000, 10.0000,
25.0000, -17.5000, -5.0000, 7.5000, -20.0000, 10.0000, 8.2500);
// sigmas
double sigma1 = 0.2;
double sigma2 = 0.1;
Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2);
// the combined linear factor
Matrix Ax2 = Matrix_(4,2,
// x2
-1., 0.,
+0.,-1.,
1., 0.,
+0.,1.
);
Matrix Al1x1 = Matrix_(4,4,
// l1 x1
1., 0., 0.00, 0., // f4
0., 1., 0.00, 0., // f4
0., 0., -1., 0., // f2
0., 0., 0.00,-1. // f2
);
// the RHS
Vector b2(4);
b2(0) = -0.2;
b2(1) = 0.3;
b2(2) = 0.2;
b2(3) = -0.1;
vector<pair<Index, Matrix> > meas;
meas.push_back(make_pair(0, Ax2));
meas.push_back(make_pair(1, Al1x1));
JacobianFactorOrdered combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));
HessianFactorOrdered actual(combined);
VectorValuesOrdered values(std::vector<size_t>(dims, dims+2));
values[0] = Vector_(2, 1.0, 2.0);
values[1] = Vector_(4, 3.0, 4.0, 5.0, 6.0);
EXPECT_LONGS_EQUAL(2, actual.size());
EXPECT(assert_equal(expected, actual, 1e-9));
// error terms
EXPECT_DOUBLES_EQUAL(combined.error(values), actual.error(values), 1e-9);
}
/* ************************************************************************* */
TEST(HessianFactorOrdered, Constructor1)
{
Matrix G = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0);
Vector g = Vector_(2, -8.0, -9.0);
double f = 10.0;
Vector dxv = Vector_(2, 1.5, 2.5);
vector<size_t> dims;
dims.push_back(2);
VectorValuesOrdered dx(dims);
dx[0] = dxv;
HessianFactorOrdered factor(0, G, g, f);
// extract underlying parts
Matrix info_matrix = factor.info_.range(0, 1, 0, 1);
EXPECT(assert_equal(Matrix(G), info_matrix));
EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10);
EXPECT(assert_equal(g, Vector(factor.linearTerm()), 1e-10));
EXPECT_LONGS_EQUAL(1, factor.size());
// error 0.5*(f - 2*x'*g + x'*G*x)
double expected = 80.375;
double actual = factor.error(dx);
double expected_manual = 0.5 * (f - 2.0 * dxv.dot(g) + dxv.transpose() * G.selfadjointView<Eigen::Upper>() * dxv);
EXPECT_DOUBLES_EQUAL(expected, expected_manual, 1e-10);
DOUBLES_EQUAL(expected, actual, 1e-10);
}
/* ************************************************************************* */
TEST(HessianFactorOrdered, Constructor1b)
{
Vector mu = Vector_(2,1.0,2.0);
Matrix Sigma = eye(2,2);
HessianFactorOrdered factor(0, mu, Sigma);
Matrix G = eye(2,2);
Vector g = G*mu;
double f = dot(g,mu);
// Check
Matrix info_matrix = factor.info_.range(0, 1, 0, 1);
EXPECT(assert_equal(Matrix(G), info_matrix));
EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10);
EXPECT(assert_equal(g, Vector(factor.linearTerm()), 1e-10));
EXPECT_LONGS_EQUAL(1, factor.size());
}
/* ************************************************************************* */
TEST(HessianFactorOrdered, Constructor2)
{
Matrix G11 = Matrix_(1,1, 1.0);
Matrix G12 = Matrix_(1,2, 2.0, 4.0);
Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0);
Vector g1 = Vector_(1, -7.0);
Vector g2 = Vector_(2, -8.0, -9.0);
double f = 10.0;
Vector dx0 = Vector_(1, 0.5);
Vector dx1 = Vector_(2, 1.5, 2.5);
vector<size_t> dims;
dims.push_back(1);
dims.push_back(2);
VectorValuesOrdered dx(dims);
dx[0] = dx0;
dx[1] = dx1;
HessianFactorOrdered factor(0, 1, G11, G12, g1, G22, g2, f);
double expected = 90.5;
double actual = factor.error(dx);
DOUBLES_EQUAL(expected, actual, 1e-10);
LONGS_EQUAL(4, factor.rows());
DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10);
Vector linearExpected(3); linearExpected << g1, g2;
EXPECT(assert_equal(linearExpected, factor.linearTerm()));
EXPECT(assert_equal(G11, factor.info(factor.begin(), factor.begin())));
EXPECT(assert_equal(G12, factor.info(factor.begin(), factor.begin()+1)));
EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1)));
// Check case when vector values is larger than factor
dims.push_back(2);
VectorValuesOrdered dxLarge(dims);
dxLarge[0] = dx0;
dxLarge[1] = dx1;
dxLarge[2] = Vector_(2, 0.1, 0.2);
EXPECT_DOUBLES_EQUAL(expected, factor.error(dxLarge), 1e-10);
}
/* ************************************************************************* */
TEST(HessianFactorOrdered, Constructor3)
{
Matrix G11 = Matrix_(1,1, 1.0);
Matrix G12 = Matrix_(1,2, 2.0, 4.0);
Matrix G13 = Matrix_(1,3, 3.0, 6.0, 9.0);
Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0);
Matrix G23 = Matrix_(2,3, 4.0, 6.0, 8.0, 1.0, 2.0, 4.0);
Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0);
Vector g1 = Vector_(1, -7.0);
Vector g2 = Vector_(2, -8.0, -9.0);
Vector g3 = Vector_(3, 1.0, 2.0, 3.0);
double f = 10.0;
Vector dx0 = Vector_(1, 0.5);
Vector dx1 = Vector_(2, 1.5, 2.5);
Vector dx2 = Vector_(3, 1.5, 2.5, 3.5);
vector<size_t> dims;
dims.push_back(1);
dims.push_back(2);
dims.push_back(3);
VectorValuesOrdered dx(dims);
dx[0] = dx0;
dx[1] = dx1;
dx[2] = dx2;
HessianFactorOrdered factor(0, 1, 2, G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
double expected = 371.3750;
double actual = factor.error(dx);
DOUBLES_EQUAL(expected, actual, 1e-10);
LONGS_EQUAL(7, factor.rows());
DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10);
Vector linearExpected(6); linearExpected << g1, g2, g3;
EXPECT(assert_equal(linearExpected, factor.linearTerm()));
EXPECT(assert_equal(G11, factor.info(factor.begin()+0, factor.begin()+0)));
EXPECT(assert_equal(G12, factor.info(factor.begin()+0, factor.begin()+1)));
EXPECT(assert_equal(G13, factor.info(factor.begin()+0, factor.begin()+2)));
EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1)));
EXPECT(assert_equal(G23, factor.info(factor.begin()+1, factor.begin()+2)));
EXPECT(assert_equal(G33, factor.info(factor.begin()+2, factor.begin()+2)));
}
/* ************************************************************************* */
TEST(HessianFactorOrdered, ConstructorNWay)
{
Matrix G11 = Matrix_(1,1, 1.0);
Matrix G12 = Matrix_(1,2, 2.0, 4.0);
Matrix G13 = Matrix_(1,3, 3.0, 6.0, 9.0);
Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0);
Matrix G23 = Matrix_(2,3, 4.0, 6.0, 8.0, 1.0, 2.0, 4.0);
Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0);
Vector g1 = Vector_(1, -7.0);
Vector g2 = Vector_(2, -8.0, -9.0);
Vector g3 = Vector_(3, 1.0, 2.0, 3.0);
double f = 10.0;
Vector dx0 = Vector_(1, 0.5);
Vector dx1 = Vector_(2, 1.5, 2.5);
Vector dx2 = Vector_(3, 1.5, 2.5, 3.5);
vector<size_t> dims;
dims.push_back(1);
dims.push_back(2);
dims.push_back(3);
VectorValuesOrdered dx(dims);
dx[0] = dx0;
dx[1] = dx1;
dx[2] = dx2;
std::vector<Index> js;
js.push_back(0); js.push_back(1); js.push_back(2);
std::vector<Matrix> Gs;
Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33);
std::vector<Vector> gs;
gs.push_back(g1); gs.push_back(g2); gs.push_back(g3);
HessianFactorOrdered factor(js, Gs, gs, f);
double expected = 371.3750;
double actual = factor.error(dx);
DOUBLES_EQUAL(expected, actual, 1e-10);
LONGS_EQUAL(7, factor.rows());
DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10);
Vector linearExpected(6); linearExpected << g1, g2, g3;
EXPECT(assert_equal(linearExpected, factor.linearTerm()));
EXPECT(assert_equal(G11, factor.info(factor.begin()+0, factor.begin()+0)));
EXPECT(assert_equal(G12, factor.info(factor.begin()+0, factor.begin()+1)));
EXPECT(assert_equal(G13, factor.info(factor.begin()+0, factor.begin()+2)));
EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1)));
EXPECT(assert_equal(G23, factor.info(factor.begin()+1, factor.begin()+2)));
EXPECT(assert_equal(G33, factor.info(factor.begin()+2, factor.begin()+2)));
}
/* ************************************************************************* */
TEST_UNSAFE(HessianFactorOrdered, CopyConstructor_and_assignment)
{
Matrix G11 = Matrix_(1,1, 1.0);
Matrix G12 = Matrix_(1,2, 2.0, 4.0);
Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0);
Vector g1 = Vector_(1, -7.0);
Vector g2 = Vector_(2, -8.0, -9.0);
double f = 10.0;
Vector dx0 = Vector_(1, 0.5);
Vector dx1 = Vector_(2, 1.5, 2.5);
vector<size_t> dims;
dims.push_back(1);
dims.push_back(2);
VectorValuesOrdered dx(dims);
dx[0] = dx0;
dx[1] = dx1;
HessianFactorOrdered originalFactor(0, 1, G11, G12, g1, G22, g2, f);
// Make a copy
HessianFactorOrdered copy1(originalFactor);
double expected = 90.5;
double actual = copy1.error(dx);
DOUBLES_EQUAL(expected, actual, 1e-10);
LONGS_EQUAL(4, copy1.rows());
DOUBLES_EQUAL(10.0, copy1.constantTerm(), 1e-10);
Vector linearExpected(3); linearExpected << g1, g2;
EXPECT(assert_equal(linearExpected, copy1.linearTerm()));
EXPECT(assert_equal(G11, copy1.info(copy1.begin(), copy1.begin())));
EXPECT(assert_equal(G12, copy1.info(copy1.begin(), copy1.begin()+1)));
EXPECT(assert_equal(G22, copy1.info(copy1.begin()+1, copy1.begin()+1)));
// Make a copy using the assignment operator
HessianFactorOrdered copy2;
copy2 = HessianFactorOrdered(originalFactor); // Make a temporary to make sure copying does not shared references
actual = copy2.error(dx);
DOUBLES_EQUAL(expected, actual, 1e-10);
LONGS_EQUAL(4, copy2.rows());
DOUBLES_EQUAL(10.0, copy2.constantTerm(), 1e-10);
EXPECT(assert_equal(linearExpected, copy2.linearTerm()));
EXPECT(assert_equal(G11, copy2.info(copy2.begin(), copy2.begin())));
EXPECT(assert_equal(G12, copy2.info(copy2.begin(), copy2.begin()+1)));
EXPECT(assert_equal(G22, copy2.info(copy2.begin()+1, copy2.begin()+1)));
}
/* ************************************************************************* */
TEST_UNSAFE(HessianFactorOrdered, CombineAndEliminate)
{
Matrix A01 = Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b0 = Vector_(3, 1.5, 1.5, 1.5);
Vector s0 = Vector_(3, 1.6, 1.6, 1.6);
Matrix A10 = Matrix_(3,3,
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 2.0);
Matrix A11 = Matrix_(3,3,
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0);
Vector b1 = Vector_(3, 2.5, 2.5, 2.5);
Vector s1 = Vector_(3, 2.6, 2.6, 2.6);
Matrix A21 = Matrix_(3,3,
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0);
Vector b2 = Vector_(3, 3.5, 3.5, 3.5);
Vector s2 = Vector_(3, 3.6, 3.6, 3.6);
GaussianFactorGraphOrdered gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Vector b = gtsam::concatVectors(3, &b1, &b0, &b2);
Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2);
// create a full, uneliminated version of the factor
JacobianFactorOrdered expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
// perform elimination
GaussianConditionalOrdered::shared_ptr expectedBN = expectedFactor.eliminate(1);
// create expected Hessian after elimination
HessianFactorOrdered expectedCholeskyFactor(expectedFactor);
// Convert all factors to hessians
FactorGraphOrdered<HessianFactorOrdered> hessians;
BOOST_FOREACH(const GaussianFactorGraphOrdered::sharedFactor& factor, gfg) {
if(boost::shared_ptr<HessianFactorOrdered> hf = boost::dynamic_pointer_cast<HessianFactorOrdered>(factor))
hessians.push_back(hf);
else if(boost::shared_ptr<JacobianFactorOrdered> jf = boost::dynamic_pointer_cast<JacobianFactorOrdered>(factor))
hessians.push_back(boost::make_shared<HessianFactorOrdered>(*jf));
else
CHECK(false);
}
// Eliminate
GaussianFactorGraphOrdered::EliminationResult actualCholesky = EliminateCholeskyOrdered(gfg, 1);
HessianFactorOrdered::shared_ptr actualFactor = boost::dynamic_pointer_cast<HessianFactorOrdered>(actualCholesky.second);
EXPECT(assert_equal(*expectedBN, *actualCholesky.first, 1e-6));
EXPECT(assert_equal(expectedCholeskyFactor, *actualFactor, 1e-6));
}
/* ************************************************************************* */
TEST(HessianFactorOrdered, eliminate2 )
{
// sigmas
double sigma1 = 0.2;
double sigma2 = 0.1;
Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2);
// the combined linear factor
Matrix Ax2 = Matrix_(4,2,
// x2
-1., 0.,
+0.,-1.,
1., 0.,
+0.,1.
);
Matrix Al1x1 = Matrix_(4,4,
// l1 x1
1., 0., 0.00, 0., // f4
0., 1., 0.00, 0., // f4
0., 0., -1., 0., // f2
0., 0., 0.00,-1. // f2
);
// the RHS
Vector b2(4);
b2(0) = -0.2;
b2(1) = 0.3;
b2(2) = 0.2;
b2(3) = -0.1;
vector<pair<Index, Matrix> > meas;
meas.push_back(make_pair(0, Ax2));
meas.push_back(make_pair(1, Al1x1));
JacobianFactorOrdered combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));
// eliminate the combined factor
HessianFactorOrdered::shared_ptr combinedLF_Chol(new HessianFactorOrdered(combined));
FactorGraphOrdered<HessianFactorOrdered> combinedLFG_Chol;
combinedLFG_Chol.push_back(combinedLF_Chol);
GaussianFactorGraphOrdered::EliminationResult actual_Chol = EliminateCholeskyOrdered(
combinedLFG_Chol, 1);
HessianFactorOrdered::shared_ptr actualFactor = boost::dynamic_pointer_cast<
HessianFactorOrdered>(actual_Chol.second);
// create expected Conditional Gaussian
double oldSigma = 0.0894427; // from when R was made unit
Matrix R11 = Matrix_(2,2,
1.00, 0.00,
0.00, 1.00
)/oldSigma;
Matrix S12 = Matrix_(2,4,
-0.20, 0.00,-0.80, 0.00,
+0.00,-0.20,+0.00,-0.80
)/oldSigma;
Vector d = Vector_(2,0.2,-0.14)/oldSigma;
GaussianConditionalOrdered expectedCG(0,d,R11,1,S12,ones(2));
EXPECT(assert_equal(expectedCG,*actual_Chol.first,1e-4));
// the expected linear factor
double sigma = 0.2236;
Matrix Bl1x1 = Matrix_(2,4,
// l1 x1
1.00, 0.00, -1.00, 0.00,
0.00, 1.00, +0.00, -1.00
)/sigma;
Vector b1 = Vector_(2,0.0,0.894427);
JacobianFactorOrdered expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0));
EXPECT(assert_equal(HessianFactorOrdered(expectedLF), *actualFactor, 1.5e-3));
}
/* ************************************************************************* */
TEST(HessianFactorOrdered, eliminateUnsorted) {
JacobianFactorOrdered::shared_ptr factor1(
new JacobianFactorOrdered(0,
Matrix_(3,3,
44.7214, 0.0, 0.0,
0.0, 44.7214, 0.0,
0.0, 0.0, 44.7214),
1,
Matrix_(3,3,
-0.179168, -44.721, 0.717294,
44.721, -0.179168, -44.9138,
0.0, 0.0, -44.7214),
Vector_(3, 1.98916e-17, -4.96503e-15, -7.75792e-17),
noiseModel::Unit::Create(3)));
HessianFactorOrdered::shared_ptr unsorted_factor2(
new HessianFactorOrdered(JacobianFactorOrdered(0,
Matrix_(6,3,
25.8367, 0.1211, 0.0593,
0.0, 23.4099, 30.8733,
0.0, 0.0, 25.8729,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0),
1,
Matrix_(6,3,
25.7429, -1.6897, 0.4587,
1.6400, 23.3095, -8.4816,
0.0034, 0.0509, -25.7855,
0.9997, -0.0002, 0.0824,
0.0, 0.9973, 0.9517,
0.0, 0.0, 0.9973),
Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
noiseModel::Unit::Create(6))));
Permutation permutation(2);
permutation[0] = 1;
permutation[1] = 0;
unsorted_factor2->permuteWithInverse(permutation);
HessianFactorOrdered::shared_ptr sorted_factor2(
new HessianFactorOrdered(JacobianFactorOrdered(0,
Matrix_(6,3,
25.7429, -1.6897, 0.4587,
1.6400, 23.3095, -8.4816,
0.0034, 0.0509, -25.7855,
0.9997, -0.0002, 0.0824,
0.0, 0.9973, 0.9517,
0.0, 0.0, 0.9973),
1,
Matrix_(6,3,
25.8367, 0.1211, 0.0593,
0.0, 23.4099, 30.8733,
0.0, 0.0, 25.8729,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0),
Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
noiseModel::Unit::Create(6))));
GaussianFactorGraphOrdered sortedGraph;
// sortedGraph.push_back(factor1);
sortedGraph.push_back(sorted_factor2);
GaussianFactorGraphOrdered unsortedGraph;
// unsortedGraph.push_back(factor1);
unsortedGraph.push_back(unsorted_factor2);
GaussianConditionalOrdered::shared_ptr expected_bn;
GaussianFactorOrdered::shared_ptr expected_factor;
boost::tie(expected_bn, expected_factor) =
EliminatePreferCholeskyOrdered(sortedGraph, 1);
GaussianConditionalOrdered::shared_ptr actual_bn;
GaussianFactorOrdered::shared_ptr actual_factor;
boost::tie(actual_bn, actual_factor) =
EliminatePreferCholeskyOrdered(unsortedGraph, 1);
EXPECT(assert_equal(*expected_bn, *actual_bn, 1e-10));
EXPECT(assert_equal(*expected_factor, *actual_factor, 1e-10));
}
/* ************************************************************************* */
TEST(HessianFactorOrdered, combine) {
// update the information matrix with a single jacobian factor
Matrix A0 = Matrix_(2, 2,
11.1803399, 0.0,
0.0, 11.1803399);
Matrix A1 = Matrix_(2, 2,
-2.23606798, 0.0,
0.0, -2.23606798);
Matrix A2 = Matrix_(2, 2,
-8.94427191, 0.0,
0.0, -8.94427191);
Vector b = Vector_(2, 2.23606798,-1.56524758);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2));
GaussianFactorOrdered::shared_ptr f(new JacobianFactorOrdered(0, A0, 1, A1, 2, A2, b, model));
FactorGraphOrdered<GaussianFactorOrdered> factors;
factors.push_back(f);
// Form Ab' * Ab
HessianFactorOrdered actual(factors);
Matrix expected = Matrix_(7, 7,
125.0000, 0.0, -25.0000, 0.0, -100.0000, 0.0, 25.0000,
0.0, 125.0000, 0.0, -25.0000, 0.0, -100.0000, -17.5000,
-25.0000, 0.0, 5.0000, 0.0, 20.0000, 0.0, -5.0000,
0.0, -25.0000, 0.0, 5.0000, 0.0, 20.0000, 3.5000,
-100.0000, 0.0, 20.0000, 0.0, 80.0000, 0.0, -20.0000,
0.0, -100.0000, 0.0, 20.0000, 0.0, 80.0000, 14.0000,
25.0000, -17.5000, -5.0000, 3.5000, -20.0000, 14.0000, 7.4500);
EXPECT(assert_equal(Matrix(expected.triangularView<Eigen::Upper>()),
Matrix(actual.matrix_.triangularView<Eigen::Upper>()), tol));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

468
gtsam/linear/tests/testJacobianFactorObsolete.cpp
View File

@ -1,468 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testJacobianFactor.cpp
* @brief Unit tests for Linear Factor
* @author Christian Potthast
* @author Frank Dellaert
**/
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/linear/JacobianFactorOrdered.h>
#include <gtsam/linear/HessianFactorOrdered.h>
#include <gtsam/linear/GaussianConditionalOrdered.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
using namespace std;
using namespace gtsam;
static const Index _x0_=0, _x1_=1, _x2_=2, _x_=5, _y_=6, _l11_=8;
static SharedDiagonal constraintModel = noiseModel::Constrained::All(2);
/* ************************************************************************* */
TEST(JacobianFactorOrdered, constructor)
{
Vector b = Vector_(3, 1., 2., 3.);
SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3,1.,1.,1.));
std::list<std::pair<Index, Matrix> > terms;
terms.push_back(make_pair(_x0_, eye(3)));
terms.push_back(make_pair(_x1_, 2.*eye(3)));
JacobianFactorOrdered actual(terms, b, noise);
JacobianFactorOrdered expected(_x0_, eye(3), _x1_, 2.*eye(3), b, noise);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(JacobianFactorOrdered, constructor2)
{
Vector b = Vector_(3, 1., 2., 3.);
SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3,1.,1.,1.));
std::list<std::pair<Index, Matrix> > terms;
terms.push_back(make_pair(_x0_, eye(3)));
terms.push_back(make_pair(_x1_, 2.*eye(3)));
const JacobianFactorOrdered actual(terms, b, noise);
JacobianFactorOrdered::const_iterator key0 = actual.begin();
JacobianFactorOrdered::const_iterator key1 = key0 + 1;
EXPECT(assert_equal(*key0, _x0_));
EXPECT(assert_equal(*key1, _x1_));
EXPECT(!actual.empty());
EXPECT_LONGS_EQUAL(3, actual.Ab_.nBlocks());
Matrix actualA0 = actual.getA(key0);
Matrix actualA1 = actual.getA(key1);
Vector actualb = actual.getb();
EXPECT(assert_equal(eye(3), actualA0));
EXPECT(assert_equal(2.*eye(3), actualA1));
EXPECT(assert_equal(b, actualb));
}
/* ************************************************************************* */
JacobianFactorOrdered construct() {
Matrix A = Matrix_(2,2, 1.,2.,3.,4.);
Vector b = Vector_(2, 1.0, 2.0);
SharedDiagonal s = noiseModel::Diagonal::Sigmas(Vector_(2, 3.0, 4.0));
JacobianFactorOrdered::shared_ptr shared(
new JacobianFactorOrdered(0, A, b, s));
return *shared;
}
TEST(JacobianFactorOrdered, return_value)
{
Matrix A = Matrix_(2,2, 1.,2.,3.,4.);
Vector b = Vector_(2, 1.0, 2.0);
SharedDiagonal s = noiseModel::Diagonal::Sigmas(Vector_(2, 3.0, 4.0));
JacobianFactorOrdered copied = construct();
EXPECT(assert_equal(b, copied.getb()));
EXPECT(assert_equal(*s, *copied.get_model()));
EXPECT(assert_equal(A, copied.getA(copied.begin())));
}
/* ************************************************************************* */
TEST(JabobianFactor, Hessian_conversion) {
HessianFactorOrdered hessian(0, (Matrix(4,4) <<
1.57, 2.695, -1.1, -2.35,
2.695, 11.3125, -0.65, -10.225,
-1.1, -0.65, 1, 0.5,
-2.35, -10.225, 0.5, 9.25).finished(),
(Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(),
73.1725);
JacobianFactorOrdered expected(0, (Matrix(2,4) <<
1.2530, 2.1508, -0.8779, -1.8755,
0, 2.5858, 0.4789, -2.3943).finished(),
(Vector(2) << -6.2929, -5.7941).finished(),
noiseModel::Unit::Create(2));
JacobianFactorOrdered actual(hessian);
EXPECT(assert_equal(expected, actual, 1e-3));
}
/* ************************************************************************* */
TEST( JacobianFactorOrdered, constructor_combined){
double sigma1 = 0.0957;
Matrix A11(2,2);
A11(0,0) = 1; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = 1;
Vector b(2);
b(0) = 2; b(1) = -1;
JacobianFactorOrdered::shared_ptr f1(new JacobianFactorOrdered(0, A11, b*sigma1, noiseModel::Isotropic::Sigma(2,sigma1)));
double sigma2 = 0.5;
A11(0,0) = 1; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = -1;
b(0) = 4 ; b(1) = -5;
JacobianFactorOrdered::shared_ptr f2(new JacobianFactorOrdered(0, A11, b*sigma2, noiseModel::Isotropic::Sigma(2,sigma2)));
double sigma3 = 0.25;
A11(0,0) = 1; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = -1;
b(0) = 3 ; b(1) = -88;
JacobianFactorOrdered::shared_ptr f3(new JacobianFactorOrdered(0, A11, b*sigma3, noiseModel::Isotropic::Sigma(2,sigma3)));
// TODO: find a real sigma value for this example
double sigma4 = 0.1;
A11(0,0) = 6; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = 7;
b(0) = 5 ; b(1) = -6;
JacobianFactorOrdered::shared_ptr f4(new JacobianFactorOrdered(0, A11*sigma4, b*sigma4, noiseModel::Isotropic::Sigma(2,sigma4)));
GaussianFactorGraphOrdered lfg;
lfg.push_back(f1);
lfg.push_back(f2);
lfg.push_back(f3);
lfg.push_back(f4);
JacobianFactorOrdered combined(lfg);
Vector sigmas = Vector_(8, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3, sigma4, sigma4);
Matrix A22(8,2);
A22(0,0) = 1; A22(0,1) = 0;
A22(1,0) = 0; A22(1,1) = 1;
A22(2,0) = 1; A22(2,1) = 0;
A22(3,0) = 0; A22(3,1) = -1;
A22(4,0) = 1; A22(4,1) = 0;
A22(5,0) = 0; A22(5,1) = -1;
A22(6,0) = 0.6; A22(6,1) = 0;
A22(7,0) = 0; A22(7,1) = 0.7;
Vector exb(8);
exb(0) = 2*sigma1 ; exb(1) = -1*sigma1; exb(2) = 4*sigma2 ; exb(3) = -5*sigma2;
exb(4) = 3*sigma3 ; exb(5) = -88*sigma3; exb(6) = 5*sigma4 ; exb(7) = -6*sigma4;
vector<pair<Index, Matrix> > meas;
meas.push_back(make_pair(0, A22));
JacobianFactorOrdered expected(meas, exb, noiseModel::Diagonal::Sigmas(sigmas));
EXPECT(assert_equal(expected,combined));
}
/* ************************************************************************* */
TEST(JacobianFactorOrdered, linearFactorN){
Matrix I = eye(2);
GaussianFactorGraphOrdered f;
SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1.0);
f.push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(0, I, Vector_(2, 10.0, 5.0), model)));
f.push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(0, -10 * I, 1, 10 * I, Vector_(2, 1.0, -2.0), model)));
f.push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(1, -10 * I, 2, 10 * I, Vector_(2, 1.5, -1.5), model)));
f.push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(2, -10 * I, 3, 10 * I, Vector_(2, 2.0, -1.0), model)));
JacobianFactorOrdered combinedFactor(f);
vector<pair<Index, Matrix> > combinedMeasurement;
combinedMeasurement.push_back(make_pair(0, Matrix_(8,2,
1.0, 0.0,
0.0, 1.0,
-10.0, 0.0,
0.0,-10.0,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0)));
combinedMeasurement.push_back(make_pair(1, Matrix_(8,2,
0.0, 0.0,
0.0, 0.0,
10.0, 0.0,
0.0, 10.0,
-10.0, 0.0,
0.0,-10.0,
0.0, 0.0,
0.0, 0.0)));
combinedMeasurement.push_back(make_pair(2, Matrix_(8,2,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
10.0, 0.0,
0.0, 10.0,
-10.0, 0.0,
0.0,-10.0)));
combinedMeasurement.push_back(make_pair(3, Matrix_(8,2,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
10.0, 0.0,
0.0,10.0)));
Vector b = Vector_(8,
10.0, 5.0, 1.0, -2.0, 1.5, -1.5, 2.0, -1.0);
Vector sigmas = repeat(8,1.0);
JacobianFactorOrdered expected(combinedMeasurement, b, noiseModel::Diagonal::Sigmas(sigmas));
EXPECT(assert_equal(expected,combinedFactor));
}
/* ************************************************************************* */
TEST(JacobianFactorOrdered, error) {
Vector b = Vector_(3, 1., 2., 3.);
SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3,2.,2.,2.));
std::list<std::pair<Index, Matrix> > terms;
terms.push_back(make_pair(_x0_, eye(3)));
terms.push_back(make_pair(_x1_, 2.*eye(3)));
const JacobianFactorOrdered jf(terms, b, noise);
VectorValuesOrdered values(2, 3);
values[0] = Vector_(3, 1.,2.,3.);
values[1] = Vector_(3, 4.,5.,6.);
Vector expected_unwhitened = Vector_(3, 8., 10., 12.);
Vector actual_unwhitened = jf.unweighted_error(values);
EXPECT(assert_equal(expected_unwhitened, actual_unwhitened));
Vector expected_whitened = Vector_(3, 4., 5., 6.);
Vector actual_whitened = jf.error_vector(values);
EXPECT(assert_equal(expected_whitened, actual_whitened));
// check behavior when there are more values than in this factor
VectorValuesOrdered largeValues(3, 3);
largeValues[0] = Vector_(3, 1.,2.,3.);
largeValues[1] = Vector_(3, 4.,5.,6.);
largeValues[2] = Vector_(3, 7.,8.,9.);
EXPECT(assert_equal(expected_unwhitened, jf.unweighted_error(largeValues)));
EXPECT(assert_equal(expected_whitened, jf.error_vector(largeValues)));
}
/* ************************************************************************* */
TEST(JacobianFactorOrdered, operators )
{
SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
Matrix I = eye(2);
Vector b = Vector_(2,0.2,-0.1);
JacobianFactorOrdered lf(_x1_, -I, _x2_, I, b, sigma0_1);
VectorValuesOrdered c;
c.insert(_x1_, Vector_(2,10.,20.));
c.insert(_x2_, Vector_(2,30.,60.));
// test A*x
Vector expectedE = Vector_(2,200.,400.), e = lf*c;
EXPECT(assert_equal(expectedE,e));
// test A^e
VectorValuesOrdered expectedX;
expectedX.insert(_x1_, Vector_(2,-2000.,-4000.));
expectedX.insert(_x2_, Vector_(2, 2000., 4000.));
VectorValuesOrdered actualX = VectorValuesOrdered::Zero(expectedX);
lf.transposeMultiplyAdd(1.0, e, actualX);
EXPECT(assert_equal(expectedX, actualX));
}
/* ************************************************************************* */
TEST(JacobianFactorOrdered, eliminate2 )
{
// sigmas
double sigma1 = 0.2;
double sigma2 = 0.1;
Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2);
// the combined linear factor
Matrix Ax2 = Matrix_(4,2,
// x2
-1., 0.,
+0.,-1.,
1., 0.,
+0.,1.
);
Matrix Al1x1 = Matrix_(4,4,
// l1 x1
1., 0., 0.00, 0., // f4
0., 1., 0.00, 0., // f4
0., 0., -1., 0., // f2
0., 0., 0.00,-1. // f2
);
// the RHS
Vector b2(4);
b2(0) = -0.2;
b2(1) = 0.3;
b2(2) = 0.2;
b2(3) = -0.1;
vector<pair<Index, Matrix> > meas;
meas.push_back(make_pair(_x2_, Ax2));
meas.push_back(make_pair(_l11_, Al1x1));
JacobianFactorOrdered combined(meas, b2, noiseModel::Diagonal::Sigmas(sigmas));
// eliminate the combined factor
GaussianConditionalOrdered::shared_ptr actualCG_QR;
JacobianFactorOrdered::shared_ptr actualLF_QR(new JacobianFactorOrdered(combined));
actualCG_QR = actualLF_QR->eliminateFirst();
// create expected Conditional Gaussian
double oldSigma = 0.0894427; // from when R was made unit
Matrix R11 = Matrix_(2,2,
1.00, 0.00,
0.00, 1.00
)/oldSigma;
Matrix S12 = Matrix_(2,4,
-0.20, 0.00,-0.80, 0.00,
+0.00,-0.20,+0.00,-0.80
)/oldSigma;
Vector d = Vector_(2,0.2,-0.14)/oldSigma;
GaussianConditionalOrdered expectedCG(_x2_,d,R11,_l11_,S12,ones(2));
EXPECT_LONGS_EQUAL(0, actualCG_QR->rsd().firstBlock());
EXPECT_LONGS_EQUAL(0, actualCG_QR->rsd().rowStart());
EXPECT_LONGS_EQUAL(2, actualCG_QR->rsd().rowEnd());
EXPECT_LONGS_EQUAL(3, actualCG_QR->rsd().nBlocks());
EXPECT(assert_equal(expectedCG,*actualCG_QR,1e-4));
// the expected linear factor
double sigma = 0.2236;
Matrix Bl1x1 = Matrix_(2,4,
// l1 x1
1.00, 0.00, -1.00, 0.00,
0.00, 1.00, +0.00, -1.00
)/sigma;
Vector b1 = Vector_(2,0.0,0.894427);
JacobianFactorOrdered expectedLF(_l11_, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0));
EXPECT(assert_equal(expectedLF,*actualLF_QR,1e-3));
}
/* ************************************************************************* */
TEST(JacobianFactorOrdered, default_error )
{
JacobianFactorOrdered f;
vector<size_t> dims;
VectorValuesOrdered c(dims);
double actual = f.error(c);
EXPECT(actual==0.0);
}
//* ************************************************************************* */
TEST(JacobianFactorOrdered, empty )
{
// create an empty factor
JacobianFactorOrdered f;
EXPECT(f.empty()==true);
}
/* ************************************************************************* */
// small aux. function to print out lists of anything
template<class T>
void print(const list<T>& i) {
copy(i.begin(), i.end(), ostream_iterator<T> (cout, ","));
cout << endl;
}
/* ************************************************************************* */
TEST(JacobianFactorOrdered, CONSTRUCTOR_GaussianConditional )
{
Matrix R11 = eye(2);
Matrix S12 = Matrix_(2,2,
-0.200001, 0.00,
+0.00,-0.200001
);
Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
Vector sigmas =repeat(2,0.29907);
GaussianConditionalOrdered::shared_ptr CG(new GaussianConditionalOrdered(_x2_,d,R11,_l11_,S12,sigmas));
// Call the constructor we are testing !
JacobianFactorOrdered actualLF(*CG);
JacobianFactorOrdered expectedLF(_x2_,R11,_l11_,S12,d, noiseModel::Diagonal::Sigmas(sigmas));
EXPECT(assert_equal(expectedLF,actualLF,1e-5));
}
/* ************************************************************************* */
TEST ( JacobianFactorOrdered, constraint_eliminate1 )
{
// construct a linear constraint
Vector v(2); v(0)=1.2; v(1)=3.4;
Index key = _x0_;
JacobianFactorOrdered lc(key, eye(2), v, constraintModel);
// eliminate it
GaussianConditionalOrdered::shared_ptr actualCG;
JacobianFactorOrdered::shared_ptr actualLF(new JacobianFactorOrdered(lc));
actualCG = actualLF->eliminateFirst();
// verify linear factor
EXPECT(actualLF->size() == 0);
// verify conditional Gaussian
Vector sigmas = Vector_(2, 0.0, 0.0);
GaussianConditionalOrdered expCG(_x0_, v, eye(2), sigmas);
EXPECT(assert_equal(expCG, *actualCG));
}
/* ************************************************************************* */
TEST ( JacobianFactorOrdered, constraint_eliminate2 )
{
// Construct a linear constraint
// RHS
Vector b(2); b(0)=3.0; b(1)=4.0;
// A1 - invertible
Matrix A1(2,2);
A1(0,0) = 1.0 ; A1(0,1) = 2.0;
A1(1,0) = 2.0 ; A1(1,1) = 1.0;
// A2 - not invertible
Matrix A2(2,2);
A2(0,0) = 1.0 ; A2(0,1) = 2.0;
A2(1,0) = 2.0 ; A2(1,1) = 4.0;
JacobianFactorOrdered lc(_x_, A1, _y_, A2, b, constraintModel);
// eliminate x and verify results
GaussianConditionalOrdered::shared_ptr actualCG;
JacobianFactorOrdered::shared_ptr actualLF(new JacobianFactorOrdered(lc));
actualCG = actualLF->eliminateFirst();
// LF should be null
JacobianFactorOrdered expectedLF;
EXPECT(assert_equal(*actualLF, expectedLF));
// verify CG
Matrix R = Matrix_(2, 2,
1.0, 2.0,
0.0, 1.0);
Matrix S = Matrix_(2,2,
1.0, 2.0,
0.0, 0.0);
Vector d = Vector_(2, 3.0, 0.6666);
GaussianConditionalOrdered expectedCG(_x_, d, R, _y_, S, zero(2));
EXPECT(assert_equal(expectedCG, *actualCG, 1e-4));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

456
gtsam/linear/tests/testVectorValuesObsolete.cpp
View File

@ -1,456 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testVectorValues.cpp
* @author Richard Roberts
* @date Sep 16, 2010
*/
#include <boost/assign/std/vector.hpp>
#include <gtsam/base/Testable.h>
#include <gtsam/linear/VectorValuesOrdered.h>
#include <gtsam/inference/PermutationOrdered.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace boost::assign;
using namespace gtsam;
/* ************************************************************************* */
TEST(VectorValuesOrdered, insert) {
// insert, with out-of-order indices
VectorValuesOrdered actual;
actual.insert(0, Vector_(1, 1.0));
actual.insert(1, Vector_(2, 2.0, 3.0));
actual.insert(5, Vector_(2, 6.0, 7.0));
actual.insert(2, Vector_(2, 4.0, 5.0));
// Check dimensions
LONGS_EQUAL(6, actual.size());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
LONGS_EQUAL(2, actual.dim(5));
// Logic
EXPECT(actual.exists(0));
EXPECT(actual.exists(1));
EXPECT(actual.exists(2));
EXPECT(!actual.exists(3));
EXPECT(!actual.exists(4));
EXPECT(actual.exists(5));
EXPECT(!actual.exists(6));
// Check values
EXPECT(assert_equal(Vector_(1, 1.0), actual[0]));
EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5]));
EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector()));
// Check exceptions
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
CHECK_EXCEPTION(actual.dim(3), out_of_range);
}
/* ************************************************************************* */
TEST(VectorValuesOrdered, dimsConstructor) {
vector<size_t> dims;
dims.push_back(1);
dims.push_back(2);
dims.push_back(2);
VectorValuesOrdered actual(dims);
actual[0] = Vector_(1, 1.0);
actual[1] = Vector_(2, 2.0, 3.0);
actual[2] = Vector_(2, 4.0, 5.0);
// Check dimensions
LONGS_EQUAL(3, actual.size());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
// Check values
EXPECT(assert_equal(Vector_(1, 1.0), actual[0]));
EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.asVector()));
}
/* ************************************************************************* */
TEST(VectorValuesOrdered, copyConstructor) {
// insert, with out-of-order indices
VectorValuesOrdered original;
original.insert(0, Vector_(1, 1.0));
original.insert(1, Vector_(2, 2.0, 3.0));
original.insert(5, Vector_(2, 6.0, 7.0));
original.insert(2, Vector_(2, 4.0, 5.0));
VectorValuesOrdered actual(original);
// Check dimensions
LONGS_EQUAL(6, actual.size());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
LONGS_EQUAL(2, actual.dim(5));
// Logic
EXPECT(actual.exists(0));
EXPECT(actual.exists(1));
EXPECT(actual.exists(2));
EXPECT(!actual.exists(3));
EXPECT(!actual.exists(4));
EXPECT(actual.exists(5));
EXPECT(!actual.exists(6));
// Check values
EXPECT(assert_equal(Vector_(1, 1.0), actual[0]));
EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5]));
EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector()));
// Check exceptions
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
}
/* ************************************************************************* */
TEST(VectorValuesOrdered, assignment) {
VectorValuesOrdered actual;
{
// insert, with out-of-order indices
VectorValuesOrdered original;
original.insert(0, Vector_(1, 1.0));
original.insert(1, Vector_(2, 2.0, 3.0));
original.insert(5, Vector_(2, 6.0, 7.0));
original.insert(2, Vector_(2, 4.0, 5.0));
actual = original;
}
// Check dimensions
LONGS_EQUAL(6, actual.size());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
LONGS_EQUAL(2, actual.dim(5));
// Logic
EXPECT(actual.exists(0));
EXPECT(actual.exists(1));
EXPECT(actual.exists(2));
EXPECT(!actual.exists(3));
EXPECT(!actual.exists(4));
EXPECT(actual.exists(5));
EXPECT(!actual.exists(6));
// Check values
EXPECT(assert_equal(Vector_(1, 1.0), actual[0]));
EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5]));
EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.asVector()));
// Check exceptions
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
}
/* ************************************************************************* */
TEST(VectorValuesOrdered, SameStructure) {
// insert, with out-of-order indices
VectorValuesOrdered original;
original.insert(0, Vector_(1, 1.0));
original.insert(1, Vector_(2, 2.0, 3.0));
original.insert(5, Vector_(2, 6.0, 7.0));
original.insert(2, Vector_(2, 4.0, 5.0));
VectorValuesOrdered actual(VectorValuesOrdered::SameStructure(original));
// Check dimensions
LONGS_EQUAL(6, actual.size());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
LONGS_EQUAL(2, actual.dim(5));
// Logic
EXPECT(actual.exists(0));
EXPECT(actual.exists(1));
EXPECT(actual.exists(2));
EXPECT(!actual.exists(3));
EXPECT(!actual.exists(4));
EXPECT(actual.exists(5));
EXPECT(!actual.exists(6));
// Check exceptions
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
}
/* ************************************************************************* */
TEST(VectorValuesOrdered, Zero_fromModel) {
// insert, with out-of-order indices
VectorValuesOrdered original;
original.insert(0, Vector_(1, 1.0));
original.insert(1, Vector_(2, 2.0, 3.0));
original.insert(5, Vector_(2, 6.0, 7.0));
original.insert(2, Vector_(2, 4.0, 5.0));
VectorValuesOrdered actual(VectorValuesOrdered::Zero(original));
// Check dimensions
LONGS_EQUAL(6, actual.size());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
LONGS_EQUAL(2, actual.dim(5));
// Values
EXPECT(assert_equal(Vector::Zero(1), actual[0]));
EXPECT(assert_equal(Vector::Zero(2), actual[1]));
EXPECT(assert_equal(Vector::Zero(2), actual[5]));
EXPECT(assert_equal(Vector::Zero(2), actual[2]));
// Logic
EXPECT(actual.exists(0));
EXPECT(actual.exists(1));
EXPECT(actual.exists(2));
EXPECT(!actual.exists(3));
EXPECT(!actual.exists(4));
EXPECT(actual.exists(5));
EXPECT(!actual.exists(6));
// Check exceptions
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
}
/* ************************************************************************* */
TEST(VectorValuesOrdered, Zero_fromDims) {
vector<size_t> dims;
dims.push_back(1);
dims.push_back(2);
dims.push_back(2);
VectorValuesOrdered actual(VectorValuesOrdered::Zero(dims));
// Check dimensions
LONGS_EQUAL(3, actual.size());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
// Values
EXPECT(assert_equal(Vector::Zero(1), actual[0]));
EXPECT(assert_equal(Vector::Zero(2), actual[1]));
EXPECT(assert_equal(Vector::Zero(2), actual[2]));
}
/* ************************************************************************* */
TEST(VectorValuesOrdered, Zero_fromUniform) {
VectorValuesOrdered actual(VectorValuesOrdered::Zero(3, 2));
// Check dimensions
LONGS_EQUAL(3, actual.size());
LONGS_EQUAL(2, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
// Values
EXPECT(assert_equal(Vector::Zero(2), actual[0]));
EXPECT(assert_equal(Vector::Zero(2), actual[1]));
EXPECT(assert_equal(Vector::Zero(2), actual[2]));
}
/* ************************************************************************* */
TEST(VectorValuesOrdered, resizeLike) {
// insert, with out-of-order indices
VectorValuesOrdered original;
original.insert(0, Vector_(1, 1.0));
original.insert(1, Vector_(2, 2.0, 3.0));
original.insert(5, Vector_(2, 6.0, 7.0));
original.insert(2, Vector_(2, 4.0, 5.0));
VectorValuesOrdered actual(10, 3);
actual.resizeLike(original);
// Check dimensions
LONGS_EQUAL(6, actual.size());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
LONGS_EQUAL(2, actual.dim(5));
// Logic
EXPECT(actual.exists(0));
EXPECT(actual.exists(1));
EXPECT(actual.exists(2));
EXPECT(!actual.exists(3));
EXPECT(!actual.exists(4));
EXPECT(actual.exists(5));
EXPECT(!actual.exists(6));
// Check exceptions
CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
}
/* ************************************************************************* */
TEST(VectorValuesOrdered, resize_fromUniform) {
VectorValuesOrdered actual(4, 10);
actual.resize(3, 2);
actual[0] = Vector_(2, 1.0, 2.0);
actual[1] = Vector_(2, 2.0, 3.0);
actual[2] = Vector_(2, 4.0, 5.0);
// Check dimensions
LONGS_EQUAL(3, actual.size());
LONGS_EQUAL(2, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
// Check values
EXPECT(assert_equal(Vector_(2, 1.0, 2.0), actual[0]));
EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
EXPECT(assert_equal(Vector_(6, 1.0, 2.0, 2.0, 3.0, 4.0, 5.0), actual.asVector()));
}
/* ************************************************************************* */
TEST(VectorValuesOrdered, resize_fromDims) {
vector<size_t> dims;
dims.push_back(1);
dims.push_back(2);
dims.push_back(2);
VectorValuesOrdered actual(4, 10);
actual.resize(dims);
actual[0] = Vector_(1, 1.0);
actual[1] = Vector_(2, 2.0, 3.0);
actual[2] = Vector_(2, 4.0, 5.0);
// Check dimensions
LONGS_EQUAL(3, actual.size());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
// Check values
EXPECT(assert_equal(Vector_(1, 1.0), actual[0]));
EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
EXPECT(assert_equal(Vector_(5, 1.0, 2.0, 3.0, 4.0, 5.0), actual.asVector()));
}
/* ************************************************************************* */
TEST(VectorValuesOrdered, append) {
// insert
VectorValuesOrdered actual;
actual.insert(0, Vector_(1, 1.0));
actual.insert(1, Vector_(2, 2.0, 3.0));
actual.insert(2, Vector_(2, 4.0, 5.0));
// append
vector<size_t> dims(2);
dims[0] = 3;
dims[1] = 5;
actual.append(dims);
// Check dimensions
LONGS_EQUAL(5, actual.size());
LONGS_EQUAL(1, actual.dim(0));
LONGS_EQUAL(2, actual.dim(1));
LONGS_EQUAL(2, actual.dim(2));
LONGS_EQUAL(3, actual.dim(3));
LONGS_EQUAL(5, actual.dim(4));
// Logic
EXPECT(actual.exists(0));
EXPECT(actual.exists(1));
EXPECT(actual.exists(2));
EXPECT(actual.exists(3));
EXPECT(actual.exists(4));
EXPECT(!actual.exists(5));
// Check values
EXPECT(assert_equal(Vector_(1, 1.0), actual[0]));
EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1]));
EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2]));
// Check exceptions
CHECK_EXCEPTION(actual.insert(3, Vector()), invalid_argument);
}
/* ************************************************************************* */
TEST(VectorValuesOrdered, hasSameStructure) {
VectorValuesOrdered v1(2, 3);
VectorValuesOrdered v2(3, 2);
VectorValuesOrdered v3(4, 2);
VectorValuesOrdered v4(4, 2);
EXPECT(!v1.hasSameStructure(v2));
EXPECT(!v2.hasSameStructure(v3));
EXPECT(v3.hasSameStructure(v4));
EXPECT(VectorValuesOrdered().hasSameStructure(VectorValuesOrdered()));
EXPECT(!v1.hasSameStructure(VectorValuesOrdered()));
}
/* ************************************************************************* */
TEST(VectorValuesOrdered, permute) {
VectorValuesOrdered original;
original.insert(0, Vector_(1, 1.0));
original.insert(1, Vector_(2, 2.0, 3.0));
original.insert(2, Vector_(2, 4.0, 5.0));
original.insert(3, Vector_(2, 6.0, 7.0));
VectorValuesOrdered expected;
expected.insert(0, Vector_(2, 4.0, 5.0)); // from 2
expected.insert(1, Vector_(1, 1.0)); // from 0
expected.insert(2, Vector_(2, 6.0, 7.0)); // from 3
expected.insert(3, Vector_(2, 2.0, 3.0)); // from 1
Permutation permutation(4);
permutation[0] = 2;
permutation[1] = 0;
permutation[2] = 3;
permutation[3] = 1;
VectorValuesOrdered actual = original;
actual.permuteInPlace(permutation);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(VectorValuesOrdered, subvector) {
VectorValuesOrdered init;
init.insert(0, Vector_(1, 1.0));
init.insert(1, Vector_(2, 2.0, 3.0));
init.insert(2, Vector_(2, 4.0, 5.0));
init.insert(3, Vector_(2, 6.0, 7.0));
std::vector<gtsam::Index> indices;
indices += 0, 2, 3;
Vector expSubVector = Vector_(5, 1.0, 4.0, 5.0, 6.0, 7.0);
EXPECT(assert_equal(expSubVector, init.vector(indices)));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

189
tests/testGaussianBayesNetObsolete.cpp
View File

@ -1,189 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testGaussianBayesNet.cpp
* @brief Unit tests for GaussianBayesNet
* @author Frank Dellaert
*/
// STL/C++
#include <iostream>
#include <sstream>
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <gtsam/base/Testable.h>
#include <gtsam/inference/BayesNetOrdered.h>
#include <gtsam/linear/GaussianBayesNetOrdered.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <tests/smallExample.h>
using namespace std;
using namespace gtsam;
using namespace example;
static const Index _x_=0, _y_=1, _z_=2;
/* ************************************************************************* */
TEST( GaussianBayesNetOrdered, constructor )
{
// small Bayes Net x <- y
// x y d
// 1 1 9
// 1 5
Matrix R11 = Matrix_(1,1,1.0), S12 = Matrix_(1,1,1.0);
Matrix R22 = Matrix_(1,1,1.0);
Vector d1(1), d2(1);
d1(0) = 9; d2(0) = 5;
Vector sigmas(1);
sigmas(0) = 1.;
// define nodes and specify in reverse topological sort (i.e. parents last)
GaussianConditionalOrdered x(_x_,d1,R11,_y_,S12, sigmas), y(_y_,d2,R22, sigmas);
// check small example which uses constructor
GaussianBayesNetOrdered cbn = createSmallGaussianBayesNet();
EXPECT( x.equals(*cbn[_x_]) );
EXPECT( y.equals(*cbn[_y_]) );
}
/* ************************************************************************* */
TEST( GaussianBayesNetOrdered, matrix )
{
// Create a test graph
GaussianBayesNetOrdered cbn = createSmallGaussianBayesNet();
Matrix R; Vector d;
boost::tie(R,d) = matrix(cbn); // find matrix and RHS
Matrix R1 = Matrix_(2,2,
1.0, 1.0,
0.0, 1.0
);
Vector d1 = Vector_(2, 9.0, 5.0);
EXPECT(assert_equal(R,R1));
EXPECT(assert_equal(d,d1));
}
/* ************************************************************************* */
TEST( GaussianBayesNetOrdered, optimize )
{
GaussianBayesNetOrdered cbn = createSmallGaussianBayesNet();
VectorValuesOrdered actual = optimize(cbn);
VectorValuesOrdered expected(vector<size_t>(2,1));
expected[_x_] = Vector_(1,4.);
expected[_y_] = Vector_(1,5.);
EXPECT(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( GaussianBayesNetOrdered, optimize2 )
{
// Create empty graph
GaussianFactorGraphOrdered fg;
SharedDiagonal noise = noiseModel::Unit::Create(1);
fg.add(_y_, eye(1), 2*ones(1), noise);
fg.add(_x_, eye(1),_y_, -eye(1), -ones(1), noise);
fg.add(_y_, eye(1),_z_, -eye(1), -ones(1), noise);
fg.add(_x_, -eye(1), _z_, eye(1), 2*ones(1), noise);
VectorValuesOrdered actual = *GaussianSequentialSolver(fg).optimize();
VectorValuesOrdered expected(vector<size_t>(3,1));
expected[_x_] = Vector_(1,1.);
expected[_y_] = Vector_(1,2.);
expected[_z_] = Vector_(1,3.);
EXPECT(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( GaussianBayesNetOrdered, optimize3 )
{
// y=R*x, x=inv(R)*y
// 9 = 1 1 4
// 5 1 5
// NOTE: we are supplying a new RHS here
GaussianBayesNetOrdered cbn = createSmallGaussianBayesNet();
VectorValuesOrdered expected(vector<size_t>(2,1)), x(vector<size_t>(2,1));
expected[_x_] = Vector_(1, 4.);
expected[_y_] = Vector_(1, 5.);
// test functional version
VectorValuesOrdered actual = optimize(cbn);
EXPECT(assert_equal(expected,actual));
// test imperative version
optimizeInPlace(cbn,x);
EXPECT(assert_equal(expected,x));
}
/* ************************************************************************* */
TEST( GaussianBayesNetOrdered, backSubstituteTranspose )
{
// x=R'*y, y=inv(R')*x
// 2 = 1 2
// 5 1 1 3
GaussianBayesNetOrdered cbn = createSmallGaussianBayesNet();
VectorValuesOrdered y(vector<size_t>(2,1)), x(vector<size_t>(2,1));
x[_x_] = Vector_(1,2.);
x[_y_] = Vector_(1,5.);
y[_x_] = Vector_(1,2.);
y[_y_] = Vector_(1,3.);
// test functional version
VectorValuesOrdered actual = backSubstituteTranspose(cbn,x);
EXPECT(assert_equal(y,actual));
}
/* ************************************************************************* */
// Tests computing Determinant
TEST( GaussianBayesNetOrdered, DeterminantTest )
{
GaussianBayesNetOrdered cbn;
cbn += boost::shared_ptr<GaussianConditionalOrdered>(new GaussianConditionalOrdered(
0, Vector_( 2, 3.0, 4.0 ), Matrix_(2, 2, 1.0, 3.0, 0.0, 4.0 ),
1, Matrix_(2, 2, 2.0, 1.0, 2.0, 3.0),
ones(2)));
cbn += boost::shared_ptr<GaussianConditionalOrdered>(new GaussianConditionalOrdered(
1, Vector_( 2, 5.0, 6.0 ), Matrix_(2, 2, 1.0, 1.0, 0.0, 3.0 ),
2, Matrix_(2, 2, 1.0, 0.0, 5.0, 2.0),
ones(2)));
cbn += boost::shared_ptr<GaussianConditionalOrdered>(new GaussianConditionalOrdered(
3, Vector_( 2, 7.0, 8.0 ), Matrix_(2, 2, 1.0, 1.0, 0.0, 5.0 ),
ones(2)));
double expectedDeterminant = 60;
double actualDeterminant = determinant(cbn);
EXPECT_DOUBLES_EQUAL( expectedDeterminant, actualDeterminant, 1e-9);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

228
tests/testGaussianFactorObsolete.cpp
View File

@ -1,228 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testGaussianFactor.cpp
* @brief Unit tests for Linear Factor
* @author Christian Potthast
* @author Frank Dellaert
**/
#include <tests/smallExample.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/OrderingOrdered.h>
#include <gtsam/linear/GaussianConditionalOrdered.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp>
#include <boost/assign/std/map.hpp> // for insert
using namespace boost::assign;
#include <iostream>
using namespace std;
using namespace gtsam;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
static SharedDiagonal
sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2),
constraintModel = noiseModel::Constrained::All(2);
//const Key kx1 = X(1), kx2 = X(2), kl1 = L(1); // FIXME: throws exception
/* ************************************************************************* */
TEST( GaussianFactorOrdered, linearFactor )
{
const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
OrderingOrdered ordering; ordering += kx1,kx2,kl1;
Matrix I = eye(2);
Vector b = Vector_(2, 2.0, -1.0);
JacobianFactorOrdered expected(ordering[kx1], -10*I,ordering[kx2], 10*I, b, noiseModel::Unit::Create(2));
// create a small linear factor graph
GaussianFactorGraphOrdered fg = example::createGaussianFactorGraph(ordering);
// get the factor kf2 from the factor graph
JacobianFactorOrdered::shared_ptr lf =
boost::dynamic_pointer_cast<JacobianFactorOrdered>(fg[1]);
// check if the two factors are the same
EXPECT(assert_equal(expected,*lf));
}
/* ************************************************************************* */
TEST( GaussianFactorOrdered, getDim )
{
const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
// get a factor
OrderingOrdered ordering; ordering += kx1,kx2,kl1;
GaussianFactorGraphOrdered fg = example::createGaussianFactorGraph(ordering);
GaussianFactorOrdered::shared_ptr factor = fg[0];
// get the size of a variable
size_t actual = factor->getDim(factor->find(ordering[kx1]));
// verify
size_t expected = 2;
EXPECT_LONGS_EQUAL(expected, actual);
}
/* ************************************************************************* */
TEST( GaussianFactorOrdered, error )
{
const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
// create a small linear factor graph
OrderingOrdered ordering; ordering += kx1,kx2,kl1;
GaussianFactorGraphOrdered fg = example::createGaussianFactorGraph(ordering);
// get the first factor from the factor graph
GaussianFactorOrdered::shared_ptr lf = fg[0];
// check the error of the first factor with noisy config
VectorValuesOrdered cfg = example::createZeroDelta(ordering);
// calculate the error from the factor kf1
// note the error is the same as in testNonlinearFactor
double actual = lf->error(cfg);
DOUBLES_EQUAL( 1.0, actual, 0.00000001 );
}
/* ************************************************************************* */
TEST( GaussianFactorOrdered, matrix )
{
const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
// create a small linear factor graph
OrderingOrdered ordering; ordering += kx1,kx2,kl1;
GaussianFactorGraphOrdered fg = example::createGaussianFactorGraph(ordering);
// get the factor kf2 from the factor graph
//GaussianFactorOrdered::shared_ptr lf = fg[1]; // NOTE: using the older version
Vector b2 = Vector_(2, 0.2, -0.1);
Matrix I = eye(2);
// render with a given ordering
OrderingOrdered ord;
ord += kx1,kx2;
JacobianFactorOrdered::shared_ptr lf(new JacobianFactorOrdered(ord[kx1], -I, ord[kx2], I, b2, sigma0_1));
// Test whitened version
Matrix A_act1; Vector b_act1;
boost::tie(A_act1,b_act1) = lf->matrix(true);
Matrix A1 = Matrix_(2,4,
-10.0, 0.0, 10.0, 0.0,
000.0,-10.0, 0.0, 10.0 );
Vector b1 = Vector_(2, 2.0, -1.0);
EQUALITY(A_act1,A1);
EQUALITY(b_act1,b1);
// Test unwhitened version
Matrix A_act2; Vector b_act2;
boost::tie(A_act2,b_act2) = lf->matrix(false);
Matrix A2 = Matrix_(2,4,
-1.0, 0.0, 1.0, 0.0,
000.0,-1.0, 0.0, 1.0 );
//Vector b2 = Vector_(2, 2.0, -1.0);
EQUALITY(A_act2,A2);
EQUALITY(b_act2,b2);
// Ensure that whitening is consistent
boost::shared_ptr<noiseModel::Gaussian> model = lf->get_model();
model->WhitenSystem(A_act2, b_act2);
EQUALITY(A_act1, A_act2);
EQUALITY(b_act1, b_act2);
}
/* ************************************************************************* */
TEST( GaussianFactorOrdered, matrix_aug )
{
const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
// create a small linear factor graph
OrderingOrdered ordering; ordering += kx1,kx2,kl1;
GaussianFactorGraphOrdered fg = example::createGaussianFactorGraph(ordering);
// get the factor kf2 from the factor graph
//GaussianFactorOrdered::shared_ptr lf = fg[1];
Vector b2 = Vector_(2, 0.2, -0.1);
Matrix I = eye(2);
// render with a given ordering
OrderingOrdered ord;
ord += kx1,kx2;
JacobianFactorOrdered::shared_ptr lf(new JacobianFactorOrdered(ord[kx1], -I, ord[kx2], I, b2, sigma0_1));
// Test unwhitened version
Matrix Ab_act1;
Ab_act1 = lf->matrix_augmented(false);
Matrix Ab1 = Matrix_(2,5,
-1.0, 0.0, 1.0, 0.0, 0.2,
00.0,- 1.0, 0.0, 1.0, -0.1 );
EQUALITY(Ab_act1,Ab1);
// Test whitened version
Matrix Ab_act2;
Ab_act2 = lf->matrix_augmented(true);
Matrix Ab2 = Matrix_(2,5,
-10.0, 0.0, 10.0, 0.0, 2.0,
00.0, -10.0, 0.0, 10.0, -1.0 );
EQUALITY(Ab_act2,Ab2);
// Ensure that whitening is consistent
boost::shared_ptr<noiseModel::Gaussian> model = lf->get_model();
model->WhitenInPlace(Ab_act1);
EQUALITY(Ab_act1, Ab_act2);
}
/* ************************************************************************* */
// small aux. function to print out lists of anything
template<class T>
void print(const list<T>& i) {
copy(i.begin(), i.end(), ostream_iterator<T> (cout, ","));
cout << endl;
}
/* ************************************************************************* */
TEST( GaussianFactorOrdered, size )
{
// create a linear factor graph
const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
OrderingOrdered ordering; ordering += kx1,kx2,kl1;
GaussianFactorGraphOrdered fg = example::createGaussianFactorGraph(ordering);
// get some factors from the graph
boost::shared_ptr<GaussianFactorOrdered> factor1 = fg[0];
boost::shared_ptr<GaussianFactorOrdered> factor2 = fg[1];
boost::shared_ptr<GaussianFactorOrdered> factor3 = fg[2];
EXPECT_LONGS_EQUAL(1, factor1->size());
EXPECT_LONGS_EQUAL(2, factor2->size());
EXPECT_LONGS_EQUAL(2, factor3->size());
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

79
tests/testSymbolicBayesNetBObsolete.cpp
View File

@ -1,79 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testSymbolicBayesNetB.cpp
* @brief Unit tests for a symbolic Bayes chain
* @author Frank Dellaert
*/
#include <boost/assign/list_inserter.hpp> // for 'insert()'
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <tests/smallExample.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/SymbolicSequentialSolverOrdered.h>
#include <gtsam/nonlinear/OrderingOrdered.h>
#include <gtsam/nonlinear/Symbol.h>
using namespace std;
using namespace gtsam;
using namespace example;
using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */
TEST( SymbolicBayesNetOrdered, constructor )
{
OrderingOrdered o; o += X(2),L(1),X(1);
// Create manually
IndexConditionalOrdered::shared_ptr
x2(new IndexConditionalOrdered(o[X(2)],o[L(1)], o[X(1)])),
l1(new IndexConditionalOrdered(o[L(1)],o[X(1)])),
x1(new IndexConditionalOrdered(o[X(1)]));
BayesNetOrdered<IndexConditionalOrdered> expected;
expected.push_back(x2);
expected.push_back(l1);
expected.push_back(x1);
// Create from a factor graph
GaussianFactorGraphOrdered factorGraph = createGaussianFactorGraph(o);
SymbolicFactorGraphOrdered fg(factorGraph);
// eliminate it
SymbolicBayesNetOrdered actual = *SymbolicSequentialSolver(fg).eliminate();
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( SymbolicBayesNetOrdered, FromGaussian) {
SymbolicBayesNetOrdered expected;
expected.push_back(IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(0, 1)));
expected.push_back(IndexConditionalOrdered::shared_ptr(new IndexConditionalOrdered(1)));
GaussianBayesNetOrdered gbn = createSmallGaussianBayesNet();
SymbolicBayesNetOrdered actual(gbn);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

156
tests/testSymbolicFactorGraphBObsolete.cpp
View File

@ -1,156 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testSymbolicFactorGraphB.cpp
* @brief Unit tests for a symbolic Factor Graph
* @author Frank Dellaert
*/
#include <tests/smallExample.h>
#include <gtsam/nonlinear/OrderingOrdered.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/SymbolicFactorGraphOrdered.h>
#include <gtsam/inference/BayesNetOrdered-inl.h>
#include <gtsam/inference/SymbolicSequentialSolverOrdered.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
using namespace std;
using namespace gtsam;
using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */
TEST( SymbolicFactorGraphOrdered, symbolicFactorGraph )
{
OrderingOrdered o; o += X(1),L(1),X(2);
// construct expected symbolic graph
SymbolicFactorGraphOrdered expected;
expected.push_factor(o[X(1)]);
expected.push_factor(o[X(1)],o[X(2)]);
expected.push_factor(o[X(1)],o[L(1)]);
expected.push_factor(o[X(2)],o[L(1)]);
// construct it from the factor graph
GaussianFactorGraphOrdered factorGraph = example::createGaussianFactorGraph(o);
SymbolicFactorGraphOrdered actual(factorGraph);
CHECK(assert_equal(expected, actual));
}
///* ************************************************************************* */
//TEST( SymbolicFactorGraphOrdered, findAndRemoveFactors )
//{
// // construct it from the factor graph graph
// GaussianFactorGraphOrdered factorGraph = example::createGaussianFactorGraph();
// SymbolicFactorGraphOrdered actual(factorGraph);
// SymbolicFactor::shared_ptr f1 = actual[0];
// SymbolicFactor::shared_ptr f3 = actual[2];
// actual.findAndRemoveFactors(X(2));
//
// // construct expected graph after find_factors_and_remove
// SymbolicFactorGraphOrdered expected;
// SymbolicFactor::shared_ptr null;
// expected.push_back(f1);
// expected.push_back(null);
// expected.push_back(f3);
// expected.push_back(null);
//
// CHECK(assert_equal(expected, actual));
//}
///* ************************************************************************* */
//TEST( SymbolicFactorGraphOrdered, factors)
//{
// // create a test graph
// GaussianFactorGraphOrdered factorGraph = example::createGaussianFactorGraph();
// SymbolicFactorGraphOrdered fg(factorGraph);
//
// // ask for all factor indices connected to x1
// list<size_t> x1_factors = fg.factors(X(1));
// int x1_indices[] = { 0, 1, 2 };
// list<size_t> x1_expected(x1_indices, x1_indices + 3);
// CHECK(x1_factors==x1_expected);
//
// // ask for all factor indices connected to x2
// list<size_t> x2_factors = fg.factors(X(2));
// int x2_indices[] = { 1, 3 };
// list<size_t> x2_expected(x2_indices, x2_indices + 2);
// CHECK(x2_factors==x2_expected);
//}
///* ************************************************************************* */
//TEST( SymbolicFactorGraphOrdered, removeAndCombineFactors )
//{
// // create a test graph
// GaussianFactorGraphOrdered factorGraph = example::createGaussianFactorGraph();
// SymbolicFactorGraphOrdered fg(factorGraph);
//
// // combine all factors connected to x1
// SymbolicFactor::shared_ptr actual = removeAndCombineFactors(fg,X(1));
//
// // check result
// SymbolicFactor expected(L(1),X(1),X(2));
// CHECK(assert_equal(expected,*actual));
//}
///* ************************************************************************* */
//TEST( SymbolicFactorGraphOrdered, eliminateOne )
//{
// OrderingOrdered o; o += X(1),L(1),X(2);
// // create a test graph
// GaussianFactorGraphOrdered factorGraph = example::createGaussianFactorGraph(o);
// SymbolicFactorGraphOrdered fg(factorGraph);
//
// // eliminate
// IndexConditionalOrdered::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, o[X(1)]+1);
//
// // create expected symbolic IndexConditional
// IndexConditionalOrdered expected(o[X(1)],o[L(1)],o[X(2)]);
//
// CHECK(assert_equal(expected,*actual));
//}
/* ************************************************************************* */
TEST( SymbolicFactorGraphOrdered, eliminate )
{
OrderingOrdered o; o += X(2),L(1),X(1);
// create expected Chordal bayes Net
IndexConditionalOrdered::shared_ptr x2(new IndexConditionalOrdered(o[X(2)], o[L(1)], o[X(1)]));
IndexConditionalOrdered::shared_ptr l1(new IndexConditionalOrdered(o[L(1)], o[X(1)]));
IndexConditionalOrdered::shared_ptr x1(new IndexConditionalOrdered(o[X(1)]));
SymbolicBayesNetOrdered expected;
expected.push_back(x2);
expected.push_back(l1);
expected.push_back(x1);
// create a test graph
GaussianFactorGraphOrdered factorGraph = example::createGaussianFactorGraph(o);
SymbolicFactorGraphOrdered fg(factorGraph);
// eliminate it
SymbolicBayesNetOrdered actual = *SymbolicSequentialSolver(fg).eliminate();
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */