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

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.

release/4.3a0
Richard Roberts 2010-12-21 18:23:56 +00:00
parent 2b69e340e5
commit 9edeb1102c
6 changed files with 108 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
gtsam/inference/BayesTree-inl.h
View File

@ -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_) {
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: ");
(void)c; p_S_R.push_front(*(conditional++)); }
// for(Index j=0; j<integrands.size(); ++j)
// p_S_R.pop_front();
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)

44
gtsam/inference/Permutation.cpp
View File

@ -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) {
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];
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());
}
assert(nextVar == size);
} 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 - toBack.size());
assert(nextVar == size - toBackUniqueSize);
return ret;
}

4
gtsam/inference/Permutation.h
View File

@ -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(); }

2
gtsam/linear/GaussianMultifrontalSolver.h
View File

@ -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).
*

10
gtsam/nonlinear/NonlinearOptimizer.h
View File

@ -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
*/

34
tests/testInference.cpp
View File

@ -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);}
/* ************************************************************************* */