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

Renamed LinearFactor -> GaussianFactor, LinearFactorGraph -> GaussianFactorGraph

release/4.3a0
Alex Cunningham 2009-11-12 16:16:32 +00:00
parent 1ae6bb4030
commit 77a1754b69
38 changed files with 465 additions and 465 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.cproject
View File

@ -380,16 +380,16 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testLinearFactor.run</buildTarget>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testLinearFactorGraph.run</buildTarget>
<buildTarget>testGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -432,16 +432,16 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="timeGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>timeLinearFactor.run</buildTarget>
<buildTarget>timeGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="timeGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>timeLinearFactorGraph.run</buildTarget>
<buildTarget>timeGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>

54
cpp/LinearFactor.cpp → cpp/GaussianFactor.cpp
View File

@ -1,5 +1,5 @@
/**
* @file LinearFactor.cpp
* @file GaussianFactor.cpp
* @brief Linear Factor....A Gaussian
* @brief linearFactor
* @author Christian Potthast
@ -12,7 +12,7 @@
#include "Matrix.h"
#include "Ordering.h"
#include "ConditionalGaussian.h"
#include "LinearFactor.h"
#include "GaussianFactor.h"
using namespace std;
using namespace boost::assign;
@ -26,7 +26,7 @@ using namespace gtsam;
typedef pair<const string, Matrix>& mypair;
/* ************************************************************************* */
LinearFactor::LinearFactor(const boost::shared_ptr<ConditionalGaussian>& cg) :
GaussianFactor::GaussianFactor(const boost::shared_ptr<ConditionalGaussian>& cg) :
b_(cg->get_d()) {
As_.insert(make_pair(cg->key(), cg->get_R()));
std::map<std::string, Matrix>::const_iterator it = cg->parentsBegin();
@ -41,10 +41,10 @@ LinearFactor::LinearFactor(const boost::shared_ptr<ConditionalGaussian>& cg) :
}
/* ************************************************************************* */
LinearFactor::LinearFactor(const vector<shared_ptr> & factors)
GaussianFactor::GaussianFactor(const vector<shared_ptr> & factors)
{
bool verbose = false;
if (verbose) cout << "LinearFactor::LinearFactor (factors)" << endl;
if (verbose) cout << "GaussianFactor::GaussianFactor (factors)" << endl;
// Create RHS and sigmas of right size by adding together row counts
size_t m = 0;
@ -72,11 +72,11 @@ LinearFactor::LinearFactor(const vector<shared_ptr> & factors)
pos += mf;
}
if (verbose) cout << "LinearFactor::LinearFactor done" << endl;
if (verbose) cout << "GaussianFactor::GaussianFactor done" << endl;
}
/* ************************************************************************* */
void LinearFactor::print(const string& s) const {
void GaussianFactor::print(const string& s) const {
cout << s << endl;
if (empty()) cout << " empty" << endl;
else {
@ -88,7 +88,7 @@ void LinearFactor::print(const string& s) const {
}
/* ************************************************************************* */
size_t LinearFactor::getDim(const std::string& key) const {
size_t GaussianFactor::getDim(const std::string& key) const {
const_iterator it = As_.find(key);
if (it != As_.end())
return it->second.size2();
@ -98,9 +98,9 @@ size_t LinearFactor::getDim(const std::string& key) const {
/* ************************************************************************* */
// Check if two linear factors are equal
bool LinearFactor::equals(const Factor<VectorConfig>& f, double tol) const {
bool GaussianFactor::equals(const Factor<VectorConfig>& f, double tol) const {
const LinearFactor* lf = dynamic_cast<const LinearFactor*>(&f);
const GaussianFactor* lf = dynamic_cast<const GaussianFactor*>(&f);
if (lf == NULL) return false;
if (empty()) return (lf->empty());
@ -127,7 +127,7 @@ bool LinearFactor::equals(const Factor<VectorConfig>& f, double tol) const {
/* ************************************************************************* */
// we might have multiple As, so iterate and subtract from b
double LinearFactor::error(const VectorConfig& c) const {
double GaussianFactor::error(const VectorConfig& c) const {
if (empty()) return 0;
Vector e = b_;
string j; Matrix Aj;
@ -138,7 +138,7 @@ double LinearFactor::error(const VectorConfig& c) const {
}
/* ************************************************************************* */
list<string> LinearFactor::keys() const {
list<string> GaussianFactor::keys() const {
list<string> result;
string j; Matrix A;
FOREACH_PAIR(j,A,As_)
@ -147,7 +147,7 @@ list<string> LinearFactor::keys() const {
}
/* ************************************************************************* */
Dimensions LinearFactor::dimensions() const {
Dimensions GaussianFactor::dimensions() const {
Dimensions result;
string j; Matrix A;
FOREACH_PAIR(j,A,As_)
@ -156,7 +156,7 @@ Dimensions LinearFactor::dimensions() const {
}
/* ************************************************************************* */
void LinearFactor::tally_separator(const string& key, set<string>& separator) const {
void GaussianFactor::tally_separator(const string& key, set<string>& separator) const {
if(involves(key)) {
string j; Matrix A;
FOREACH_PAIR(j,A,As_)
@ -165,7 +165,7 @@ void LinearFactor::tally_separator(const string& key, set<string>& separator) co
}
/* ************************************************************************* */
pair<Matrix,Vector> LinearFactor::matrix(const Ordering& ordering, bool weight) const {
pair<Matrix,Vector> GaussianFactor::matrix(const Ordering& ordering, bool weight) const {
// get pointers to the matrices
vector<const Matrix *> matrices;
@ -189,7 +189,7 @@ pair<Matrix,Vector> LinearFactor::matrix(const Ordering& ordering, bool weight)
}
/* ************************************************************************* */
Matrix LinearFactor::matrix_augmented(const Ordering& ordering) const {
Matrix GaussianFactor::matrix_augmented(const Ordering& ordering) const {
// get pointers to the matrices
vector<const Matrix *> matrices;
BOOST_FOREACH(string j, ordering) {
@ -208,7 +208,7 @@ Matrix LinearFactor::matrix_augmented(const Ordering& ordering) const {
/* ************************************************************************* */
boost::tuple<list<int>, list<int>, list<double> >
LinearFactor::sparse(const Ordering& ordering, const Dimensions& variables) const {
GaussianFactor::sparse(const Ordering& ordering, const Dimensions& variables) const {
// declare return values
list<int> I,J;
@ -244,13 +244,13 @@ LinearFactor::sparse(const Ordering& ordering, const Dimensions& variables) cons
}
/* ************************************************************************* */
void LinearFactor::append_factor(LinearFactor::shared_ptr f, const size_t m,
void GaussianFactor::append_factor(GaussianFactor::shared_ptr f, const size_t m,
const size_t pos) {
bool verbose = false;
if (verbose) cout << "LinearFactor::append_factor" << endl;
if (verbose) cout << "GaussianFactor::append_factor" << endl;
// iterate over all matrices from the factor f
LinearFactor::const_iterator it = f->begin();
GaussianFactor::const_iterator it = f->begin();
for (; it != f->end(); it++) {
string j = it->first;
Matrix A = it->second;
@ -274,7 +274,7 @@ void LinearFactor::append_factor(LinearFactor::shared_ptr f, const size_t m,
if (exists) As_.erase(j);
insert(j, Anew);
}
if (verbose) cout << "LinearFactor::append_factor done" << endl;
if (verbose) cout << "GaussianFactor::append_factor done" << endl;
}
@ -286,17 +286,17 @@ void LinearFactor::append_factor(LinearFactor::shared_ptr f, const size_t m,
* and last rows to make a new joint linear factor on separator
*/
/* ************************************************************************* */
pair<ConditionalGaussian::shared_ptr, LinearFactor::shared_ptr>
LinearFactor::eliminate(const string& key) const
pair<ConditionalGaussian::shared_ptr, GaussianFactor::shared_ptr>
GaussianFactor::eliminate(const string& key) const
{
bool verbose = false;
if (verbose) cout << "LinearFactor::eliminate(" << key << ")" << endl;
if (verbose) cout << "GaussianFactor::eliminate(" << key << ")" << endl;
// if this factor does not involve key, we exit with empty CG and LF
const_iterator it = As_.find(key);
if (it==As_.end()) {
// Conditional Gaussian is just a parent-less node with P(x)=1
LinearFactor::shared_ptr lf(new LinearFactor);
GaussianFactor::shared_ptr lf(new GaussianFactor);
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key));
return make_pair(cg,lf);
}
@ -324,7 +324,7 @@ LinearFactor::eliminate(const string& key) const
// if m<n1, this factor cannot be eliminated
size_t maxRank = solution.size();
if (maxRank<n1)
throw(domain_error("LinearFactor::eliminate: fewer constraints than unknowns"));
throw(domain_error("GaussianFactor::eliminate: fewer constraints than unknowns"));
// unpack the solutions
Matrix R(maxRank, n);
@ -345,7 +345,7 @@ LinearFactor::eliminate(const string& key) const
sub(newSigmas, 0, n1))); // get standard deviations
// extract the block matrices for parents in both CG and LF
LinearFactor::shared_ptr lf(new LinearFactor);
GaussianFactor::shared_ptr lf(new GaussianFactor);
size_t j = n1;
BOOST_FOREACH(string cur_key, ordering)
if (cur_key!=key) {

32
cpp/LinearFactor.h → cpp/GaussianFactor.h
View File

@ -1,5 +1,5 @@
/**
* @file LinearFactor.h
* @file GaussianFactor.h
* @brief Linear Factor....A Gaussian
* @brief linearFactor
* @author Christian Potthast
@ -24,13 +24,13 @@ namespace gtsam {
/**
* Base Class for a linear factor.
* LinearFactor is non-mutable (all methods const!).
* GaussianFactor is non-mutable (all methods const!).
* The factor value is exp(-0.5*||Ax-b||^2)
*/
class LinearFactor: public Factor<VectorConfig> {
class GaussianFactor: public Factor<VectorConfig> {
public:
typedef boost::shared_ptr<LinearFactor> shared_ptr;
typedef boost::shared_ptr<GaussianFactor> shared_ptr;
typedef std::map<std::string, Matrix>::iterator iterator;
typedef std::map<std::string, Matrix>::const_iterator const_iterator;
@ -43,23 +43,23 @@ protected:
public:
// TODO: eradicate, as implies non-const
LinearFactor() {
GaussianFactor() {
}
/** Construct Null factor */
LinearFactor(const Vector& b_in) :
GaussianFactor(const Vector& b_in) :
b_(b_in), sigmas_(ones(b_in.size())){
}
/** Construct unary factor */
LinearFactor(const std::string& key1, const Matrix& A1,
GaussianFactor(const std::string& key1, const Matrix& A1,
const Vector& b_in, double sigma) :
b_(b_in), sigmas_(repeat(b_in.size(),sigma)) {
As_.insert(make_pair(key1, A1));
}
/** Construct binary factor */
LinearFactor(const std::string& key1, const Matrix& A1,
GaussianFactor(const std::string& key1, const Matrix& A1,
const std::string& key2, const Matrix& A2,
const Vector& b_in, double sigma) :
b_(b_in), sigmas_(repeat(b_in.size(),sigma)) {
@ -68,7 +68,7 @@ public:
}
/** Construct ternary factor */
LinearFactor(const std::string& key1, const Matrix& A1,
GaussianFactor(const std::string& key1, const Matrix& A1,
const std::string& key2, const Matrix& A2,
const std::string& key3, const Matrix& A3,
const Vector& b_in, double sigma) :
@ -79,7 +79,7 @@ public:
}
/** Construct an n-ary factor */
LinearFactor(const std::vector<std::pair<std::string, Matrix> > &terms,
GaussianFactor(const std::vector<std::pair<std::string, Matrix> > &terms,
const Vector &b_in, double sigma) :
b_(b_in), sigmas_(repeat(b_in.size(),sigma)) {
for(unsigned int i=0; i<terms.size(); i++)
@ -87,7 +87,7 @@ public:
}
/** Construct an n-ary factor with a multiple sigmas*/
LinearFactor(const std::vector<std::pair<std::string, Matrix> > &terms,
GaussianFactor(const std::vector<std::pair<std::string, Matrix> > &terms,
const Vector &b_in, const Vector& sigmas) :
b_(b_in), sigmas_(sigmas) {
for (unsigned int i = 0; i < terms.size(); i++)
@ -95,13 +95,13 @@ public:
}
/** Construct from Conditional Gaussian */
LinearFactor(const boost::shared_ptr<ConditionalGaussian>& cg);
GaussianFactor(const boost::shared_ptr<ConditionalGaussian>& cg);
/**
* Constructor that combines a set of factors
* @param factors Set of factors to combine
*/
LinearFactor(const std::vector<shared_ptr> & factors);
GaussianFactor(const std::vector<shared_ptr> & factors);
// Implementing Testable virtual functions
@ -135,7 +135,7 @@ public:
const Matrix& get_A(const std::string& key) const {
const_iterator it = As_.find(key);
if (it == As_.end())
throw(std::invalid_argument("LinearFactor::[] invalid key: " + key));
throw(std::invalid_argument("GaussianFactor::[] invalid key: " + key));
return it->second;
}
@ -241,10 +241,10 @@ public:
* @param m final number of rows of f, needs to be known in advance
* @param pos where to insert in the m-sized matrices
*/
void append_factor(LinearFactor::shared_ptr f, const size_t m,
void append_factor(GaussianFactor::shared_ptr f, const size_t m,
const size_t pos);
}; // LinearFactor
}; // GaussianFactor
/* ************************************************************************* */

50
cpp/LinearFactorGraph.cpp → cpp/GaussianFactorGraph.cpp
View File

@ -1,5 +1,5 @@
/**
* @file LinearFactorGraph.cpp
* @file GaussianFactorGraph.cpp
* @brief Linear Factor Graph where all factors are Gaussians
* @author Kai Ni
* @author Christian Potthast
@ -12,8 +12,8 @@
#include <colamd/colamd.h>
#include "LinearFactorGraph.h"
#include "LinearFactorSet.h"
#include "GaussianFactorGraph.h"
#include "GaussianFactorSet.h"
#include "FactorGraph-inl.h"
#include "inference-inl.h"
@ -24,15 +24,15 @@ using namespace gtsam;
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
// Explicitly instantiate so we don't have to include everywhere
template class FactorGraph<LinearFactor>;
template class FactorGraph<GaussianFactor>;
/* ************************************************************************* */
LinearFactorGraph::LinearFactorGraph(const GaussianBayesNet& CBN) :
FactorGraph<LinearFactor> (CBN) {
GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) :
FactorGraph<GaussianFactor> (CBN) {
}
/* ************************************************************************* */
set<string> LinearFactorGraph::find_separator(const string& key) const
set<string> GaussianFactorGraph::find_separator(const string& key) const
{
set<string> separator;
BOOST_FOREACH(sharedFactor factor,factors_)
@ -43,13 +43,13 @@ set<string> LinearFactorGraph::find_separator(const string& key) const
/* ************************************************************************* */
ConditionalGaussian::shared_ptr
LinearFactorGraph::eliminateOne(const std::string& key) {
return gtsam::eliminateOne<LinearFactor,ConditionalGaussian>(*this, key);
GaussianFactorGraph::eliminateOne(const std::string& key) {
return gtsam::eliminateOne<GaussianFactor,ConditionalGaussian>(*this, key);
}
/* ************************************************************************* */
GaussianBayesNet
LinearFactorGraph::eliminate(const Ordering& ordering)
GaussianFactorGraph::eliminate(const Ordering& ordering)
{
GaussianBayesNet chordalBayesNet; // empty
BOOST_FOREACH(string key, ordering) {
@ -60,7 +60,7 @@ LinearFactorGraph::eliminate(const Ordering& ordering)
}
/* ************************************************************************* */
VectorConfig LinearFactorGraph::optimize(const Ordering& ordering)
VectorConfig GaussianFactorGraph::optimize(const Ordering& ordering)
{
// eliminate all nodes in the given ordering -> chordal Bayes net
GaussianBayesNet chordalBayesNet = eliminate(ordering);
@ -71,7 +71,7 @@ VectorConfig LinearFactorGraph::optimize(const Ordering& ordering)
/* ************************************************************************* */
boost::shared_ptr<GaussianBayesNet>
LinearFactorGraph::eliminate_(const Ordering& ordering)
GaussianFactorGraph::eliminate_(const Ordering& ordering)
{
boost::shared_ptr<GaussianBayesNet> chordalBayesNet(new GaussianBayesNet); // empty
BOOST_FOREACH(string key, ordering) {
@ -83,23 +83,23 @@ LinearFactorGraph::eliminate_(const Ordering& ordering)
/* ************************************************************************* */
boost::shared_ptr<VectorConfig>
LinearFactorGraph::optimize_(const Ordering& ordering) {
GaussianFactorGraph::optimize_(const Ordering& ordering) {
return boost::shared_ptr<VectorConfig>(new VectorConfig(optimize(ordering)));
}
/* ************************************************************************* */
void LinearFactorGraph::combine(const LinearFactorGraph &lfg){
void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg){
for(const_iterator factor=lfg.factors_.begin(); factor!=lfg.factors_.end(); factor++){
push_back(*factor);
}
}
/* ************************************************************************* */
LinearFactorGraph LinearFactorGraph::combine2(const LinearFactorGraph& lfg1,
const LinearFactorGraph& lfg2) {
GaussianFactorGraph GaussianFactorGraph::combine2(const GaussianFactorGraph& lfg1,
const GaussianFactorGraph& lfg2) {
// create new linear factor graph equal to the first one
LinearFactorGraph fg = lfg1;
GaussianFactorGraph fg = lfg1;
// add the second factors_ in the graph
for (const_iterator factor = lfg2.factors_.begin(); factor
@ -111,7 +111,7 @@ LinearFactorGraph LinearFactorGraph::combine2(const LinearFactorGraph& lfg1,
/* ************************************************************************* */
Dimensions LinearFactorGraph::dimensions() const {
Dimensions GaussianFactorGraph::dimensions() const {
Dimensions result;
BOOST_FOREACH(sharedFactor factor,factors_) {
Dimensions vs = factor->dimensions();
@ -122,10 +122,10 @@ Dimensions LinearFactorGraph::dimensions() const {
}
/* ************************************************************************* */
LinearFactorGraph LinearFactorGraph::add_priors(double sigma) const {
GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma) const {
// start with this factor graph
LinearFactorGraph result = *this;
GaussianFactorGraph result = *this;
// find all variables and their dimensions
Dimensions vs = dimensions();
@ -135,29 +135,29 @@ LinearFactorGraph LinearFactorGraph::add_priors(double sigma) const {
FOREACH_PAIR(key,dim,vs) {
Matrix A = eye(dim);
Vector b = zero(dim);
sharedFactor prior(new LinearFactor(key,A,b, sigma));
sharedFactor prior(new GaussianFactor(key,A,b, sigma));
result.push_back(prior);
}
return result;
}
/* ************************************************************************* */
pair<Matrix,Vector> LinearFactorGraph::matrix(const Ordering& ordering) const {
pair<Matrix,Vector> GaussianFactorGraph::matrix(const Ordering& ordering) const {
// get all factors
LinearFactorSet found;
GaussianFactorSet found;
BOOST_FOREACH(sharedFactor factor,factors_)
found.push_back(factor);
// combine them
LinearFactor lf(found);
GaussianFactor lf(found);
// Return Matrix and Vector
return lf.matrix(ordering);
}
/* ************************************************************************* */
Matrix LinearFactorGraph::sparse(const Ordering& ordering) const {
Matrix GaussianFactorGraph::sparse(const Ordering& ordering) const {
// return values
list<int> I,J;

24
cpp/LinearFactorGraph.h → cpp/GaussianFactorGraph.h
View File

@ -1,12 +1,12 @@
/**
* @file LinearFactorGraph.h
* @file GaussianFactorGraph.h
* @brief Linear Factor Graph where all factors are Gaussians
* @author Kai Ni
* @author Christian Potthast
* @author Alireza Fathi
*/
// $Id: LinearFactorGraph.h,v 1.24 2009/08/14 20:48:51 acunning Exp $
// $Id: GaussianFactorGraph.h,v 1.24 2009/08/14 20:48:51 acunning Exp $
// \callgraph
@ -14,7 +14,7 @@
#include <boost/shared_ptr.hpp>
#include "FactorGraph.h"
#include "LinearFactor.h"
#include "GaussianFactor.h"
#include "GaussianBayesNet.h" // needed for MATLAB toolbox !!
namespace gtsam {
@ -23,22 +23,22 @@ namespace gtsam {
/**
* A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
* Factor == LinearFactor
* Factor == GaussianFactor
* VectorConfig = A configuration of vectors
* Most of the time, linear factor graphs arise by linearizing a non-linear factor graph.
*/
class LinearFactorGraph : public FactorGraph<LinearFactor> {
class GaussianFactorGraph : public FactorGraph<GaussianFactor> {
public:
/**
* Default constructor
*/
LinearFactorGraph() {}
GaussianFactorGraph() {}
/**
* Constructor that receives a Chordal Bayes Net and returns a LinearFactorGraph
* Constructor that receives a Chordal Bayes Net and returns a GaussianFactorGraph
*/
LinearFactorGraph(const GaussianBayesNet& CBN);
GaussianFactorGraph(const GaussianBayesNet& CBN);
/** unnormalized error */
double error(const VectorConfig& c) const {
@ -93,14 +93,14 @@ namespace gtsam {
* @param const &lfg2 Linear factor graph
* @return a new combined factor graph
*/
static LinearFactorGraph combine2(const LinearFactorGraph& lfg1,
const LinearFactorGraph& lfg2);
static GaussianFactorGraph combine2(const GaussianFactorGraph& lfg1,
const GaussianFactorGraph& lfg2);
/**
* combine two factor graphs
* @param *lfg Linear factor graph
*/
void combine(const LinearFactorGraph &lfg);
void combine(const GaussianFactorGraph &lfg);
/**
* Find all variables and their dimensions
@ -112,7 +112,7 @@ namespace gtsam {
* Add zero-mean i.i.d. Gaussian prior terms to each variable
* @param sigma Standard deviation of Gaussian
*/
LinearFactorGraph add_priors(double sigma) const;
GaussianFactorGraph add_priors(double sigma) const;
/**
* Return (dense) matrix associated with factor graph

10
cpp/LinearFactorSet.h → cpp/GaussianFactorSet.h
View File

@ -1,5 +1,5 @@
/**
* @file LinearFactorSet.h
* @file GaussianFactorSet.h
* @brief Utility class: an STL set of linear factors, basically a wrappable typedef
* @author Frank Dellaert
*/
@ -8,14 +8,14 @@
#include <set>
#include <boost/shared_ptr.hpp>
#include "LinearFactor.h"
#include "GaussianFactor.h"
namespace gtsam {
class LinearFactor;
class GaussianFactor;
// We use a vector not a an STL set, to get predictable ordering across platforms
struct LinearFactorSet : std::vector<boost::shared_ptr<LinearFactor> > {
LinearFactorSet() {}
struct GaussianFactorSet : std::vector<boost::shared_ptr<GaussianFactor> > {
GaussianFactorSet() {}
};
}

26
cpp/Makefile.am
View File

@ -84,29 +84,29 @@ testSymbolicBayesNet_SOURCES = $(example) testSymbolicBayesNet.cpp
testSymbolicBayesNet_LDADD = libgtsam.la
# Gaussian inference
headers += LinearFactorSet.h
sources += VectorConfig.cpp LinearFactor.cpp LinearFactorGraph.cpp ConditionalGaussian.cpp GaussianBayesNet.cpp
check_PROGRAMS += testVectorConfig testLinearFactor testLinearFactorGraph testConditionalGaussian testGaussianBayesNet
headers += GaussianFactorSet.h
sources += VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp ConditionalGaussian.cpp GaussianBayesNet.cpp
check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testConditionalGaussian testGaussianBayesNet
testVectorConfig_SOURCES = testVectorConfig.cpp
testVectorConfig_LDADD = libgtsam.la
testLinearFactor_SOURCES = $(example) testLinearFactor.cpp
testLinearFactor_LDADD = libgtsam.la
testLinearFactorGraph_SOURCES = $(example) testLinearFactorGraph.cpp
testLinearFactorGraph_LDADD = libgtsam.la
testGaussianFactor_SOURCES = $(example) testGaussianFactor.cpp
testGaussianFactor_LDADD = libgtsam.la
testGaussianFactorGraph_SOURCES = $(example) testGaussianFactorGraph.cpp
testGaussianFactorGraph_LDADD = libgtsam.la
testConditionalGaussian_SOURCES = $(example) testConditionalGaussian.cpp
testConditionalGaussian_LDADD = libgtsam.la
testGaussianBayesNet_SOURCES = $(example) testGaussianBayesNet.cpp
testGaussianBayesNet_LDADD = libgtsam.la
# not the correct way, I'm sure: Kai ?
timeLinearFactor: timeLinearFactor.cpp
timeLinearFactor: CXXFLAGS += -I /opt/local/include
timeLinearFactor: LDFLAGS += -L.libs -lgtsam
timeGaussianFactor: timeGaussianFactor.cpp
timeGaussianFactor: CXXFLAGS += -I /opt/local/include
timeGaussianFactor: LDFLAGS += -L.libs -lgtsam
# not the correct way, I'm sure: Kai ?
timeLinearFactorGraph: timeLinearFactorGraph.cpp
timeLinearFactorGraph: CXXFLAGS += -I /opt/local/include -I ..
timeLinearFactorGraph: LDFLAGS += SmallExample.o -L.libs -lgtsam -L../CppUnitLite -lCppUnitLite
timeGaussianFactorGraph: timeGaussianFactorGraph.cpp
timeGaussianFactorGraph: CXXFLAGS += -I /opt/local/include -I ..
timeGaussianFactorGraph: LDFLAGS += smallExample.o -L.libs -lgtsam -L../CppUnitLite -lCppUnitLite
# Nonlinear inference
headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h

2
cpp/Matrix.cpp
View File

@ -264,7 +264,7 @@ pair<Matrix,Matrix> qr(const Matrix& A) {
* i.e. do outer product update A = (I-beta vv')*A = A - v*(beta*A'*v)' = A - v*w'
* but only in relevant part, from row j onwards
* If called from householder_ does actually more work as first j columns
* will not be touched. However, is called from LinearFactor.eliminate
* will not be touched. However, is called from GaussianFactor.eliminate
* on a number of different matrices for which all columns change.
*/
/* ************************************************************************* */

12
cpp/NonlinearFactor.cpp
View File

@ -26,7 +26,7 @@ NonlinearFactor1::NonlinearFactor1(const Vector& z,
/* ************************************************************************* */
void NonlinearFactor1::print(const string& s) const {
cout << "NonLinearFactor1 " << s << endl;
cout << "NonGaussianFactor1 " << s << endl;
cout << "h : " << (void*)h_ << endl;
cout << "key: " << key_ << endl;
cout << "H : " << (void*)H_ << endl;
@ -34,7 +34,7 @@ void NonlinearFactor1::print(const string& s) const {
}
/* ************************************************************************* */
LinearFactor::shared_ptr NonlinearFactor1::linearize(const VectorConfig& c) const {
GaussianFactor::shared_ptr NonlinearFactor1::linearize(const VectorConfig& c) const {
// get argument 1 from config
Vector x1 = c[key_];
@ -44,7 +44,7 @@ LinearFactor::shared_ptr NonlinearFactor1::linearize(const VectorConfig& c) cons
// Right-hand-side b = error(c) = (z - h(x1))/sigma
Vector b = (z_ - h_(x1));
LinearFactor::shared_ptr p(new LinearFactor(key_, A, b, sigma_));
GaussianFactor::shared_ptr p(new GaussianFactor(key_, A, b, sigma_));
return p;
}
@ -77,7 +77,7 @@ NonlinearFactor2::NonlinearFactor2(const Vector& z,
/* ************************************************************************* */
void NonlinearFactor2::print(const string& s) const {
cout << "NonLinearFactor2 " << s << endl;
cout << "NonGaussianFactor2 " << s << endl;
cout << "h : " << (void*)h_ << endl;
cout << "key1: " << key1_ << endl;
cout << "H2 : " << (void*)H2_ << endl;
@ -87,7 +87,7 @@ void NonlinearFactor2::print(const string& s) const {
}
/* ************************************************************************* */
LinearFactor::shared_ptr NonlinearFactor2::linearize(const VectorConfig& c) const {
GaussianFactor::shared_ptr NonlinearFactor2::linearize(const VectorConfig& c) const {
// get arguments from config
Vector x1 = c[key1_];
Vector x2 = c[key2_];
@ -99,7 +99,7 @@ LinearFactor::shared_ptr NonlinearFactor2::linearize(const VectorConfig& c) cons
// Right-hand-side b = (z - h(x))/sigma
Vector b = (z_ - h_(x1, x2));
LinearFactor::shared_ptr p(new LinearFactor(key1_, A1, key2_, A2, b, sigma_));
GaussianFactor::shared_ptr p(new GaussianFactor(key1_, A1, key2_, A2, b, sigma_));
return p;
}

18
cpp/NonlinearFactor.h
View File

@ -20,7 +20,7 @@
#include "Factor.h"
#include "Vector.h"
#include "Matrix.h"
#include "LinearFactor.h"
#include "GaussianFactor.h"
/**
* Base Class
@ -29,9 +29,9 @@
namespace gtsam {
// forward declaration of LinearFactor
//class LinearFactor;
//typedef boost::shared_ptr<LinearFactor> shared_ptr;
// forward declaration of GaussianFactor
//class GaussianFactor;
//typedef boost::shared_ptr<GaussianFactor> shared_ptr;
/**
* Nonlinear factor which assumes Gaussian noise on a measurement
@ -66,7 +66,7 @@ namespace gtsam {
/** print */
void print(const std::string& s = "") const {
std::cout << "NonLinearFactor " << s << std::endl;
std::cout << "NonGaussianFactor " << s << std::endl;
gtsam::print(z_, " z = ");
std::cout << " sigma = " << sigma_ << std::endl;
}
@ -81,8 +81,8 @@ namespace gtsam {
/** Vector of errors */
virtual Vector error_vector(const Config& c) const = 0;
/** linearize to a LinearFactor */
virtual boost::shared_ptr<LinearFactor> linearize(const Config& c) const = 0;
/** linearize to a GaussianFactor */
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const = 0;
/** get functions */
double sigma() const {return sigma_;}
@ -145,7 +145,7 @@ namespace gtsam {
}
/** linearize a non-linearFactor1 to get a linearFactor1 */
boost::shared_ptr<LinearFactor> linearize(const VectorConfig& c) const;
boost::shared_ptr<GaussianFactor> linearize(const VectorConfig& c) const;
};
/**
@ -187,7 +187,7 @@ namespace gtsam {
}
/** Linearize a non-linearFactor2 to get a linearFactor2 */
boost::shared_ptr<LinearFactor> linearize(const VectorConfig& c) const;
boost::shared_ptr<GaussianFactor> linearize(const VectorConfig& c) const;
};
/* ************************************************************************* */

8
cpp/NonlinearFactorGraph-inl.h
View File

@ -8,7 +8,7 @@
#pragma once
#include "LinearFactorGraph.h"
#include "GaussianFactorGraph.h"
#include "NonlinearFactorGraph.h"
namespace gtsam {
@ -28,21 +28,21 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class Config>
LinearFactorGraph NonlinearFactorGraph<Config>::linearize(
GaussianFactorGraph NonlinearFactorGraph<Config>::linearize(
const Config& config) const {
// TODO speed up the function either by returning a pointer or by
// returning the linearisation as a second argument and returning
// the reference
// create an empty linear FG
LinearFactorGraph linearFG;
GaussianFactorGraph linearFG;
typedef typename FactorGraph<NonlinearFactor<Config> >::const_iterator
const_iterator;
// linearize all factors
for (const_iterator factor = this->factors_.begin(); factor
< this->factors_.end(); factor++) {
boost::shared_ptr<LinearFactor> lf = (*factor)->linearize(config);
boost::shared_ptr<GaussianFactor> lf = (*factor)->linearize(config);
linearFG.push_back(lf);
}

6
cpp/NonlinearFactorGraph.h
View File

@ -11,13 +11,13 @@
#pragma once
#include "NonlinearFactor.h"
#include "LinearFactorGraph.h"
#include "GaussianFactorGraph.h"
namespace gtsam {
/**
* A non-linear factor graph is templated on a configuration, but the factor type
* is fixed as a NonLinearFactor. The configurations are typically (in SAM) more general
* is fixed as a NonGaussianFactor. The configurations are typically (in SAM) more general
* than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds.
* Linearizing the non-linear factor graph creates a linear factor graph on the
* tangent vector space at the linearization point. Because the tangent space is a true
@ -39,7 +39,7 @@ namespace gtsam {
/**
* linearize a nonlinear factor graph
*/
LinearFactorGraph linearize(const Config& config) const;
GaussianFactorGraph linearize(const Config& config) const;
};

8
cpp/NonlinearOptimizer-inl.h
View File

@ -58,7 +58,7 @@ namespace gtsam {
// linearize the non-linear graph around the current config
// which gives a linear optimization problem in the tangent space
LinearFactorGraph linear = graph_->linearize(*config_);
GaussianFactorGraph linear = graph_->linearize(*config_);
// solve for the optimal displacement in the tangent space
VectorConfig delta = linear.optimize(*ordering_);
@ -118,13 +118,13 @@ namespace gtsam {
/* ************************************************************************* */
template<class G, class C>
NonlinearOptimizer<G, C> NonlinearOptimizer<G, C>::try_lambda(
const LinearFactorGraph& linear, verbosityLevel verbosity, double factor) const {
const GaussianFactorGraph& linear, verbosityLevel verbosity, double factor) const {
if (verbosity >= TRYLAMBDA)
cout << "trying lambda = " << lambda_ << endl;
// add prior-factors
LinearFactorGraph damped = linear.add_priors(1.0/sqrt(lambda_));
GaussianFactorGraph damped = linear.add_priors(1.0/sqrt(lambda_));
if (verbosity >= DAMPED)
damped.print("damped");
@ -167,7 +167,7 @@ namespace gtsam {
cout << "lambda = " << lambda_ << endl;
// linearize all factors once
LinearFactorGraph linear = graph_->linearize(*config_);
GaussianFactorGraph linear = graph_->linearize(*config_);
if (verbosity >= LINEAR)
linear.print("linear");

2
cpp/NonlinearOptimizer.h
View File

@ -62,7 +62,7 @@ namespace gtsam {
double lambda_;
// Recursively try to do tempered Gauss-Newton steps until we succeed
NonlinearOptimizer try_lambda(const LinearFactorGraph& linear,
NonlinearOptimizer try_lambda(const GaussianFactorGraph& linear,
verbosityLevel verbosity, double factor) const;
public:

2
cpp/Testable.h
View File

@ -32,7 +32,7 @@ namespace gtsam {
/**
* equality up to tolerance
* tricky to implement, see NonLinearFactor1 for an example
* tricky to implement, see NonGaussianFactor1 for an example
* equals is not supposed to print out *anything*, just return true|false
*/
virtual bool equals(const Derived& expected, double tol) const = 0;

6
cpp/VSLAMFactor.cpp
View File

@ -57,7 +57,7 @@ Vector VSLAMFactor::error_vector(const VSLAMConfig& c) const {
}
/* ************************************************************************* */
LinearFactor::shared_ptr VSLAMFactor::linearize(const VSLAMConfig& c) const
GaussianFactor::shared_ptr VSLAMFactor::linearize(const VSLAMConfig& c) const
{
// get arguments from config
Pose3 pose = c.cameraPose(cameraFrameNumber_);
@ -74,8 +74,8 @@ LinearFactor::shared_ptr VSLAMFactor::linearize(const VSLAMConfig& c) const
Vector b = this->z_ - h;
// Make new linearfactor, divide by sigma
LinearFactor::shared_ptr
p(new LinearFactor(cameraFrameName_, Dh1, landmarkName_, Dh2, b, this->sigma_));
GaussianFactor::shared_ptr
p(new GaussianFactor(cameraFrameName_, Dh1, landmarkName_, Dh2, b, this->sigma_));
return p;
}

4
cpp/VSLAMFactor.h
View File

@ -7,7 +7,7 @@
#pragma once
#include "NonlinearFactor.h"
#include "LinearFactor.h"
#include "GaussianFactor.h"
#include "Cal3_S2.h"
#include "Testable.h"
@ -69,7 +69,7 @@ class VSLAMFactor : public NonlinearFactor<VSLAMConfig>, Testable<VSLAMFactor>
/**
* linerarization
*/
LinearFactor::shared_ptr linearize(const VSLAMConfig&) const;
GaussianFactor::shared_ptr linearize(const VSLAMConfig&) const;
int getCameraFrameNumber() const { return cameraFrameNumber_; }
int getLandmarkNumber() const { return landmarkNumber_; }

30
cpp/gtsam.h
View File

@ -18,23 +18,23 @@ class VectorConfig {
void clear();
};
class LinearFactorSet {
LinearFactorSet();
void push_back(LinearFactor* factor);
class GaussianFactorSet {
GaussianFactorSet();
void push_back(GaussianFactor* factor);
};
class LinearFactor {
LinearFactor(string key1,
class GaussianFactor {
GaussianFactor(string key1,
Matrix A1,
Vector b_in,
double sigma);
LinearFactor(string key1,
GaussianFactor(string key1,
Matrix A1,
string key2,
Matrix A2,
Vector b_in,
double sigma);
LinearFactor(string key1,
GaussianFactor(string key1,
Matrix A1,
string key2,
Matrix A2,
@ -48,7 +48,7 @@ class LinearFactor {
double error(const VectorConfig& c) const;
bool involves(string key) const;
void print(string s) const;
bool equals(const LinearFactor& lf, double tol) const;
bool equals(const GaussianFactor& lf, double tol) const;
pair<Matrix,Vector> matrix(const Ordering& ordering) const;
};
@ -86,15 +86,15 @@ class GaussianBayesNet {
void push_front(ConditionalGaussian* conditional);
};
class LinearFactorGraph {
LinearFactorGraph();
class GaussianFactorGraph {
GaussianFactorGraph();
size_t size() const;
void push_back(LinearFactor* ptr_f);
void push_back(GaussianFactor* ptr_f);
double error(const VectorConfig& c) const;
double probPrime(const VectorConfig& c) const;
void print(string s) const;
bool equals(const LinearFactorGraph& lfgraph, double tol) const;
bool equals(const GaussianFactorGraph& lfgraph, double tol) const;
ConditionalGaussian* eliminateOne(string key);
GaussianBayesNet* eliminate_(const Ordering& ordering);
@ -128,7 +128,7 @@ class Point3 {
class Point2Prior {
Point2Prior(Vector mu, double sigma, string key);
Vector error_vector(const VectorConfig& c) const;
LinearFactor* linearize(const VectorConfig& c) const;
GaussianFactor* linearize(const VectorConfig& c) const;
double sigma();
Vector measurement();
double error(const VectorConfig& c) const;
@ -138,7 +138,7 @@ class Point2Prior {
class Simulated2DOdometry {
Simulated2DOdometry(Vector odo, double sigma, string key, string key2);
Vector error_vector(const VectorConfig& c) const;
LinearFactor* linearize(const VectorConfig& c) const;
GaussianFactor* linearize(const VectorConfig& c) const;
double sigma();
Vector measurement();
double error(const VectorConfig& c) const;
@ -148,7 +148,7 @@ class Simulated2DOdometry {
class Simulated2DMeasurement {
Simulated2DMeasurement(Vector odo, double sigma, string key, string key2);
Vector error_vector(const VectorConfig& c) const;
LinearFactor* linearize(const VectorConfig& c) const;
GaussianFactor* linearize(const VectorConfig& c) const;
double sigma();
Vector measurement();
double error(const VectorConfig& c) const;

2
cpp/inference-inl.h
View File

@ -35,7 +35,7 @@ namespace gtsam {
}
/* ************************************************************************* */
// This doubly templated function is generic. There is a LinearFactorGraph
// This doubly templated function is generic. There is a GaussianFactorGraph
// version that returns a more specific GaussianBayesNet.
// Note, you will need to include this file to instantiate the function.
/* ************************************************************************* */

8
cpp/manual.mk
View File

@ -19,9 +19,9 @@ CXXFLAGS += -DBOOST_UBLAS_NDEBUG
# basic
sources = Vector.cpp svdcmp.cpp Matrix.cpp numericalDerivative.cpp Ordering.cpp
# nodes
sources += FGConfig.cpp LinearFactor.cpp ConditionalGaussian.cpp NonlinearFactor.cpp
sources += FGConfig.cpp GaussianFactor.cpp ConditionalGaussian.cpp NonlinearFactor.cpp
# graphs
sources += FactorGraph.cpp LinearFactorGraph.cpp NonlinearFactorGraph.cpp ChordalBayesNet.cpp
sources += FactorGraph.cpp GaussianFactorGraph.cpp NonlinearFactorGraph.cpp ChordalBayesNet.cpp
# geometry
sources += Point2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
@ -109,8 +109,8 @@ tests: unit-tests timing-tests
clean-tests:
-rm -rf $(executables)
# make a version of timeLinearFactor instrumented for Saturn profiler
saturn: timeLinearFactor
# make a version of timeGaussianFactor instrumented for Saturn profiler
saturn: timeGaussianFactor
saturn: CXXFLAGS += -finstrument-functions
saturn: LDLIBS += -lSaturn

56
cpp/smallExample.cpp
View File

@ -121,12 +121,12 @@ VectorConfig createZeroDelta() {
}
/* ************************************************************************* */
LinearFactorGraph createLinearFactorGraph()
GaussianFactorGraph createGaussianFactorGraph()
{
VectorConfig c = createNoisyConfig();
// Create
LinearFactorGraph fg;
GaussianFactorGraph fg;
double sigma1 = 0.1;
@ -137,7 +137,7 @@ LinearFactorGraph createLinearFactorGraph()
Vector b = - c["x1"];
LinearFactor::shared_ptr f1(new LinearFactor("x1", A11, b, sigma1));
GaussianFactor::shared_ptr f1(new GaussianFactor("x1", A11, b, sigma1));
fg.push_back(f1);
// odometry between x1 and x2
@ -153,7 +153,7 @@ LinearFactorGraph createLinearFactorGraph()
// Vector b(2);
b(0) = 0.2 ; b(1) = -0.1;
LinearFactor::shared_ptr f2(new LinearFactor("x1", A21, "x2", A22, b, sigma2));
GaussianFactor::shared_ptr f2(new GaussianFactor("x1", A21, "x2", A22, b, sigma2));
fg.push_back(f2);
// measurement between x1 and l1
@ -168,7 +168,7 @@ LinearFactorGraph createLinearFactorGraph()
b(0) = 0 ; b(1) = 0.2;
LinearFactor::shared_ptr f3(new LinearFactor("x1", A31, "l1", A32, b, sigma3));
GaussianFactor::shared_ptr f3(new GaussianFactor("x1", A31, "l1", A32, b, sigma3));
fg.push_back(f3);
// measurement between x2 and l1
@ -183,7 +183,7 @@ LinearFactorGraph createLinearFactorGraph()
b(0)= -0.2 ; b(1) = 0.3;
LinearFactor::shared_ptr f4(new LinearFactor("x2", A41, "l1", A42, b, sigma4));
GaussianFactor::shared_ptr f4(new GaussianFactor("x2", A41, "l1", A42, b, sigma4));
fg.push_back(f4);
return fg;
@ -245,7 +245,7 @@ ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph() {
}
/* ************************************************************************* */
LinearFactorGraph createSmoother(int T) {
GaussianFactorGraph createSmoother(int T) {
// noise on measurements and odometry, respectively
double sigma1 = 1, sigma2 = 1;
@ -277,12 +277,12 @@ LinearFactorGraph createSmoother(int T) {
poses.insert(key, xt);
}
LinearFactorGraph lfg = nlfg.linearize(poses);
GaussianFactorGraph lfg = nlfg.linearize(poses);
return lfg;
}
/* ************************************************************************* */
LinearFactorGraph createSimpleConstraintGraph() {
GaussianFactorGraph createSimpleConstraintGraph() {
// create unary factor
// prior on "x", mean = [1,-1], sigma=0.1
double sigma = 0.1;
@ -290,7 +290,7 @@ LinearFactorGraph createSimpleConstraintGraph() {
Vector b1(2);
b1(0) = 1.0;
b1(1) = -1.0;
LinearFactor::shared_ptr f1(new LinearFactor("x", Ax, b1, sigma));
GaussianFactor::shared_ptr f1(new GaussianFactor("x", Ax, b1, sigma));
// create binary constraint factor
// between "x" and "y", that is going to be the only factor on "y"
@ -299,11 +299,11 @@ LinearFactorGraph createSimpleConstraintGraph() {
Matrix Ax1 = eye(2);
Matrix Ay1 = eye(2) * -1;
Vector b2 = Vector_(2, 0.0, 0.0);
LinearFactor::shared_ptr f2(
new LinearFactor("x", Ax1, "y", Ay1, b2, 0.0));
GaussianFactor::shared_ptr f2(
new GaussianFactor("x", Ax1, "y", Ay1, b2, 0.0));
// construct the graph
LinearFactorGraph fg;
GaussianFactorGraph fg;
fg.push_back(f1);
fg.push_back(f2);
@ -320,7 +320,7 @@ VectorConfig createSimpleConstraintConfig() {
}
/* ************************************************************************* */
LinearFactorGraph createSingleConstraintGraph() {
GaussianFactorGraph createSingleConstraintGraph() {
// create unary factor
// prior on "x", mean = [1,-1], sigma=0.1
double sigma = 0.1;
@ -328,7 +328,7 @@ LinearFactorGraph createSingleConstraintGraph() {
Vector b1(2);
b1(0) = 1.0;
b1(1) = -1.0;
LinearFactor::shared_ptr f1(new LinearFactor("x", Ax, b1, sigma));
GaussianFactor::shared_ptr f1(new GaussianFactor("x", Ax, b1, sigma));
// create binary constraint factor
// between "x" and "y", that is going to be the only factor on "y"
@ -339,11 +339,11 @@ LinearFactorGraph createSingleConstraintGraph() {
Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0;
Matrix Ay1 = eye(2) * 10;
Vector b2 = Vector_(2, 1.0, 2.0);
LinearFactor::shared_ptr f2(
new LinearFactor("x", Ax1, "y", Ay1, b2, 0.0));
GaussianFactor::shared_ptr f2(
new GaussianFactor("x", Ax1, "y", Ay1, b2, 0.0));
// construct the graph
LinearFactorGraph fg;
GaussianFactorGraph fg;
fg.push_back(f1);
fg.push_back(f2);
@ -359,12 +359,12 @@ VectorConfig createSingleConstraintConfig() {
}
/* ************************************************************************* */
LinearFactorGraph createMultiConstraintGraph() {
GaussianFactorGraph createMultiConstraintGraph() {
// unary factor 1
double sigma = 0.1;
Matrix A = eye(2);
Vector b = Vector_(2, -2.0, 2.0);
LinearFactor::shared_ptr lf1(new LinearFactor("x", A, b, sigma));
GaussianFactor::shared_ptr lf1(new GaussianFactor("x", A, b, sigma));
// constraint 1
Matrix A11(2,2);
@ -377,7 +377,7 @@ LinearFactorGraph createMultiConstraintGraph() {
Vector b1(2);
b1(0) = 1.0; b1(1) = 2.0;
LinearFactor::shared_ptr lc1(new LinearFactor("x", A11, "y", A12, b1, 0.0));
GaussianFactor::shared_ptr lc1(new GaussianFactor("x", A11, "y", A12, b1, 0.0));
// constraint 2
Matrix A21(2,2);
@ -390,10 +390,10 @@ LinearFactorGraph createMultiConstraintGraph() {
Vector b2(2);
b2(0) = 3.0; b2(1) = 4.0;
LinearFactor::shared_ptr lc2(new LinearFactor("x", A21, "z", A22, b2, 0.0));
GaussianFactor::shared_ptr lc2(new GaussianFactor("x", A21, "z", A22, b2, 0.0));
// construct the graph
LinearFactorGraph fg;
GaussianFactorGraph fg;
fg.push_back(lf1);
fg.push_back(lc1);
fg.push_back(lc2);
@ -411,13 +411,13 @@ VectorConfig createMultiConstraintConfig() {
}
/* ************************************************************************* */
//LinearFactorGraph createConstrainedLinearFactorGraph()
//GaussianFactorGraph createConstrainedGaussianFactorGraph()
//{
// LinearFactorGraph graph;
// GaussianFactorGraph graph;
//
// // add an equality factor
// Vector v1(2); v1(0)=1.;v1(1)=2.;
// LinearFactor::shared_ptr f1(new LinearFactor(v1, "x0"));
// GaussianFactor::shared_ptr f1(new GaussianFactor(v1, "x0"));
// graph.push_back_eq(f1);
//
// // add a normal linear factor
@ -429,7 +429,7 @@ VectorConfig createMultiConstraintConfig() {
// b(0) = 2 ; b(1) = 3;
//
// double sigma = 0.1;
// LinearFactor::shared_ptr f2(new LinearFactor("x0", A21/sigma, "x1", A22/sigma, b/sigma));
// GaussianFactor::shared_ptr f2(new GaussianFactor("x0", A21/sigma, "x1", A22/sigma, b/sigma));
// graph.push_back(f2);
// return graph;
//}
@ -440,7 +440,7 @@ VectorConfig createMultiConstraintConfig() {
// VectorConfig c = createConstrainedConfig();
//
// // equality constraint for initial pose
// LinearFactor::shared_ptr f1(new LinearFactor(c["x0"], "x0"));
// GaussianFactor::shared_ptr f1(new GaussianFactor(c["x0"], "x0"));
// graph.push_back_eq(f1);
//
// // odometry between x0 and x1

12
cpp/smallExample.h
View File

@ -49,7 +49,7 @@ namespace gtsam {
* create a linear factor graph
* The non-linear graph above evaluated at NoisyConfig
*/
LinearFactorGraph createLinearFactorGraph();
GaussianFactorGraph createGaussianFactorGraph();
/**
* create small Chordal Bayes Net x <- y
@ -66,7 +66,7 @@ namespace gtsam {
* Create a Kalman smoother by linearizing a non-linear factor graph
* @param T number of time-steps
*/
LinearFactorGraph createSmoother(int T);
GaussianFactorGraph createSmoother(int T);
/* ******************************************************* */
@ -77,21 +77,21 @@ namespace gtsam {
* Creates a simple constrained graph with one linear factor and
* one binary equality constraint that sets x = y
*/
LinearFactorGraph createSimpleConstraintGraph();
GaussianFactorGraph createSimpleConstraintGraph();
VectorConfig createSimpleConstraintConfig();
/**
* Creates a simple constrained graph with one linear factor and
* one binary constraint.
*/
LinearFactorGraph createSingleConstraintGraph();
GaussianFactorGraph createSingleConstraintGraph();
VectorConfig createSingleConstraintConfig();
/**
* Creates a constrained graph with a linear factor and two
* binary constraints that share a node
*/
LinearFactorGraph createMultiConstraintGraph();
GaussianFactorGraph createMultiConstraintGraph();
VectorConfig createMultiConstraintConfig();
/**
@ -117,7 +117,7 @@ namespace gtsam {
/**
* Create small example constrained factor graph
*/
//LinearFactorGraph createConstrainedLinearFactorGraph();
//GaussianFactorGraph createConstrainedGaussianFactorGraph();
/**
* Create small example constrained nonlinear factor graph

46
cpp/testBayesTree.cpp
View File

@ -95,7 +95,7 @@ C6 x1 : x2
TEST( BayesTree, linear_smoother_shortcuts )
{
// Create smoother with 7 nodes
LinearFactorGraph smoother = createSmoother(7);
GaussianFactorGraph smoother = createSmoother(7);
Ordering ordering;
for (int t = 1; t <= 7; t++)
ordering.push_back(symbol('x', t));
@ -110,12 +110,12 @@ TEST( BayesTree, linear_smoother_shortcuts )
// Check the conditional P(Root|Root)
GaussianBayesNet empty;
Gaussian::sharedClique R = bayesTree.root();
GaussianBayesNet actual1 = R->shortcut<LinearFactor>(R);
GaussianBayesNet actual1 = R->shortcut<GaussianFactor>(R);
CHECK(assert_equal(empty,actual1,1e-4));
// Check the conditional P(C2|Root)
Gaussian::sharedClique C2 = bayesTree["x5"];
GaussianBayesNet actual2 = C2->shortcut<LinearFactor>(R);
GaussianBayesNet actual2 = C2->shortcut<GaussianFactor>(R);
CHECK(assert_equal(empty,actual2,1e-4));
// Check the conditional P(C3|Root)
@ -124,7 +124,7 @@ TEST( BayesTree, linear_smoother_shortcuts )
GaussianBayesNet expected3;
push_front(expected3,"x5", zero(2), eye(2), "x6", A56, sigma3);
Gaussian::sharedClique C3 = bayesTree["x4"];
GaussianBayesNet actual3 = C3->shortcut<LinearFactor>(R);
GaussianBayesNet actual3 = C3->shortcut<GaussianFactor>(R);
CHECK(assert_equal(expected3,actual3,1e-4));
// Check the conditional P(C4|Root)
@ -133,7 +133,7 @@ TEST( BayesTree, linear_smoother_shortcuts )
GaussianBayesNet expected4;
push_front(expected4,"x4", zero(2), eye(2), "x6", A46, sigma4);
Gaussian::sharedClique C4 = bayesTree["x3"];
GaussianBayesNet actual4 = C4->shortcut<LinearFactor>(R);
GaussianBayesNet actual4 = C4->shortcut<GaussianFactor>(R);
CHECK(assert_equal(expected4,actual4,1e-4));
}
@ -159,7 +159,7 @@ TEST( BayesTree, linear_smoother_shortcuts )
TEST( BayesTree, balanced_smoother_marginals )
{
// Create smoother with 7 nodes
LinearFactorGraph smoother = createSmoother(7);
GaussianFactorGraph smoother = createSmoother(7);
Ordering ordering;
ordering += "x1","x3","x5","x7","x2","x6","x4";
@ -178,27 +178,27 @@ TEST( BayesTree, balanced_smoother_marginals )
// Check marginal on x1
GaussianBayesNet expected1 = simpleGaussian("x1", zero(2), sigmax1);
GaussianBayesNet actual1 = bayesTree.marginalBayesNet<LinearFactor>("x1");
GaussianBayesNet actual1 = bayesTree.marginalBayesNet<GaussianFactor>("x1");
CHECK(assert_equal(expected1,actual1,1e-4));
// Check marginal on x2
GaussianBayesNet expected2 = simpleGaussian("x2", zero(2), sigmax2);
GaussianBayesNet actual2 = bayesTree.marginalBayesNet<LinearFactor>("x2");
GaussianBayesNet actual2 = bayesTree.marginalBayesNet<GaussianFactor>("x2");
CHECK(assert_equal(expected2,actual2,1e-4));
// Check marginal on x3
GaussianBayesNet expected3 = simpleGaussian("x3", zero(2), sigmax3);
GaussianBayesNet actual3 = bayesTree.marginalBayesNet<LinearFactor>("x3");
GaussianBayesNet actual3 = bayesTree.marginalBayesNet<GaussianFactor>("x3");
CHECK(assert_equal(expected3,actual3,1e-4));
// Check marginal on x4
GaussianBayesNet expected4 = simpleGaussian("x4", zero(2), sigmax4);
GaussianBayesNet actual4 = bayesTree.marginalBayesNet<LinearFactor>("x4");
GaussianBayesNet actual4 = bayesTree.marginalBayesNet<GaussianFactor>("x4");
CHECK(assert_equal(expected4,actual4,1e-4));
// Check marginal on x7 (should be equal to x1)
GaussianBayesNet expected7 = simpleGaussian("x7", zero(2), sigmax7);
GaussianBayesNet actual7 = bayesTree.marginalBayesNet<LinearFactor>("x7");
GaussianBayesNet actual7 = bayesTree.marginalBayesNet<GaussianFactor>("x7");
CHECK(assert_equal(expected7,actual7,1e-4));
}
@ -206,7 +206,7 @@ TEST( BayesTree, balanced_smoother_marginals )
TEST( BayesTree, balanced_smoother_shortcuts )
{
// Create smoother with 7 nodes
LinearFactorGraph smoother = createSmoother(7);
GaussianFactorGraph smoother = createSmoother(7);
Ordering ordering;
ordering += "x1","x3","x5","x7","x2","x6","x4";
@ -217,19 +217,19 @@ TEST( BayesTree, balanced_smoother_shortcuts )
// Check the conditional P(Root|Root)
GaussianBayesNet empty;
Gaussian::sharedClique R = bayesTree.root();
GaussianBayesNet actual1 = R->shortcut<LinearFactor>(R);
GaussianBayesNet actual1 = R->shortcut<GaussianFactor>(R);
CHECK(assert_equal(empty,actual1,1e-4));
// Check the conditional P(C2|Root)
Gaussian::sharedClique C2 = bayesTree["x3"];
GaussianBayesNet actual2 = C2->shortcut<LinearFactor>(R);
GaussianBayesNet actual2 = C2->shortcut<GaussianFactor>(R);
CHECK(assert_equal(empty,actual2,1e-4));
// Check the conditional P(C3|Root), which should be equal to P(x2|x4)
ConditionalGaussian::shared_ptr p_x2_x4 = chordalBayesNet["x2"];
GaussianBayesNet expected3; expected3.push_back(p_x2_x4);
Gaussian::sharedClique C3 = bayesTree["x1"];
GaussianBayesNet actual3 = C3->shortcut<LinearFactor>(R);
GaussianBayesNet actual3 = C3->shortcut<GaussianFactor>(R);
CHECK(assert_equal(expected3,actual3,1e-4));
}
@ -237,7 +237,7 @@ TEST( BayesTree, balanced_smoother_shortcuts )
TEST( BayesTree, balanced_smoother_clique_marginals )
{
// Create smoother with 7 nodes
LinearFactorGraph smoother = createSmoother(7);
GaussianFactorGraph smoother = createSmoother(7);
Ordering ordering;
ordering += "x1","x3","x5","x7","x2","x6","x4";
@ -251,8 +251,8 @@ TEST( BayesTree, balanced_smoother_clique_marginals )
Matrix A12 = (-0.5)*eye(2);
push_front(expected,"x1", zero(2), eye(2), "x2", A12, sigma);
Gaussian::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"];
FactorGraph<LinearFactor> marginal = C3->marginal<LinearFactor>(R);
GaussianBayesNet actual = eliminate<LinearFactor,ConditionalGaussian>(marginal,C3->keys());
FactorGraph<GaussianFactor> marginal = C3->marginal<GaussianFactor>(R);
GaussianBayesNet actual = eliminate<GaussianFactor,ConditionalGaussian>(marginal,C3->keys());
CHECK(assert_equal(expected,actual,1e-4));
}
@ -260,7 +260,7 @@ TEST( BayesTree, balanced_smoother_clique_marginals )
TEST( BayesTree, balanced_smoother_joint )
{
// Create smoother with 7 nodes
LinearFactorGraph smoother = createSmoother(7);
GaussianFactorGraph smoother = createSmoother(7);
Ordering ordering;
ordering += "x1","x3","x5","x7","x2","x6","x4";
@ -275,13 +275,13 @@ TEST( BayesTree, balanced_smoother_joint )
// Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
GaussianBayesNet expected1 = simpleGaussian("x7", zero(2), sigmax7);
push_front(expected1,"x1", zero(2), eye(2), "x7", A, sigma);
GaussianBayesNet actual1 = bayesTree.jointBayesNet<LinearFactor>("x1","x7");
GaussianBayesNet actual1 = bayesTree.jointBayesNet<GaussianFactor>("x1","x7");
CHECK(assert_equal(expected1,actual1,1e-4));
// Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
GaussianBayesNet expected2 = simpleGaussian("x1", zero(2), sigmax1);
push_front(expected2,"x7", zero(2), eye(2), "x1", A, sigma);
GaussianBayesNet actual2 = bayesTree.jointBayesNet<LinearFactor>("x7","x1");
GaussianBayesNet actual2 = bayesTree.jointBayesNet<GaussianFactor>("x7","x1");
CHECK(assert_equal(expected2,actual2,1e-4));
// Check the joint density P(x1,x4), i.e. with a root variable
@ -289,7 +289,7 @@ TEST( BayesTree, balanced_smoother_joint )
Vector sigma14 = repeat(2, 0.784465);
Matrix A14 = (-0.0769231)*eye(2);
push_front(expected3,"x1", zero(2), eye(2), "x4", A14, sigma14);
GaussianBayesNet actual3 = bayesTree.jointBayesNet<LinearFactor>("x1","x4");
GaussianBayesNet actual3 = bayesTree.jointBayesNet<GaussianFactor>("x1","x4");
CHECK(assert_equal(expected3,actual3,1e-4));
// Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
@ -297,7 +297,7 @@ TEST( BayesTree, balanced_smoother_joint )
Vector sigma41 = repeat(2, 0.668096);
Matrix A41 = (-0.055794)*eye(2);
push_front(expected4,"x4", zero(2), eye(2), "x1", A41, sigma41);
GaussianBayesNet actual4 = bayesTree.jointBayesNet<LinearFactor>("x4","x1");
GaussianBayesNet actual4 = bayesTree.jointBayesNet<GaussianFactor>("x4","x1");
CHECK(assert_equal(expected4,actual4,1e-4));
}

168
cpp/testLinearFactor.cpp → cpp/testGaussianFactor.cpp
View File

@ -1,5 +1,5 @@
/**
* @file testLinearFactor.cpp
* @file testGaussianFactor.cpp
* @brief Unit tests for Linear Factor
* @author Christian Potthast
* @author Frank Dellaert
@ -24,7 +24,7 @@ using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST( LinearFactor, linearFactor )
TEST( GaussianFactor, linearFactor )
{
double sigma = 0.1;
@ -39,24 +39,24 @@ TEST( LinearFactor, linearFactor )
Vector b(2);
b(0) = 0.2 ; b(1) = -0.1;
LinearFactor expected("x1", A1, "x2", A2, b, sigma);
GaussianFactor expected("x1", A1, "x2", A2, b, sigma);
// create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// get the factor "f2" from the factor graph
LinearFactor::shared_ptr lf = fg[1];
GaussianFactor::shared_ptr lf = fg[1];
// check if the two factors are the same
CHECK(assert_equal(expected,*lf));
}
/* ************************************************************************* */
TEST( LinearFactor, keys )
TEST( GaussianFactor, keys )
{
// get the factor "f2" from the small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
LinearFactor::shared_ptr lf = fg[1];
GaussianFactorGraph fg = createGaussianFactorGraph();
GaussianFactor::shared_ptr lf = fg[1];
list<string> expected;
expected.push_back("x1");
expected.push_back("x2");
@ -64,10 +64,10 @@ TEST( LinearFactor, keys )
}
/* ************************************************************************* */
TEST( LinearFactor, dimensions )
TEST( GaussianFactor, dimensions )
{
// get the factor "f2" from the small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// Check a single factor
Dimensions expected;
@ -77,11 +77,11 @@ TEST( LinearFactor, dimensions )
}
/* ************************************************************************* */
TEST( LinearFactor, getDim )
TEST( GaussianFactor, getDim )
{
// get a factor
LinearFactorGraph fg = createLinearFactorGraph();
LinearFactor::shared_ptr factor = fg[0];
GaussianFactorGraph fg = createGaussianFactorGraph();
GaussianFactor::shared_ptr factor = fg[0];
// get the size of a variable
size_t actual = factor->getDim("x1");
@ -92,18 +92,18 @@ TEST( LinearFactor, getDim )
}
/* ************************************************************************* */
TEST( LinearFactor, combine )
TEST( GaussianFactor, combine )
{
// create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// get two factors from it and insert the factors into a vector
vector<LinearFactor::shared_ptr> lfg;
vector<GaussianFactor::shared_ptr> lfg;
lfg.push_back(fg[4 - 1]);
lfg.push_back(fg[2 - 1]);
// combine in a factor
LinearFactor combined(lfg);
GaussianFactor combined(lfg);
// sigmas
double sigma2 = 0.1;
@ -142,7 +142,7 @@ TEST( LinearFactor, combine )
meas.push_back(make_pair("x2", Ax2));
meas.push_back(make_pair("l1", Al1));
meas.push_back(make_pair("x1", Ax1));
LinearFactor expected(meas, b2, sigmas);
GaussianFactor expected(meas, b2, sigmas);
CHECK(assert_equal(expected,combined));
}
@ -154,33 +154,33 @@ TEST( NonlinearFactorGraph, combine2){
A11(1,0) = 0; A11(1,1) = 1;
Vector b(2);
b(0) = 2; b(1) = -1;
LinearFactor::shared_ptr f1(new LinearFactor("x1", A11, b*sigma1, sigma1));
GaussianFactor::shared_ptr f1(new GaussianFactor("x1", A11, b*sigma1, 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;
LinearFactor::shared_ptr f2(new LinearFactor("x1", A11, b*sigma2, sigma2));
GaussianFactor::shared_ptr f2(new GaussianFactor("x1", A11, b*sigma2, 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;
LinearFactor::shared_ptr f3(new LinearFactor("x1", A11, b*sigma3, sigma3));
GaussianFactor::shared_ptr f3(new GaussianFactor("x1", A11, b*sigma3, 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;
LinearFactor::shared_ptr f4(new LinearFactor("x1", A11*sigma4, b*sigma4, sigma4));
GaussianFactor::shared_ptr f4(new GaussianFactor("x1", A11*sigma4, b*sigma4, sigma4));
vector<LinearFactor::shared_ptr> lfg;
vector<GaussianFactor::shared_ptr> lfg;
lfg.push_back(f1);
lfg.push_back(f2);
lfg.push_back(f3);
lfg.push_back(f4);
LinearFactor combined(lfg);
GaussianFactor combined(lfg);
Vector sigmas = Vector_(8, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3, sigma4, sigma4);
Matrix A22(8,2);
@ -198,19 +198,19 @@ TEST( NonlinearFactorGraph, combine2){
vector<pair<string, Matrix> > meas;
meas.push_back(make_pair("x1", A22));
LinearFactor expected(meas, exb, sigmas);
GaussianFactor expected(meas, exb, sigmas);
CHECK(assert_equal(expected,combined));
}
/* ************************************************************************* */
TEST( LinearFactor, linearFactorN){
vector<LinearFactor::shared_ptr> f;
f.push_back(LinearFactor::shared_ptr(new LinearFactor("x1", Matrix_(2,2,
TEST( GaussianFactor, linearFactorN){
vector<GaussianFactor::shared_ptr> f;
f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", Matrix_(2,2,
1.0, 0.0,
0.0, 1.0),
Vector_(2,
10.0, 5.0), 1)));
f.push_back(LinearFactor::shared_ptr(new LinearFactor("x1", Matrix_(2,2,
f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", Matrix_(2,2,
-10.0, 0.0,
0.0, -10.0),
"x2", Matrix_(2,2,
@ -218,7 +218,7 @@ TEST( LinearFactor, linearFactorN){
0.0, 10.0),
Vector_(2,
1.0, -2.0), 1)));
f.push_back(LinearFactor::shared_ptr(new LinearFactor("x2", Matrix_(2,2,
f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2", Matrix_(2,2,
-10.0, 0.0,
0.0, -10.0),
"x3", Matrix_(2,2,
@ -226,7 +226,7 @@ TEST( LinearFactor, linearFactorN){
0.0, 10.0),
Vector_(2,
1.5, -1.5), 1)));
f.push_back(LinearFactor::shared_ptr(new LinearFactor("x3", Matrix_(2,2,
f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x3", Matrix_(2,2,
-10.0, 0.0,
0.0, -10.0),
"x4", Matrix_(2,2,
@ -235,7 +235,7 @@ TEST( LinearFactor, linearFactorN){
Vector_(2,
2.0, -1.0), 1)));
LinearFactor combinedFactor(f);
GaussianFactor combinedFactor(f);
vector<pair<string, Matrix> > combinedMeasurement;
combinedMeasurement.push_back(make_pair("x1", Matrix_(8,2,
@ -277,18 +277,18 @@ TEST( LinearFactor, linearFactorN){
Vector b = Vector_(8,
10.0, 5.0, 1.0, -2.0, 1.5, -1.5, 2.0, -1.0);
LinearFactor expected(combinedMeasurement, b, 1.);
GaussianFactor expected(combinedMeasurement, b, 1.);
CHECK(combinedFactor.equals(expected));
}
/* ************************************************************************* */
TEST( LinearFactor, error )
TEST( GaussianFactor, error )
{
// create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// get the first factor from the factor graph
LinearFactor::shared_ptr lf = fg[0];
GaussianFactor::shared_ptr lf = fg[0];
// check the error of the first factor with noisy config
VectorConfig cfg = createZeroDelta();
@ -300,22 +300,22 @@ TEST( LinearFactor, error )
}
/* ************************************************************************* */
TEST( LinearFactor, eliminate )
TEST( GaussianFactor, eliminate )
{
// create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// get two factors from it and insert the factors into a vector
vector<LinearFactor::shared_ptr> lfg;
vector<GaussianFactor::shared_ptr> lfg;
lfg.push_back(fg[4 - 1]);
lfg.push_back(fg[2 - 1]);
// combine in a factor
LinearFactor combined(lfg);
GaussianFactor combined(lfg);
// eliminate the combined factor
ConditionalGaussian::shared_ptr actualCG;
LinearFactor::shared_ptr actualLF;
GaussianFactor::shared_ptr actualLF;
boost::tie(actualCG,actualLF) = combined.eliminate("x2");
// create expected Conditional Gaussian
@ -357,7 +357,7 @@ TEST( LinearFactor, eliminate )
// the RHS
Vector b1(2); b1(0) = 0.0; b1(1) = 0.2;
LinearFactor expectedLF("l1", Bl1, "x1", Bx1, b1, sigma);
GaussianFactor expectedLF("l1", Bl1, "x1", Bx1, b1, sigma);
// check if the result matches
CHECK(assert_equal(expectedCG,*actualCG,1e-4));
@ -366,7 +366,7 @@ TEST( LinearFactor, eliminate )
/* ************************************************************************* */
TEST( LinearFactor, eliminate2 )
TEST( GaussianFactor, eliminate2 )
{
// sigmas
double sigma1 = 0.2;
@ -400,11 +400,11 @@ TEST( LinearFactor, eliminate2 )
vector<pair<string, Matrix> > meas;
meas.push_back(make_pair("x2", Ax2));
meas.push_back(make_pair("l1x1", Al1x1));
LinearFactor combined(meas, b2, sigmas);
GaussianFactor combined(meas, b2, sigmas);
// eliminate the combined factor
ConditionalGaussian::shared_ptr actualCG;
LinearFactor::shared_ptr actualLF;
GaussianFactor::shared_ptr actualLF;
boost::tie(actualCG,actualLF) = combined.eliminate("x2");
// create expected Conditional Gaussian
@ -435,7 +435,7 @@ TEST( LinearFactor, eliminate2 )
// the RHS
Vector b1(2); b1(0) = 0.0; b1(1) = 0.894427;
LinearFactor expectedLF("l1x1", Bl1x1, b1*sigma, sigma);
GaussianFactor expectedLF("l1x1", Bl1x1, b1*sigma, sigma);
// check if the result matches
CHECK(assert_equal(expectedCG,*actualCG,1e-4));
@ -443,30 +443,30 @@ TEST( LinearFactor, eliminate2 )
}
/* ************************************************************************* */
TEST( LinearFactor, default_error )
TEST( GaussianFactor, default_error )
{
LinearFactor f;
GaussianFactor f;
VectorConfig c;
double actual = f.error(c);
CHECK(actual==0.0);
}
//* ************************************************************************* */
TEST( LinearFactor, eliminate_empty )
TEST( GaussianFactor, eliminate_empty )
{
// create an empty factor
LinearFactor f;
GaussianFactor f;
// eliminate the empty factor
ConditionalGaussian::shared_ptr actualCG;
LinearFactor::shared_ptr actualLF;
GaussianFactor::shared_ptr actualLF;
boost::tie(actualCG,actualLF) = f.eliminate("x2");
// expected Conditional Gaussian is just a parent-less node with P(x)=1
ConditionalGaussian expectedCG("x2");
// expected remaining factor is still empty :-)
LinearFactor expectedLF;
GaussianFactor expectedLF;
// check if the result matches
CHECK(actualCG->equals(expectedCG));
@ -474,21 +474,21 @@ TEST( LinearFactor, eliminate_empty )
}
//* ************************************************************************* */
TEST( LinearFactor, empty )
TEST( GaussianFactor, empty )
{
// create an empty factor
LinearFactor f;
GaussianFactor f;
CHECK(f.empty()==true);
}
/* ************************************************************************* */
TEST( LinearFactor, matrix )
TEST( GaussianFactor, matrix )
{
// create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// get the factor "f2" from the factor graph
LinearFactor::shared_ptr lf = fg[1];
GaussianFactor::shared_ptr lf = fg[1];
// render with a given ordering
Ordering ord;
@ -507,13 +507,13 @@ TEST( LinearFactor, matrix )
}
/* ************************************************************************* */
TEST( LinearFactor, matrix_aug )
TEST( GaussianFactor, matrix_aug )
{
// create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// get the factor "f2" from the factor graph
LinearFactor::shared_ptr lf = fg[1];
GaussianFactor::shared_ptr lf = fg[1];
// render with a given ordering
Ordering ord;
@ -538,13 +538,13 @@ void print(const list<T>& i) {
}
/* ************************************************************************* */
TEST( LinearFactor, sparse )
TEST( GaussianFactor, sparse )
{
// create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// get the factor "f2" from the factor graph
LinearFactor::shared_ptr lf = fg[1];
GaussianFactor::shared_ptr lf = fg[1];
// render with a given ordering
Ordering ord;
@ -567,13 +567,13 @@ TEST( LinearFactor, sparse )
}
/* ************************************************************************* */
TEST( LinearFactor, sparse2 )
TEST( GaussianFactor, sparse2 )
{
// create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// get the factor "f2" from the factor graph
LinearFactor::shared_ptr lf = fg[1];
GaussianFactor::shared_ptr lf = fg[1];
// render with a given ordering
Ordering ord;
@ -596,15 +596,15 @@ TEST( LinearFactor, sparse2 )
}
/* ************************************************************************* */
TEST( LinearFactor, size )
TEST( GaussianFactor, size )
{
// create a linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// get some factors from the graph
boost::shared_ptr<LinearFactor> factor1 = fg[0];
boost::shared_ptr<LinearFactor> factor2 = fg[1];
boost::shared_ptr<LinearFactor> factor3 = fg[2];
boost::shared_ptr<GaussianFactor> factor1 = fg[0];
boost::shared_ptr<GaussianFactor> factor2 = fg[1];
boost::shared_ptr<GaussianFactor> factor3 = fg[2];
CHECK(factor1->size() == 1);
CHECK(factor2->size() == 2);
@ -612,7 +612,7 @@ TEST( LinearFactor, size )
}
/* ************************************************************************* */
TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian )
TEST( GaussianFactor, CONSTRUCTOR_ConditionalGaussian )
{
Matrix R11 = Matrix_(2,2,
1.00, 0.00,
@ -629,24 +629,24 @@ TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian )
sigmas(1) = 0.29907;
ConditionalGaussian::shared_ptr CG(new ConditionalGaussian("x2",d,R11,"l1x1",S12,sigmas));
LinearFactor actualLF(CG);
GaussianFactor actualLF(CG);
// actualLF.print();
LinearFactor expectedLF("x2",R11,"l1x1",S12,d, sigmas(0));
GaussianFactor expectedLF("x2",R11,"l1x1",S12,d, sigmas(0));
CHECK(assert_equal(expectedLF,actualLF,1e-5));
}
/* ************************************************************************* */
TEST ( LinearFactor, constraint_eliminate1 )
TEST ( GaussianFactor, constraint_eliminate1 )
{
// construct a linear constraint
Vector v(2); v(0)=1.2; v(1)=3.4;
string key = "x0";
LinearFactor lc(key, eye(2), v, 0.0);
GaussianFactor lc(key, eye(2), v, 0.0);
// eliminate it
ConditionalGaussian::shared_ptr actualCG;
LinearFactor::shared_ptr actualLF;
GaussianFactor::shared_ptr actualLF;
boost::tie(actualCG,actualLF) = lc.eliminate("x0");
// verify linear factor
@ -659,7 +659,7 @@ TEST ( LinearFactor, constraint_eliminate1 )
}
/* ************************************************************************* */
TEST ( LinearFactor, constraint_eliminate2 )
TEST ( GaussianFactor, constraint_eliminate2 )
{
// Construct a linear constraint
// RHS
@ -675,15 +675,15 @@ TEST ( LinearFactor, constraint_eliminate2 )
A2(0,0) = 1.0 ; A2(0,1) = 2.0;
A2(1,0) = 2.0 ; A2(1,1) = 4.0;
LinearFactor lc("x", A1, "y", A2, b, 0.0);
GaussianFactor lc("x", A1, "y", A2, b, 0.0);
// eliminate x and verify results
ConditionalGaussian::shared_ptr actualCG;
LinearFactor::shared_ptr actualLF;
GaussianFactor::shared_ptr actualLF;
boost::tie(actualCG, actualLF) = lc.eliminate("x");
// LF should be null
LinearFactor expectedLF;
GaussianFactor expectedLF;
CHECK(assert_equal(*actualLF, expectedLF));
// verify CG
@ -699,7 +699,7 @@ TEST ( LinearFactor, constraint_eliminate2 )
}
/* ************************************************************************* */
TEST ( LinearFactor, constraint_eliminate3 )
TEST ( GaussianFactor, constraint_eliminate3 )
{
// This test shows that ordering matters if there are non-invertable
// blocks, as this example can be eliminated if x is first, but not
@ -719,13 +719,13 @@ TEST ( LinearFactor, constraint_eliminate3 )
A2(0,0) = 1.0 ; A2(0,1) = 2.0;
A2(1,0) = 2.0 ; A2(1,1) = 4.0;
LinearFactor lc("x", A1, "y", A2, b, 0.0);
GaussianFactor lc("x", A1, "y", A2, b, 0.0);
// eliminate y from original graph
// NOTE: this will throw an exception, as
// the leading matrix is rank deficient
ConditionalGaussian::shared_ptr actualCG;
LinearFactor::shared_ptr actualLF;
GaussianFactor::shared_ptr actualLF;
try {
boost::tie(actualCG, actualLF) = lc.eliminate("y");
CHECK(false);

180
cpp/testLinearFactorGraph.cpp → cpp/testGaussianFactorGraph.cpp
View File

@ -1,5 +1,5 @@
/**
* @file testLinearFactorGraph.cpp
* @file testGaussianFactorGraph.cpp
* @brief Unit tests for Linear Factor Graph
* @author Christian Potthast
**/
@ -26,19 +26,19 @@ using namespace gtsam;
double tol=1e-4;
/* ************************************************************************* */
/* unit test for equals (LinearFactorGraph1 == LinearFactorGraph2) */
/* unit test for equals (GaussianFactorGraph1 == GaussianFactorGraph2) */
/* ************************************************************************* */
TEST( LinearFactorGraph, equals ){
TEST( GaussianFactorGraph, equals ){
LinearFactorGraph fg = createLinearFactorGraph();
LinearFactorGraph fg2 = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
GaussianFactorGraph fg2 = createGaussianFactorGraph();
CHECK(fg.equals(fg2));
}
/* ************************************************************************* */
TEST( LinearFactorGraph, error )
TEST( GaussianFactorGraph, error )
{
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
VectorConfig cfg = createZeroDelta();
// note the error is the same as in testNonlinearFactorGraph as a
@ -51,9 +51,9 @@ TEST( LinearFactorGraph, error )
/* ************************************************************************* */
/* unit test for find seperator */
/* ************************************************************************* */
TEST( LinearFactorGraph, find_separator )
TEST( GaussianFactorGraph, find_separator )
{
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
set<string> separator = fg.find_separator("x2");
set<string> expected;
@ -67,10 +67,10 @@ TEST( LinearFactorGraph, find_separator )
}
/* ************************************************************************* */
TEST( LinearFactorGraph, combine_factors_x1 )
TEST( GaussianFactorGraph, combine_factors_x1 )
{
// create a small example for a linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// create sigmas
double sigma1 = 0.1;
@ -79,7 +79,7 @@ TEST( LinearFactorGraph, combine_factors_x1 )
Vector sigmas = Vector_(6, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3);
// combine all factors
LinearFactor::shared_ptr actual = removeAndCombineFactors(fg,"x1");
GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,"x1");
// the expected linear factor
Matrix Al1 = Matrix_(6,2,
@ -122,18 +122,18 @@ TEST( LinearFactorGraph, combine_factors_x1 )
meas.push_back(make_pair("l1", Al1));
meas.push_back(make_pair("x1", Ax1));
meas.push_back(make_pair("x2", Ax2));
LinearFactor expected(meas, b, sigmas);
//LinearFactor expected("l1", Al1, "x1", Ax1, "x2", Ax2, b);
GaussianFactor expected(meas, b, sigmas);
//GaussianFactor expected("l1", Al1, "x1", Ax1, "x2", Ax2, b);
// check if the two factors are the same
CHECK(assert_equal(expected,*actual));
}
/* ************************************************************************* */
TEST( LinearFactorGraph, combine_factors_x2 )
TEST( GaussianFactorGraph, combine_factors_x2 )
{
// create a small example for a linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// determine sigmas
double sigma1 = 0.1;
@ -141,7 +141,7 @@ TEST( LinearFactorGraph, combine_factors_x2 )
Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2);
// combine all factors
LinearFactor::shared_ptr actual = removeAndCombineFactors(fg,"x2");
GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,"x2");
// the expected linear factor
Matrix Al1 = Matrix_(4,2,
@ -179,7 +179,7 @@ TEST( LinearFactorGraph, combine_factors_x2 )
meas.push_back(make_pair("l1", Al1));
meas.push_back(make_pair("x1", Ax1));
meas.push_back(make_pair("x2", Ax2));
LinearFactor expected(meas, b, sigmas);
GaussianFactor expected(meas, b, sigmas);
// check if the two factors are the same
CHECK(assert_equal(expected,*actual));
@ -187,9 +187,9 @@ TEST( LinearFactorGraph, combine_factors_x2 )
/* ************************************************************************* */
TEST( LinearFactorGraph, eliminateOne_x1 )
TEST( GaussianFactorGraph, eliminateOne_x1 )
{
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
ConditionalGaussian::shared_ptr actual = fg.eliminateOne("x1");
// create expected Conditional Gaussian
@ -202,9 +202,9 @@ TEST( LinearFactorGraph, eliminateOne_x1 )
/* ************************************************************************* */
TEST( LinearFactorGraph, eliminateOne_x2 )
TEST( GaussianFactorGraph, eliminateOne_x2 )
{
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
ConditionalGaussian::shared_ptr actual = fg.eliminateOne("x2");
// create expected Conditional Gaussian
@ -216,9 +216,9 @@ TEST( LinearFactorGraph, eliminateOne_x2 )
}
/* ************************************************************************* */
TEST( LinearFactorGraph, eliminateOne_l1 )
TEST( GaussianFactorGraph, eliminateOne_l1 )
{
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
ConditionalGaussian::shared_ptr actual = fg.eliminateOne("l1");
// create expected Conditional Gaussian
@ -230,7 +230,7 @@ TEST( LinearFactorGraph, eliminateOne_l1 )
}
/* ************************************************************************* */
TEST( LinearFactorGraph, eliminateAll )
TEST( GaussianFactorGraph, eliminateAll )
{
// create expected Chordal bayes Net
Matrix I = eye(2);
@ -245,7 +245,7 @@ TEST( LinearFactorGraph, eliminateAll )
push_front(expected,"x2",d3, I,"l1", (-0.2)*I, "x1", (-0.8)*I, sigma3);
// Check one ordering
LinearFactorGraph fg1 = createLinearFactorGraph();
GaussianFactorGraph fg1 = createGaussianFactorGraph();
Ordering ordering;
ordering += "x2","l1","x1";
GaussianBayesNet actual = fg1.eliminate(ordering);
@ -253,28 +253,28 @@ TEST( LinearFactorGraph, eliminateAll )
}
/* ************************************************************************* */
TEST( LinearFactorGraph, add_priors )
TEST( GaussianFactorGraph, add_priors )
{
LinearFactorGraph fg = createLinearFactorGraph();
LinearFactorGraph actual = fg.add_priors(3);
LinearFactorGraph expected = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
GaussianFactorGraph actual = fg.add_priors(3);
GaussianFactorGraph expected = createGaussianFactorGraph();
Matrix A = eye(2);
Vector b = zero(2);
double sigma = 3.0;
expected.push_back(LinearFactor::shared_ptr(new LinearFactor("l1",A,b,sigma)));
expected.push_back(LinearFactor::shared_ptr(new LinearFactor("x1",A,b,sigma)));
expected.push_back(LinearFactor::shared_ptr(new LinearFactor("x2",A,b,sigma)));
expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("l1",A,b,sigma)));
expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1",A,b,sigma)));
expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2",A,b,sigma)));
CHECK(assert_equal(expected,actual)); // Fails
}
/* ************************************************************************* */
TEST( LinearFactorGraph, copying )
TEST( GaussianFactorGraph, copying )
{
// Create a graph
LinearFactorGraph actual = createLinearFactorGraph();
GaussianFactorGraph actual = createGaussianFactorGraph();
// Copy the graph !
LinearFactorGraph copy = actual;
GaussianFactorGraph copy = actual;
// now eliminate the copy
Ordering ord1;
@ -282,17 +282,17 @@ TEST( LinearFactorGraph, copying )
GaussianBayesNet actual1 = copy.eliminate(ord1);
// Create the same graph, but not by copying
LinearFactorGraph expected = createLinearFactorGraph();
GaussianFactorGraph expected = createGaussianFactorGraph();
// and check that original is still the same graph
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( LinearFactorGraph, matrix )
TEST( GaussianFactorGraph, matrix )
{
// Create a graph
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// render with a given ordering
Ordering ord;
@ -318,10 +318,10 @@ TEST( LinearFactorGraph, matrix )
}
/* ************************************************************************* */
TEST( LinearFactorGraph, sparse )
TEST( GaussianFactorGraph, sparse )
{
// create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// render with a given ordering
Ordering ord;
@ -337,41 +337,41 @@ TEST( LinearFactorGraph, sparse )
}
/* ************************************************************************* */
TEST( LinearFactorGraph, CONSTRUCTOR_GaussianBayesNet )
TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
{
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// render with a given ordering
Ordering ord;
ord += "x2","l1","x1";
GaussianBayesNet CBN = fg.eliminate(ord);
// True LinearFactorGraph
LinearFactorGraph fg2(CBN);
// True GaussianFactorGraph
GaussianFactorGraph fg2(CBN);
GaussianBayesNet CBN2 = fg2.eliminate(ord);
CHECK(assert_equal(CBN,CBN2));
// Base FactorGraph only
FactorGraph<LinearFactor> fg3(CBN);
GaussianBayesNet CBN3 = gtsam::eliminate<LinearFactor,ConditionalGaussian>(fg3,ord);
FactorGraph<GaussianFactor> fg3(CBN);
GaussianBayesNet CBN3 = gtsam::eliminate<GaussianFactor,ConditionalGaussian>(fg3,ord);
CHECK(assert_equal(CBN,CBN3));
}
/* ************************************************************************* */
TEST( LinearFactorGraph, GET_ORDERING)
TEST( GaussianFactorGraph, GET_ORDERING)
{
Ordering expected;
expected += "l1","x1","x2";
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
Ordering actual = fg.getOrdering();
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( LinearFactorGraph, OPTIMIZE )
TEST( GaussianFactorGraph, OPTIMIZE )
{
// create a graph
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// create an ordering
Ordering ord = fg.getOrdering();
@ -386,13 +386,13 @@ TEST( LinearFactorGraph, OPTIMIZE )
}
/* ************************************************************************* */
TEST( LinearFactorGraph, COMBINE_GRAPHS_INPLACE)
TEST( GaussianFactorGraph, COMBINE_GRAPHS_INPLACE)
{
// create a test graph
LinearFactorGraph fg1 = createLinearFactorGraph();
GaussianFactorGraph fg1 = createGaussianFactorGraph();
// create another factor graph
LinearFactorGraph fg2 = createLinearFactorGraph();
GaussianFactorGraph fg2 = createGaussianFactorGraph();
// get sizes
int size1 = fg1.size();
@ -405,20 +405,20 @@ TEST( LinearFactorGraph, COMBINE_GRAPHS_INPLACE)
}
/* ************************************************************************* */
TEST( LinearFactorGraph, COMBINE_GRAPHS)
TEST( GaussianFactorGraph, COMBINE_GRAPHS)
{
// create a test graph
LinearFactorGraph fg1 = createLinearFactorGraph();
GaussianFactorGraph fg1 = createGaussianFactorGraph();
// create another factor graph
LinearFactorGraph fg2 = createLinearFactorGraph();
GaussianFactorGraph fg2 = createGaussianFactorGraph();
// get sizes
int size1 = fg1.size();
int size2 = fg2.size();
// combine them
LinearFactorGraph fg3 = LinearFactorGraph::combine2(fg1, fg2);
GaussianFactorGraph fg3 = GaussianFactorGraph::combine2(fg1, fg2);
CHECK(size1+size2 == fg3.size());
}
@ -432,10 +432,10 @@ void print(vector<int> v) {
}
/* ************************************************************************* */
TEST( LinearFactorGraph, factor_lookup)
TEST( GaussianFactorGraph, factor_lookup)
{
// create a test graph
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// ask for all factor indices connected to x1
list<int> x1_factors = fg.factors("x1");
@ -451,18 +451,18 @@ TEST( LinearFactorGraph, factor_lookup)
}
/* ************************************************************************* */
TEST( LinearFactorGraph, findAndRemoveFactors )
TEST( GaussianFactorGraph, findAndRemoveFactors )
{
// create the graph
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// We expect to remove these three factors: 0, 1, 2
LinearFactor::shared_ptr f0 = fg[0];
LinearFactor::shared_ptr f1 = fg[1];
LinearFactor::shared_ptr f2 = fg[2];
GaussianFactor::shared_ptr f0 = fg[0];
GaussianFactor::shared_ptr f1 = fg[1];
GaussianFactor::shared_ptr f2 = fg[2];
// call the function
vector<LinearFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1");
vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1");
// Check the factors
CHECK(f0==factors[0]);
@ -474,18 +474,18 @@ TEST( LinearFactorGraph, findAndRemoveFactors )
}
/* ************************************************************************* */
TEST( LinearFactorGraph, findAndRemoveFactors_twice )
TEST( GaussianFactorGraph, findAndRemoveFactors_twice )
{
// create the graph
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
// We expect to remove these three factors: 0, 1, 2
LinearFactor::shared_ptr f0 = fg[0];
LinearFactor::shared_ptr f1 = fg[1];
LinearFactor::shared_ptr f2 = fg[2];
GaussianFactor::shared_ptr f0 = fg[0];
GaussianFactor::shared_ptr f1 = fg[1];
GaussianFactor::shared_ptr f2 = fg[2];
// call the function
vector<LinearFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1");
vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1");
// Check the factors
CHECK(f0==factors[0]);
@ -500,18 +500,18 @@ TEST( LinearFactorGraph, findAndRemoveFactors_twice )
}
/* ************************************************************************* */
TEST(LinearFactorGraph, createSmoother)
TEST(GaussianFactorGraph, createSmoother)
{
LinearFactorGraph fg1 = createSmoother(2);
GaussianFactorGraph fg1 = createSmoother(2);
LONGS_EQUAL(3,fg1.size());
LinearFactorGraph fg2 = createSmoother(3);
GaussianFactorGraph fg2 = createSmoother(3);
LONGS_EQUAL(5,fg2.size());
}
/* ************************************************************************* */
TEST( LinearFactorGraph, variables )
TEST( GaussianFactorGraph, variables )
{
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
Dimensions expected;
insert(expected)("l1", 2)("x1", 2)("x2", 2);
Dimensions actual = fg.dimensions();
@ -519,23 +519,23 @@ TEST( LinearFactorGraph, variables )
}
/* ************************************************************************* */
TEST( LinearFactorGraph, keys )
TEST( GaussianFactorGraph, keys )
{
LinearFactorGraph fg = createLinearFactorGraph();
GaussianFactorGraph fg = createGaussianFactorGraph();
Ordering expected;
expected += "l1","x1","x2";
CHECK(assert_equal(expected,fg.keys()));
}
/* ************************************************************************* */
// Tests ported from ConstrainedLinearFactorGraph
// Tests ported from ConstrainedGaussianFactorGraph
/* ************************************************************************* */
/* ************************************************************************* */
TEST( LinearFactorGraph, constrained_simple )
TEST( GaussianFactorGraph, constrained_simple )
{
// get a graph with a constraint in it
LinearFactorGraph fg = createSimpleConstraintGraph();
GaussianFactorGraph fg = createSimpleConstraintGraph();
// eliminate and solve
Ordering ord;
@ -548,10 +548,10 @@ TEST( LinearFactorGraph, constrained_simple )
}
/* ************************************************************************* */
TEST( LinearFactorGraph, constrained_single )
TEST( GaussianFactorGraph, constrained_single )
{
// get a graph with a constraint in it
LinearFactorGraph fg = createSingleConstraintGraph();
GaussianFactorGraph fg = createSingleConstraintGraph();
// eliminate and solve
Ordering ord;
@ -564,10 +564,10 @@ TEST( LinearFactorGraph, constrained_single )
}
/* ************************************************************************* */
TEST( LinearFactorGraph, constrained_single2 )
TEST( GaussianFactorGraph, constrained_single2 )
{
// get a graph with a constraint in it
LinearFactorGraph fg = createSingleConstraintGraph();
GaussianFactorGraph fg = createSingleConstraintGraph();
// eliminate and solve
Ordering ord;
@ -580,10 +580,10 @@ TEST( LinearFactorGraph, constrained_single2 )
}
/* ************************************************************************* */
TEST( LinearFactorGraph, constrained_multi1 )
TEST( GaussianFactorGraph, constrained_multi1 )
{
// get a graph with a constraint in it
LinearFactorGraph fg = createMultiConstraintGraph();
GaussianFactorGraph fg = createMultiConstraintGraph();
// eliminate and solve
Ordering ord;
@ -596,10 +596,10 @@ TEST( LinearFactorGraph, constrained_multi1 )
}
/* ************************************************************************* */
TEST( LinearFactorGraph, constrained_multi2 )
TEST( GaussianFactorGraph, constrained_multi2 )
{
// get a graph with a constraint in it
LinearFactorGraph fg = createMultiConstraintGraph();
GaussianFactorGraph fg = createMultiConstraintGraph();
// eliminate and solve
Ordering ord;

14
cpp/testInference.cpp
View File

@ -15,21 +15,21 @@ using namespace gtsam;
/* ************************************************************************* */
// The tests below test the *generic* inference algorithms. Some of these have
// specialized versions in the derived classes LinearFactorGraph etc...
// specialized versions in the derived classes GaussianFactorGraph etc...
/* ************************************************************************* */
/* ************************************************************************* */
TEST(LinearFactorGraph, createSmoother)
TEST(GaussianFactorGraph, createSmoother)
{
LinearFactorGraph fg2 = createSmoother(3);
GaussianFactorGraph fg2 = createSmoother(3);
LONGS_EQUAL(5,fg2.size());
// eliminate
Ordering ordering;
GaussianBayesNet bayesNet = fg2.eliminate(ordering);
bayesNet.print("bayesNet");
FactorGraph<LinearFactor> p_x3 = marginalize<LinearFactor,ConditionalGaussian>(bayesNet, Ordering("x3"));
FactorGraph<LinearFactor> p_x1 = marginalize<LinearFactor,ConditionalGaussian>(bayesNet, Ordering("x1"));
FactorGraph<GaussianFactor> p_x3 = marginalize<GaussianFactor,ConditionalGaussian>(bayesNet, Ordering("x3"));
FactorGraph<GaussianFactor> p_x1 = marginalize<GaussianFactor,ConditionalGaussian>(bayesNet, Ordering("x1"));
CHECK(assert_equal(p_x1,p_x3)); // should be the same because of symmetry
}
@ -39,10 +39,10 @@ TEST( Inference, marginals )
// create and marginalize a small Bayes net on "x"
GaussianBayesNet cbn = createSmallGaussianBayesNet();
Ordering keys("x");
FactorGraph<LinearFactor> fg = marginalize<LinearFactor, ConditionalGaussian>(cbn,keys);
FactorGraph<GaussianFactor> fg = marginalize<GaussianFactor, ConditionalGaussian>(cbn,keys);
// turn into Bayes net to test easily
BayesNet<ConditionalGaussian> actual = eliminate<LinearFactor,ConditionalGaussian>(fg,keys);
BayesNet<ConditionalGaussian> actual = eliminate<GaussianFactor,ConditionalGaussian>(fg,keys);
// expected is just scalar Gaussian on x
GaussianBayesNet expected = scalarGaussian("x",4,sqrt(2));

40
cpp/testNonlinearFactor.cpp
View File

@ -21,7 +21,7 @@ using namespace gtsam;
typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared_nlf;
/* ************************************************************************* */
TEST( NonLinearFactor, equals )
TEST( NonGaussianFactor, equals )
{
double sigma = 1.0;
@ -40,7 +40,7 @@ TEST( NonLinearFactor, equals )
}
/* ************************************************************************* */
TEST( NonLinearFactor, equals2 )
TEST( NonGaussianFactor, equals2 )
{
// create a non linear factor graph
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
@ -54,7 +54,7 @@ TEST( NonLinearFactor, equals2 )
}
/* ************************************************************************* */
TEST( NonLinearFactor, NonlinearFactor )
TEST( NonGaussianFactor, NonlinearFactor )
{
// create a non linear factor graph
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
@ -81,7 +81,7 @@ TEST( NonLinearFactor, NonlinearFactor )
}
/* ************************************************************************* */
TEST( NonLinearFactor, linearize_f1 )
TEST( NonGaussianFactor, linearize_f1 )
{
// Grab a non-linear factor
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
@ -90,16 +90,16 @@ TEST( NonLinearFactor, linearize_f1 )
// We linearize at noisy config from SmallExample
VectorConfig c = createNoisyConfig();
LinearFactor::shared_ptr actual = nlf->linearize(c);
GaussianFactor::shared_ptr actual = nlf->linearize(c);
LinearFactorGraph lfg = createLinearFactorGraph();
LinearFactor::shared_ptr expected = lfg[0];
GaussianFactorGraph lfg = createGaussianFactorGraph();
GaussianFactor::shared_ptr expected = lfg[0];
CHECK(expected->equals(*actual));
}
/* ************************************************************************* */
TEST( NonLinearFactor, linearize_f2 )
TEST( NonGaussianFactor, linearize_f2 )
{
// Grab a non-linear factor
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
@ -108,16 +108,16 @@ TEST( NonLinearFactor, linearize_f2 )
// We linearize at noisy config from SmallExample
VectorConfig c = createNoisyConfig();
LinearFactor::shared_ptr actual = nlf->linearize(c);
GaussianFactor::shared_ptr actual = nlf->linearize(c);
LinearFactorGraph lfg = createLinearFactorGraph();
LinearFactor::shared_ptr expected = lfg[1];
GaussianFactorGraph lfg = createGaussianFactorGraph();
GaussianFactor::shared_ptr expected = lfg[1];
CHECK(expected->equals(*actual));
}
/* ************************************************************************* */
TEST( NonLinearFactor, linearize_f3 )
TEST( NonGaussianFactor, linearize_f3 )
{
// Grab a non-linear factor
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
@ -126,16 +126,16 @@ TEST( NonLinearFactor, linearize_f3 )
// We linearize at noisy config from SmallExample
VectorConfig c = createNoisyConfig();
LinearFactor::shared_ptr actual = nlf->linearize(c);
GaussianFactor::shared_ptr actual = nlf->linearize(c);
LinearFactorGraph lfg = createLinearFactorGraph();
LinearFactor::shared_ptr expected = lfg[2];
GaussianFactorGraph lfg = createGaussianFactorGraph();
GaussianFactor::shared_ptr expected = lfg[2];
CHECK(expected->equals(*actual));
}
/* ************************************************************************* */
TEST( NonLinearFactor, linearize_f4 )
TEST( NonGaussianFactor, linearize_f4 )
{
// Grab a non-linear factor
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
@ -144,16 +144,16 @@ TEST( NonLinearFactor, linearize_f4 )
// We linearize at noisy config from SmallExample
VectorConfig c = createNoisyConfig();
LinearFactor::shared_ptr actual = nlf->linearize(c);
GaussianFactor::shared_ptr actual = nlf->linearize(c);
LinearFactorGraph lfg = createLinearFactorGraph();
LinearFactor::shared_ptr expected = lfg[3];
GaussianFactorGraph lfg = createGaussianFactorGraph();
GaussianFactor::shared_ptr expected = lfg[3];
CHECK(expected->equals(*actual));
}
/* ************************************************************************* */
TEST( NonLinearFactor, size )
TEST( NonGaussianFactor, size )
{
// create a non linear factor graph
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();

4
cpp/testNonlinearFactorGraph.cpp
View File

@ -59,8 +59,8 @@ TEST( ExampleNonlinearFactorGraph, linearize )
{
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
VectorConfig initial = createNoisyConfig();
LinearFactorGraph linearized = fg.linearize(initial);
LinearFactorGraph expected = createLinearFactorGraph();
GaussianFactorGraph linearized = fg.linearize(initial);
GaussianFactorGraph expected = createGaussianFactorGraph();
CHECK(expected.equals(linearized));
}

2
cpp/testSymbolicBayesNet.cpp
View File

@ -32,7 +32,7 @@ TEST( SymbolicBayesNet, constructor )
expected.push_back(x1);
// Create from a factor graph
LinearFactorGraph factorGraph = createLinearFactorGraph();
GaussianFactorGraph factorGraph = createGaussianFactorGraph();
SymbolicFactorGraph fg(factorGraph);
// eliminate it

18
cpp/testSymbolicFactorGraph.cpp
View File

@ -40,7 +40,7 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph )
expected.push_back(f4);
// construct it from the factor graph graph
LinearFactorGraph factorGraph = createLinearFactorGraph();
GaussianFactorGraph factorGraph = createGaussianFactorGraph();
SymbolicFactorGraph actual(factorGraph);
CHECK(assert_equal(expected, actual));
@ -50,7 +50,7 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph )
TEST( SymbolicFactorGraph, findAndRemoveFactors )
{
// construct it from the factor graph graph
LinearFactorGraph factorGraph = createLinearFactorGraph();
GaussianFactorGraph factorGraph = createGaussianFactorGraph();
SymbolicFactorGraph actual(factorGraph);
SymbolicFactor::shared_ptr f1 = actual[0];
SymbolicFactor::shared_ptr f3 = actual[2];
@ -70,7 +70,7 @@ TEST( SymbolicFactorGraph, findAndRemoveFactors )
TEST( SymbolicFactorGraph, factors)
{
// create a test graph
LinearFactorGraph factorGraph = createLinearFactorGraph();
GaussianFactorGraph factorGraph = createGaussianFactorGraph();
SymbolicFactorGraph fg(factorGraph);
// ask for all factor indices connected to x1
@ -87,10 +87,10 @@ TEST( SymbolicFactorGraph, factors)
}
/* ************************************************************************* */
TEST( LinearFactorGraph, removeAndCombineFactors )
TEST( GaussianFactorGraph, removeAndCombineFactors )
{
// create a test graph
LinearFactorGraph factorGraph = createLinearFactorGraph();
GaussianFactorGraph factorGraph = createGaussianFactorGraph();
SymbolicFactorGraph fg(factorGraph);
// combine all factors connected to x1
@ -104,10 +104,10 @@ TEST( LinearFactorGraph, removeAndCombineFactors )
}
/* ************************************************************************* */
TEST( LinearFactorGraph, eliminateOne )
TEST( GaussianFactorGraph, eliminateOne )
{
// create a test graph
LinearFactorGraph factorGraph = createLinearFactorGraph();
GaussianFactorGraph factorGraph = createGaussianFactorGraph();
SymbolicFactorGraph fg(factorGraph);
// eliminate
@ -120,7 +120,7 @@ TEST( LinearFactorGraph, eliminateOne )
}
/* ************************************************************************* */
TEST( LinearFactorGraph, eliminate )
TEST( GaussianFactorGraph, eliminate )
{
// create expected Chordal bayes Net
SymbolicConditional::shared_ptr x2(new SymbolicConditional("x2", "l1", "x1"));
@ -133,7 +133,7 @@ TEST( LinearFactorGraph, eliminate )
expected.push_back(x1);
// create a test graph
LinearFactorGraph factorGraph = createLinearFactorGraph();
GaussianFactorGraph factorGraph = createGaussianFactorGraph();
SymbolicFactorGraph fg(factorGraph);
// eliminate it

8
cpp/testVSLAMFactor.cpp
View File

@ -47,16 +47,16 @@ TEST( VSLAMFactor, error )
Matrix Al1 = Matrix_(2, 3, 61.584, 0., 0., 0., 61.584, 0.);
Matrix Ax1 = Matrix_(2, 6, 0., -369.504, 0., -61.584, 0., 0., 369.504, 0., 0., 0., -61.584, 0.);
Vector b = Vector_(2,3.,0.);
LinearFactor expected("l1", Al1, "x1", Ax1, b, 1);
LinearFactor::shared_ptr actual = factor->linearize(config);
GaussianFactor expected("l1", Al1, "x1", Ax1, b, 1);
GaussianFactor::shared_ptr actual = factor->linearize(config);
CHECK(assert_equal(expected,*actual,1e-3));
// linearize graph
VSLAMGraph graph;
graph.push_back(factor);
LinearFactorGraph expected_lfg;
GaussianFactorGraph expected_lfg;
expected_lfg.push_back(actual);
LinearFactorGraph actual_lfg = graph.linearize(config);
GaussianFactorGraph actual_lfg = graph.linearize(config);
CHECK(assert_equal(expected_lfg,actual_lfg));
// exmap on a config

10
cpp/timeLinearFactor.cpp → cpp/timeGaussianFactor.cpp
View File

@ -1,6 +1,6 @@
/**
* @file timeLinearFactor.cpp
* @brief time LinearFactor.eliminate
* @file timeGaussianFactor.cpp
* @brief time GaussianFactor.eliminate
* @author Alireza Fathi
*/
@ -12,7 +12,7 @@ using namespace std;
#include <boost/tuple/tuple.hpp>
#include "Matrix.h"
#include "LinearFactor.h"
#include "GaussianFactor.h"
using namespace gtsam;
@ -66,11 +66,11 @@ int main()
b2(6) = 2;
b2(7) = -1;
LinearFactor combined("x2", Ax2, "l1", Al1, "x1", Ax1, b2);
GaussianFactor combined("x2", Ax2, "l1", Al1, "x1", Ax1, b2);
long timeLog = clock();
int n = 1000000;
ConditionalGaussian::shared_ptr conditional;
LinearFactor::shared_ptr factor;
GaussianFactor::shared_ptr factor;
for(int i = 0; i < n; i++)
boost::tie(conditional,factor) = combined.eliminate("x2");

8
cpp/timeLinearFactorGraph.cpp → cpp/timeGaussianFactorGraph.cpp
View File

@ -1,12 +1,12 @@
/**
* @file timeLinearFactorGraph.cpp
* @file timeGaussianFactorGraph.cpp
* @brief Time elimination with simple Kalman Smoothing example
* @author Frank Dellaert
*/
#include <time.h>
#include <CppUnitLite/TestHarness.h>
#include "SmallExample.h"
#include "smallExample.h"
#include "Ordering.h"
using namespace std;
@ -15,7 +15,7 @@ using namespace gtsam;
/* ************************************************************************* */
// Create a Kalman smoother for t=1:T and optimize
double timeKalmanSmoother(int T) {
LinearFactorGraph smoother = createSmoother(T);
GaussianFactorGraph smoother = createSmoother(T);
Ordering ordering;
for (int t = 1; t <= T; t++) ordering.push_back(symbol('x',t));
clock_t start = clock();
@ -26,7 +26,7 @@ double timeKalmanSmoother(int T) {
}
/* ************************************************************************* */
TEST(timeLinearFactorGraph, linearTime)
TEST(timeGaussianFactorGraph, linearTime)
{
int T = 1000;
double time1 = timeKalmanSmoother( T); // cout << time1 << endl;

12
matlab/createLinearFactorGraph.m
View File

@ -1,17 +1,17 @@
% create a linear factor graph
% The non-linear graph above evaluated at NoisyConfig
function fg = createLinearFactorGraph()
function fg = createGaussianFactorGraph()
c = createNoisyConfig();
% Create
fg = LinearFactorGraph;
fg = GaussianFactorGraph;
% prior on x1
A11=[10 0; 0 10];
b = - c.get('x1')/0.1;
f1 = LinearFactor('x1', A11, b);
f1 = GaussianFactor('x1', A11, b);
fg.push_back(f1);
% odometry between x1 and x2
@ -19,7 +19,7 @@ A21=[-10 0; 0 -10];
A22=[ 10 0; 0 10];
b = [2;-1];
f2 = LinearFactor('x1', A21, 'x2', A22, b);
f2 = GaussianFactor('x1', A21, 'x2', A22, b);
fg.push_back(f2);
% measurement between x1 and l1
@ -27,7 +27,7 @@ A31=[-5 0; 0 -5];
A32=[ 5 0; 0 5];
b = [0;1];
f3 = LinearFactor('x1', A31, 'l1', A32, b);
f3 = GaussianFactor('x1', A31, 'l1', A32, b);
fg.push_back(f3);
% measurement between x2 and l1
@ -35,7 +35,7 @@ A41=[-5 0; 0 -5];
A42=[ 5 0; 0 5];
b = [-1;1.5];
f4 = LinearFactor('x2', A41, 'l1', A42, b);
f4 = GaussianFactor('x2', A41, 'l1', A42, b);
fg.push_back(f4);
end

2
matlab/create_linear_factor_graph.m
View File

@ -6,7 +6,7 @@ function lfg = create_linear_factor_graph(config, measurements, odometry, measur
m = size(measurements,2);
% create linear factor graph
lfg = LinearFactorGraph();
lfg = GaussianFactorGraph();
% create prior for initial robot pose
prior = Point2Prior([0;0],0.2,'x1');

4
matlab/run_tests.m
View File

@ -1,4 +1,4 @@
% run all matlab unit tests
testLinearFactor
testGaussianFactor
testConditionalGaussian
testLinearFactorGraph
testGaussianFactorGraph

8
matlab/testLinearFactor.m
View File

@ -26,12 +26,12 @@ Ax1 = [
% the RHS
b2=[-1;1.5;2;-1];
combined = LinearFactor('x2', Ax2, 'l1', Al1, 'x1', Ax1, b2, 1);
combined = GaussianFactor('x2', Ax2, 'l1', Al1, 'x1', Ax1, b2, 1);
% eliminate the combined factor
% NOT WORKING
% this is not working because there is no elimination function for a linear
% factor. Just for the MutableLinearFactor
% factor. Just for the MutableGaussianFactor
%[actualCG,actualLF] = combined.eliminate('x2');
% create expected Conditional Gaussian
@ -65,10 +65,10 @@ Bx1 = [
% the RHS
b1= [0.0;0.894427];
expectedLF = LinearFactor('l1', Bl1, 'x1', Bx1, b1, 1);
expectedLF = GaussianFactor('l1', Bl1, 'x1', Bx1, b1, 1);
% check if the result matches
% NOT WORKING
% because can not be computed with LinearFactor.eliminate
% because can not be computed with GaussianFactor.eliminate
%if(~actualCG.equals(expectedCG)), warning('GTSAM:unit','~actualCG.equals(expectedCG)'); end
%if(~actualLF.equals(expectedLF,1e-5)), warning('GTSAM:unit','~actualLF.equals(expectedLF,1e-5)');end

18
matlab/testLinearFactorGraph.m
View File

@ -1,7 +1,7 @@
%-----------------------------------------------------------------------
% equals
fg = createLinearFactorGraph();
fg2 = createLinearFactorGraph();
fg = createGaussianFactorGraph();
fg2 = createGaussianFactorGraph();
CHECK('equals',fg.equals(fg2));
%-----------------------------------------------------------------------
@ -12,7 +12,7 @@ DOUBLES_EQUAL( 5.625, actual, 1e-9 );
%-----------------------------------------------------------------------
% combine_factors_x1
fg = createLinearFactorGraph();
fg = createGaussianFactorGraph();
actual = fg.combine_factors('x1');
Al1 = [
0., 0.
@ -43,22 +43,22 @@ Ax2 = [
b=[-1;-1;2;-1;0;1];
expected = LinearFactor('l1',Al1,'x1',Ax1,'x2',Ax2,b);
expected = GaussianFactor('l1',Al1,'x1',Ax1,'x2',Ax2,b);
CHECK('combine_factors_x1', actual.equals(expected,1e-9));
%-----------------------------------------------------------------------
% combine_factors_x2
fg = createLinearFactorGraph();
fg = createGaussianFactorGraph();
actual = fg.combine_factors('x2');
%-----------------------------------------------------------------------
% eliminate_x1
fg = createLinearFactorGraph();
fg = createGaussianFactorGraph();
actual = fg.eliminate_one('x1');
%-----------------------------------------------------------------------
% eliminate_x2
fg = createLinearFactorGraph();
fg = createGaussianFactorGraph();
actual = fg.eliminate_one('x2');
%-----------------------------------------------------------------------
@ -91,7 +91,7 @@ expected.insert('l1', cg2);
expected.insert('x2', cg3);
% Check one ordering
fg1 = createLinearFactorGraph();
fg1 = createGaussianFactorGraph();
ord1 = Ordering;
ord1.push_back('x2');
ord1.push_back('l1');
@ -102,7 +102,7 @@ CHECK('eliminateAll', actual1.equals(expected));
%-----------------------------------------------------------------------
% matrix
fg = createLinearFactorGraph();
fg = createGaussianFactorGraph();
ord = Ordering;
ord.push_back('x2');
ord.push_back('l1');