Fixed bug in BayesTree shortcuts and marginals. Also added an input check to Permutation::PushToBack and PullToFront that catches the bad input of duplicate variables caused by the bug.
Richard Roberts
parent
2b69e340e5
commit
9edeb1102c
|
|
@ -306,37 +306,45 @@ namespace gtsam {
|
|||
// root and the marginal on the root, integrating out all other variables.
|
||||
// The integrands include any parents of this clique and the variables of
|
||||
// the parent clique.
|
||||
vector<Index> variablesAtBack;
|
||||
variablesAtBack.reserve(this->size() + R->size());
|
||||
FastSet<Index> variablesAtBack;
|
||||
FastSet<Index> separator;
|
||||
size_t uniqueRootVariables = 0;
|
||||
BOOST_FOREACH(const Index separatorIndex, this->separator_) {
|
||||
variablesAtBack.push_back(separatorIndex);
|
||||
variablesAtBack.insert(separatorIndex);
|
||||
separator.insert(separatorIndex);
|
||||
if(debug) cout << "At back (this): " << separatorIndex << endl;
|
||||
}
|
||||
BOOST_FOREACH(const sharedConditional& conditional, *R) {
|
||||
variablesAtBack.push_back(conditional->key());
|
||||
if(variablesAtBack.insert(conditional->key()).second)
|
||||
++ uniqueRootVariables;
|
||||
if(debug) cout << "At back (root): " << conditional->key() << endl;
|
||||
}
|
||||
|
||||
Permutation toBack = Permutation::PushToBack(variablesAtBack, R->back()->key() + 1);
|
||||
Permutation toBack = Permutation::PushToBack(
|
||||
vector<Index>(variablesAtBack.begin(), variablesAtBack.end()),
|
||||
R->back()->key() + 1);
|
||||
Permutation::shared_ptr toBackInverse(toBack.inverse());
|
||||
BOOST_FOREACH(const typename CONDITIONAL::Factor::shared_ptr& factor, p_Cp_R) {
|
||||
factor->permuteWithInverse(*toBackInverse); }
|
||||
typename BayesNet<CONDITIONAL>::shared_ptr eliminated(EliminationTree<typename CONDITIONAL::Factor>::Create(p_Cp_R)->eliminate());
|
||||
|
||||
// take only the conditionals for p(S|R)
|
||||
// Take only the conditionals for p(S|R). We check for each variable being
|
||||
// in the separator set because if some separator variables overlap with
|
||||
// root variables, we cannot rely on the number of root variables, and also
|
||||
// want to include those variables in the conditional.
|
||||
BayesNet<CONDITIONAL> p_S_R;
|
||||
typename BayesNet<CONDITIONAL>::const_reverse_iterator conditional = eliminated->rbegin();
|
||||
BOOST_FOREACH(const sharedConditional& c, *R) {
|
||||
(void)c; ++conditional; }
|
||||
BOOST_FOREACH(const Index c, this->separator_) {
|
||||
if(debug)
|
||||
(*conditional)->print("Taking C|R conditional: ");
|
||||
(void)c; p_S_R.push_front(*(conditional++)); }
|
||||
|
||||
// for(Index j=0; j<integrands.size(); ++j)
|
||||
// p_S_R.pop_front();
|
||||
BOOST_REVERSE_FOREACH(typename CONDITIONAL::shared_ptr conditional, *eliminated) {
|
||||
if(separator.find(toBack[conditional->key()]) != separator.end()) {
|
||||
if(debug)
|
||||
conditional->print("Taking C|R conditional: ");
|
||||
p_S_R.push_front(conditional);
|
||||
}
|
||||
if(p_S_R.size() == separator.size())
|
||||
break;
|
||||
}
|
||||
|
||||
// Undo the permutation
|
||||
if(debug) toBack.print("toBack: ");
|
||||
p_S_R.permuteWithInverse(toBack);
|
||||
|
||||
// return the parent shortcut P(Sp|R)
|
||||
|
|
|
|||
|
|
@ -19,6 +19,8 @@
|
|||
#include <gtsam/inference/Permutation.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
using namespace std;
|
||||
|
|
@ -34,7 +36,7 @@ Permutation Permutation::Identity(Index nVars) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Permutation Permutation::PullToFront(const vector<Index>& toFront, size_t size) {
|
||||
Permutation Permutation::PullToFront(const vector<Index>& toFront, size_t size, bool filterDuplicates) {
|
||||
|
||||
Permutation ret(size);
|
||||
|
||||
|
|
@ -43,13 +45,24 @@ Permutation Permutation::PullToFront(const vector<Index>& toFront, size_t size)
|
|||
|
||||
// Put the pulled variables at the front of the permutation and set up the
|
||||
// pulled flags.
|
||||
size_t toFrontUniqueSize;
|
||||
for(Index j=0; j<toFront.size(); ++j) {
|
||||
ret[j] = toFront[j];
|
||||
pulled[toFront[j]] = true;
|
||||
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 = toFront.size();
|
||||
Index nextVar = toFrontUniqueSize;
|
||||
for(Index j=0; j<size; ++j)
|
||||
if(!pulled[j])
|
||||
ret[nextVar++] = j;
|
||||
|
|
@ -59,7 +72,7 @@ Permutation Permutation::PullToFront(const vector<Index>& toFront, size_t size)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Permutation Permutation::PushToBack(const std::vector<Index>& toBack, size_t size) {
|
||||
Permutation Permutation::PushToBack(const std::vector<Index>& toBack, size_t size, bool filterDuplicates) {
|
||||
|
||||
Permutation ret(size);
|
||||
|
||||
|
|
@ -68,19 +81,34 @@ Permutation Permutation::PushToBack(const std::vector<Index>& toBack, size_t siz
|
|||
|
||||
// Put the pushed variables at the back of the permutation and set up the
|
||||
// pushed flags;
|
||||
Index nextVar = size - toBack.size();
|
||||
for(Index j=0; j<toBack.size(); ++j) {
|
||||
ret[nextVar++] = toBack[j];
|
||||
pushed[toBack[j]] = true;
|
||||
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);
|
||||
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 - toBack.size());
|
||||
assert(nextVar == size - toBackUniqueSize);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -96,13 +96,13 @@ public:
|
|||
* 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);
|
||||
static Permutation PullToFront(const std::vector<Index>& toFront, size_t size, bool filterDuplicates = false);
|
||||
|
||||
/**
|
||||
* Create a permutation that pulls the given variables to the front while
|
||||
* pushing the rest to the back.
|
||||
*/
|
||||
static Permutation PushToBack(const std::vector<Index>& toBack, size_t size);
|
||||
static Permutation PushToBack(const std::vector<Index>& toBack, size_t size, bool filterDuplicates = false);
|
||||
|
||||
iterator begin() { return rangeIndices_.begin(); }
|
||||
const_iterator begin() const { return rangeIndices_.begin(); }
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@ namespace gtsam {
|
|||
* 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
|
||||
* 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).
|
||||
*
|
||||
|
|
|
|||
|
|
@ -205,6 +205,16 @@ namespace gtsam {
|
|||
*/
|
||||
size_t iterations() const { return iterations_; }
|
||||
|
||||
/**
|
||||
* Return the solver
|
||||
*/
|
||||
shared_solver solver() const { return solver_; }
|
||||
|
||||
/**
|
||||
* Return the ordering
|
||||
*/
|
||||
shared_ordering ordering() const { return ordering_; }
|
||||
|
||||
/**
|
||||
* Return mean and covariance on a single variable
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -19,12 +19,12 @@
|
|||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <gtsam/slam/smallExample.h>
|
||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||
#include <gtsam/slam/smallExample.h>
|
||||
#include <gtsam/slam/planarSLAM.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace example;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// The tests below test the *generic* inference algorithms. Some of these have
|
||||
|
|
@ -34,6 +34,7 @@ using namespace example;
|
|||
/* ************************************************************************* */
|
||||
TEST(GaussianFactorGraph, createSmoother)
|
||||
{
|
||||
using namespace example;
|
||||
GaussianFactorGraph fg2;
|
||||
Ordering ordering;
|
||||
boost::tie(fg2,ordering) = createSmoother(3);
|
||||
|
|
@ -50,6 +51,7 @@ TEST(GaussianFactorGraph, createSmoother)
|
|||
/* ************************************************************************* */
|
||||
TEST( Inference, marginals )
|
||||
{
|
||||
using namespace example;
|
||||
// create and marginalize a small Bayes net on "x"
|
||||
GaussianBayesNet cbn = createSmallGaussianBayesNet();
|
||||
vector<Index> xvar; xvar.push_back(0);
|
||||
|
|
@ -60,6 +62,34 @@ TEST( Inference, marginals )
|
|||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Inference, marginals2)
|
||||
{
|
||||
using namespace gtsam::planarSLAM;
|
||||
|
||||
Graph fg;
|
||||
SharedDiagonal poseModel(sharedSigma(3, 0.1));
|
||||
SharedDiagonal pointModel(sharedSigma(3, 0.1));
|
||||
|
||||
fg.addPrior(PoseKey(0), Pose2(), poseModel);
|
||||
fg.addOdometry(PoseKey(0), PoseKey(1), Pose2(1.0,0.0,0.0), poseModel);
|
||||
fg.addOdometry(PoseKey(1), PoseKey(2), Pose2(1.0,0.0,0.0), poseModel);
|
||||
fg.addBearingRange(PoseKey(0), PointKey(0), Rot2(), 1.0, pointModel);
|
||||
fg.addBearingRange(PoseKey(1), PointKey(0), Rot2(), 1.0, pointModel);
|
||||
fg.addBearingRange(PoseKey(2), PointKey(0), Rot2(), 1.0, pointModel);
|
||||
|
||||
Values init;
|
||||
init.insert(PoseKey(0), Pose2(0.0,0.0,0.0));
|
||||
init.insert(PoseKey(1), Pose2(1.0,0.0,0.0));
|
||||
init.insert(PoseKey(2), Pose2(2.0,0.0,0.0));
|
||||
init.insert(PointKey(0), Point2(1.0,1.0));
|
||||
|
||||
Ordering ordering(*fg.orderingCOLAMD(init));
|
||||
GaussianFactorGraph::shared_ptr gfg(fg.linearize(init, ordering));
|
||||
GaussianMultifrontalSolver solver(*gfg);
|
||||
solver.marginalFactor(ordering[PointKey(0)]);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue