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

Restored NonlinearFactorGraph::symbolic() and made a few other minor fixes.

release/4.3a0
Richard Roberts 2013-09-17 03:13:32 +00:00
parent b050e96b93
commit 2b78eb8bec
4 changed files with 42 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -20,12 +20,13 @@
#include <cmath>
#include <limits>
#include <boost/foreach.hpp>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/symbolic/SymbolicFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#ifdef GTSAM_USE_TBB
@ -233,33 +234,21 @@ Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap<Key, int>
}
/* ************************************************************************* */
//SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const {
// gttic(NonlinearFactorGraph_symbolic_from_Ordering);
//
// // Generate the symbolic factor graph
// SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
// symbolicfg->reserve(this->size());
//
// BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
// if(factor)
// symbolicfg->push_back(factor->symbolic(ordering));
// else
// symbolicfg->push_back(SymbolicFactorGraph::sharedFactor());
// }
//
// return symbolicfg;
//}
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic() const
{
// Generate the symbolic factor graph
SymbolicFactorGraph::shared_ptr symbolic = boost::make_shared<SymbolicFactorGraph>();
symbolic->reserve(this->size());
/* ************************************************************************* */
//pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic(
// const Values& config) const
//{
// gttic(NonlinearFactorGraph_symbolic_from_Values);
//
// // Generate an initial key ordering in iterator order
// Ordering::shared_ptr ordering(config.orderingArbitrary());
// return make_pair(symbolic(*ordering), ordering);
//}
BOOST_FOREACH(const sharedFactor& factor, *this) {
if(factor)
*symbolic += SymbolicFactor(*factor);
else
*symbolic += SymbolicFactorGraph::sharedFactor();
}
return symbolic;
}
/* ************************************************************************* */
namespace {

14
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -31,6 +31,7 @@ namespace gtsam {
class Values;
class Ordering;
class GaussianFactorGraph;
class SymbolicFactorGraph;
/**
* Formatting options when saving in GraphViz format using
@ -106,18 +107,9 @@ namespace gtsam {
double probPrime(const Values& c) const;
/**
* Create a symbolic factor graph using an existing ordering
* Create a symbolic factor graph
*/
//SymbolicFactorGraph::shared_ptr symbolic() const;
/**
* Create a symbolic factor graph and initial variable ordering that can
* be used for graph operations like determining a fill-reducing ordering.
* The graph and ordering should be permuted after such a fill-reducing
* ordering is found.
*/
//std::pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr>
// symbolic(const Values& config) const;
boost::shared_ptr<SymbolicFactorGraph> symbolic() const;
/**
* Compute a fill-reducing ordering using COLAMD.

9
gtsam/symbolic/SymbolicFactor.h
View File

@ -52,7 +52,7 @@ namespace gtsam {
SymbolicFactor() {}
/** Construct unary factor */
SymbolicFactor(Key j) :
explicit SymbolicFactor(Key j) :
Base(boost::assign::cref_list_of<1>(j)) {}
/** Construct binary factor */
@ -75,6 +75,9 @@ namespace gtsam {
SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) :
Base(boost::assign::cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6)) {}
/** Create symbolic version of any factor */
explicit SymbolicFactor(const Factor& factor) : Base(factor.keys()) {}
virtual ~SymbolicFactor() {}
/// Copy this object as its actual derived type.
@ -91,10 +94,6 @@ namespace gtsam {
/// @name Advanced Constructors
/// @{
private:
explicit SymbolicFactor(const Base& base) :
Base(base) {}
public:
/** Constructor from a collection of keys */
template<typename KEYITERATOR>

19
tests/testNonlinearFactorGraph.cpp
View File

@ -31,8 +31,9 @@ using namespace boost::assign;
#include <gtsam/base/Matrix.h>
#include <tests/smallExample.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/symbolic/SymbolicFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
using namespace gtsam;
using namespace example;
@ -148,6 +149,22 @@ TEST( NonlinearFactorGraph, rekey )
EXPECT(assert_equal(expRekey, actRekey));
}
/* ************************************************************************* */
TEST( NonlinearFactorGraph, symbolic )
{
NonlinearFactorGraph graph = createNonlinearFactorGraph();
SymbolicFactorGraph expected;
expected.push_factor(X(1));
expected.push_factor(X(1), X(2));
expected.push_factor(X(1), L(1));
expected.push_factor(X(2), L(1));
SymbolicFactorGraph actual = *graph.symbolic();
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */