Working on ISAM
Richard Roberts
parent
735c7a8650
commit
60d5feb5cf
|
|
@ -31,6 +31,7 @@ namespace gtsam {
|
|||
// Forward declarations
|
||||
template<class FACTOR> class FactorGraph;
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* Bayes tree
|
||||
* @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain,
|
||||
|
|
@ -256,4 +257,23 @@ namespace gtsam {
|
|||
|
||||
}; // BayesTree
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
class BayesTreeOrphanWrapper : public CLIQUE::FactorType
|
||||
{
|
||||
public:
|
||||
typedef CLIQUE CliqueType;
|
||||
typedef typename CLIQUE::FactorType Base;
|
||||
|
||||
boost::shared_ptr<CliqueType> clique;
|
||||
|
||||
BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& clique) :
|
||||
clique(clique)
|
||||
{
|
||||
// Store parent keys in our base type factor so that eliminating those parent keys will pull
|
||||
// this subtree into the elimination.
|
||||
keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents());
|
||||
}
|
||||
};
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -0,0 +1,63 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 <gtsam/inference/ISAM.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE>
|
||||
void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors, Cliques& orphans, const Eliminate& function)
|
||||
{
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
// Throw exception if disconnected
|
||||
BayesNetType bn;
|
||||
if (!this->empty()) {
|
||||
const FastSet<Key> newFactorKeys = newFactors.keys();
|
||||
this->removeTop(std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans);
|
||||
if (bn.empty())
|
||||
throw std::runtime_error(
|
||||
"ISAM::update_internal(): no variables in common between existing Bayes tree and incoming factors!");
|
||||
}
|
||||
|
||||
// Add the removed top and the new factors
|
||||
FactorGraphType factors;
|
||||
factors += bn;
|
||||
factors += newFactors;
|
||||
|
||||
// Add the orphaned subtrees
|
||||
BOOST_FOREACH(const sharedClique& orphan, orphans)
|
||||
factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
|
||||
|
||||
// eliminate into a Bayes net
|
||||
Base bayesTree = *factors.eliminateMultifrontal(boost::none, function);
|
||||
this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end());
|
||||
this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE>
|
||||
void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors, const Eliminate& function)
|
||||
{
|
||||
Cliques orphans;
|
||||
this->update_internal(newFactors, orphans, function);
|
||||
}
|
||||
|
||||
}
|
||||
/// namespace gtsam
|
||||
|
|
@ -0,0 +1,69 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
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 BAYESTREE>
|
||||
class ISAM: public BAYESTREE
|
||||
{
|
||||
private:
|
||||
|
||||
typedef BAYESTREE Base;
|
||||
typedef typename Base::FactorGraphType FactorGraphType;
|
||||
typedef typename Base::Cliques Cliques;
|
||||
typedef typename Base::Eliminate Eliminate;
|
||||
typedef typename Base::EliminationTraits EliminationTraits;
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Create an empty Bayes Tree */
|
||||
ISAM() {}
|
||||
|
||||
/** Copy constructor */
|
||||
ISAM(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
|
||||
*/
|
||||
void update(const FactorGraphType& newFactors, const Eliminate& function = EliminationTraits::DefaultEliminate);
|
||||
|
||||
/** update_internal provides access to list of orphans for drawing purposes */
|
||||
void update_internal(const FactorGraphType& newFactors, Cliques& orphans,
|
||||
const Eliminate& function = EliminationTraits::DefaultEliminate);
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
}/// namespace gtsam
|
||||
|
|
@ -177,8 +177,18 @@ namespace gtsam {
|
|||
// Gather factors
|
||||
FactorGraphType gatheredFactors;
|
||||
gatheredFactors.reserve(node->factors.size() + node->children.size());
|
||||
gatheredFactors.push_back(node->factors.begin(), node->factors.end());
|
||||
gatheredFactors.push_back(myData.childFactors.begin(), myData.childFactors.end());
|
||||
gatheredFactors += node->factors;
|
||||
gatheredFactors += myData.childFactors;
|
||||
|
||||
// Check for Bayes tree orphan subtrees, and add them to our children
|
||||
BOOST_FOREACH(const sharedFactor& f, node->factors)
|
||||
{
|
||||
if(const BayesTreeOrphanWrapper<BTNode>* asSubtree = dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(f.get()))
|
||||
{
|
||||
myData.bayesTreeNode->children.push_back(asSubtree->clique);
|
||||
asSubtree->clique->parent_ = myData.bayesTreeNode;
|
||||
}
|
||||
}
|
||||
|
||||
// Do dense elimination step
|
||||
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
|
||||
|
|
|
|||
|
|
@ -0,0 +1,31 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SymbolicISAM.cpp
|
||||
* @date July 29, 2013
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#include <gtsam/symbolic/SymbolicISAM.h>
|
||||
#include <gtsam/inference/ISAM-inst.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
SymbolicISAM::SymbolicISAM() {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SymbolicISAM::SymbolicISAM(const SymbolicBayesTree& bayesTree) :
|
||||
Base(bayesTree) {}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SymbolicISAM.h
|
||||
* @date July 29, 2013
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/symbolic/SymbolicBayesTree.h>
|
||||
#include <gtsam/inference/ISAM.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class GTSAM_EXPORT SymbolicISAM : public ISAM<SymbolicBayesTree>
|
||||
{
|
||||
public:
|
||||
typedef ISAM<SymbolicBayesTree> Base;
|
||||
typedef SymbolicISAM This;
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Create an empty Bayes Tree */
|
||||
SymbolicISAM();
|
||||
|
||||
/** Copy constructor */
|
||||
SymbolicISAM(const SymbolicBayesTree& bayesTree);
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -341,6 +341,28 @@ TEST( BayesTreeOrdered, removeTop4 )
|
|||
EXPECT(orphans.empty());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( BayesTreeOrdered, removeTop5 )
|
||||
{
|
||||
// Remove top called with variables that are not in the Bayes tree
|
||||
SymbolicFactorGraph graph = list_of
|
||||
(SymbolicFactor(L(5)))
|
||||
(SymbolicFactor(X(4), L(5)))
|
||||
(SymbolicFactor(X(2), X(4)))
|
||||
(SymbolicFactor(X(3), X(2)));
|
||||
SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(
|
||||
Ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) ));
|
||||
|
||||
// Remove nonexistant
|
||||
SymbolicBayesNet bn;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
bayesTree.removeTop(list_of(X(10)), bn, orphans);
|
||||
|
||||
SymbolicBayesNet expectedBn;
|
||||
EXPECT(assert_equal(expectedBn, bn));
|
||||
EXPECT(orphans.empty());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SymbolicBayesTree, thinTree ) {
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,132 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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/symbolic/SymbolicISAM.h>
|
||||
#include <gtsam/symbolic/tests/symbolicExampleGraphs.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// 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( SymbolicISAM, iSAM )
|
||||
{
|
||||
// Now we modify the Bayes tree by inserting a new factor over B and S
|
||||
|
||||
SymbolicFactorGraph fullGraph;
|
||||
fullGraph += asiaGraph;
|
||||
fullGraph += SymbolicFactor(_B_, _S_);
|
||||
|
||||
// This ordering is chosen to match the one chosen by COLAMD during the ISAM update
|
||||
SymbolicBayesTree expected = *fullGraph.eliminateMultifrontal(Ordering(list_of(_X_)(_B_)(_S_)(_E_)(_L_)(_T_)));
|
||||
|
||||
// Add factor on B and S
|
||||
SymbolicISAM actual = *asiaGraph.eliminateMultifrontal();
|
||||
|
||||
// Check whether the same
|
||||
EXPECT(assert_equal(expected, (const SymbolicBayesTree&)actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//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);}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue