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> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildTarget>testLinearFactor.run</buildTarget> <buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildTarget>testLinearFactorGraph.run</buildTarget> <buildTarget>testGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -432,16 +432,16 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildTarget>timeLinearFactor.run</buildTarget> <buildTarget>timeGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildTarget>timeLinearFactorGraph.run</buildTarget> <buildTarget>timeGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <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 Linear Factor....A Gaussian
* @brief linearFactor * @brief linearFactor
* @author Christian Potthast * @author Christian Potthast
@ -12,7 +12,7 @@
#include "Matrix.h" #include "Matrix.h"
#include "Ordering.h" #include "Ordering.h"
#include "ConditionalGaussian.h" #include "ConditionalGaussian.h"
#include "LinearFactor.h" #include "GaussianFactor.h"
using namespace std; using namespace std;
using namespace boost::assign; using namespace boost::assign;
@ -26,7 +26,7 @@ using namespace gtsam;
typedef pair<const string, Matrix>& mypair; 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()) { b_(cg->get_d()) {
As_.insert(make_pair(cg->key(), cg->get_R())); As_.insert(make_pair(cg->key(), cg->get_R()));
std::map<std::string, Matrix>::const_iterator it = cg->parentsBegin(); 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; 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 // Create RHS and sigmas of right size by adding together row counts
size_t m = 0; size_t m = 0;
@ -72,11 +72,11 @@ LinearFactor::LinearFactor(const vector<shared_ptr> & factors)
pos += mf; 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; cout << s << endl;
if (empty()) cout << " empty" << endl; if (empty()) cout << " empty" << endl;
else { 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); const_iterator it = As_.find(key);
if (it != As_.end()) if (it != As_.end())
return it->second.size2(); return it->second.size2();
@ -98,9 +98,9 @@ size_t LinearFactor::getDim(const std::string& key) const {
/* ************************************************************************* */ /* ************************************************************************* */
// Check if two linear factors are equal // 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 (lf == NULL) return false;
if (empty()) return (lf->empty()); 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 // 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; if (empty()) return 0;
Vector e = b_; Vector e = b_;
string j; Matrix Aj; 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; list<string> result;
string j; Matrix A; string j; Matrix A;
FOREACH_PAIR(j,A,As_) FOREACH_PAIR(j,A,As_)
@ -147,7 +147,7 @@ list<string> LinearFactor::keys() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Dimensions LinearFactor::dimensions() const { Dimensions GaussianFactor::dimensions() const {
Dimensions result; Dimensions result;
string j; Matrix A; string j; Matrix A;
FOREACH_PAIR(j,A,As_) 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)) { if(involves(key)) {
string j; Matrix A; string j; Matrix A;
FOREACH_PAIR(j,A,As_) 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 // get pointers to the matrices
vector<const Matrix *> 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 // get pointers to the matrices
vector<const Matrix *> matrices; vector<const Matrix *> matrices;
BOOST_FOREACH(string j, ordering) { 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> > 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 // declare return values
list<int> I,J; 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) { const size_t pos) {
bool verbose = false; 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 // 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++) { for (; it != f->end(); it++) {
string j = it->first; string j = it->first;
Matrix A = it->second; 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); if (exists) As_.erase(j);
insert(j, Anew); 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 * and last rows to make a new joint linear factor on separator
*/ */
/* ************************************************************************* */ /* ************************************************************************* */
pair<ConditionalGaussian::shared_ptr, LinearFactor::shared_ptr> pair<ConditionalGaussian::shared_ptr, GaussianFactor::shared_ptr>
LinearFactor::eliminate(const string& key) const GaussianFactor::eliminate(const string& key) const
{ {
bool verbose = false; 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 // if this factor does not involve key, we exit with empty CG and LF
const_iterator it = As_.find(key); const_iterator it = As_.find(key);
if (it==As_.end()) { if (it==As_.end()) {
// Conditional Gaussian is just a parent-less node with P(x)=1 // 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)); ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key));
return make_pair(cg,lf); return make_pair(cg,lf);
} }
@ -324,7 +324,7 @@ LinearFactor::eliminate(const string& key) const
// if m<n1, this factor cannot be eliminated // if m<n1, this factor cannot be eliminated
size_t maxRank = solution.size(); size_t maxRank = solution.size();
if (maxRank<n1) if (maxRank<n1)
throw(domain_error("LinearFactor::eliminate: fewer constraints than unknowns")); throw(domain_error("GaussianFactor::eliminate: fewer constraints than unknowns"));
// unpack the solutions // unpack the solutions
Matrix R(maxRank, n); Matrix R(maxRank, n);
@ -345,7 +345,7 @@ LinearFactor::eliminate(const string& key) const
sub(newSigmas, 0, n1))); // get standard deviations sub(newSigmas, 0, n1))); // get standard deviations
// extract the block matrices for parents in both CG and LF // 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; size_t j = n1;
BOOST_FOREACH(string cur_key, ordering) BOOST_FOREACH(string cur_key, ordering)
if (cur_key!=key) { 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 Linear Factor....A Gaussian
* @brief linearFactor * @brief linearFactor
* @author Christian Potthast * @author Christian Potthast
@ -24,13 +24,13 @@ namespace gtsam {
/** /**
* Base Class for a linear factor. * 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) * The factor value is exp(-0.5*||Ax-b||^2)
*/ */
class LinearFactor: public Factor<VectorConfig> { class GaussianFactor: public Factor<VectorConfig> {
public: 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>::iterator iterator;
typedef std::map<std::string, Matrix>::const_iterator const_iterator; typedef std::map<std::string, Matrix>::const_iterator const_iterator;
@ -43,23 +43,23 @@ protected:
public: public:
// TODO: eradicate, as implies non-const // TODO: eradicate, as implies non-const
LinearFactor() { GaussianFactor() {
} }
/** Construct Null factor */ /** Construct Null factor */
LinearFactor(const Vector& b_in) : GaussianFactor(const Vector& b_in) :
b_(b_in), sigmas_(ones(b_in.size())){ b_(b_in), sigmas_(ones(b_in.size())){
} }
/** Construct unary factor */ /** 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) : const Vector& b_in, double sigma) :
b_(b_in), sigmas_(repeat(b_in.size(),sigma)) { b_(b_in), sigmas_(repeat(b_in.size(),sigma)) {
As_.insert(make_pair(key1, A1)); As_.insert(make_pair(key1, A1));
} }
/** Construct binary factor */ /** 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 std::string& key2, const Matrix& A2,
const Vector& b_in, double sigma) : const Vector& b_in, double sigma) :
b_(b_in), sigmas_(repeat(b_in.size(),sigma)) { b_(b_in), sigmas_(repeat(b_in.size(),sigma)) {
@ -68,7 +68,7 @@ public:
} }
/** Construct ternary factor */ /** 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& key2, const Matrix& A2,
const std::string& key3, const Matrix& A3, const std::string& key3, const Matrix& A3,
const Vector& b_in, double sigma) : const Vector& b_in, double sigma) :
@ -79,7 +79,7 @@ public:
} }
/** Construct an n-ary factor */ /** 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) : const Vector &b_in, double sigma) :
b_(b_in), sigmas_(repeat(b_in.size(),sigma)) { b_(b_in), sigmas_(repeat(b_in.size(),sigma)) {
for(unsigned int i=0; i<terms.size(); i++) for(unsigned int i=0; i<terms.size(); i++)
@ -87,7 +87,7 @@ public:
} }
/** Construct an n-ary factor with a multiple sigmas*/ /** 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) : const Vector &b_in, const Vector& sigmas) :
b_(b_in), sigmas_(sigmas) { b_(b_in), sigmas_(sigmas) {
for (unsigned int i = 0; i < terms.size(); i++) for (unsigned int i = 0; i < terms.size(); i++)
@ -95,13 +95,13 @@ public:
} }
/** Construct from Conditional Gaussian */ /** 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 * Constructor that combines a set of factors
* @param factors Set of factors to combine * @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 // Implementing Testable virtual functions
@ -135,7 +135,7 @@ public:
const Matrix& get_A(const std::string& key) const { const Matrix& get_A(const std::string& key) const {
const_iterator it = As_.find(key); const_iterator it = As_.find(key);
if (it == As_.end()) if (it == As_.end())
throw(std::invalid_argument("LinearFactor::[] invalid key: " + key)); throw(std::invalid_argument("GaussianFactor::[] invalid key: " + key));
return it->second; return it->second;
} }
@ -241,10 +241,10 @@ public:
* @param m final number of rows of f, needs to be known in advance * @param m final number of rows of f, needs to be known in advance
* @param pos where to insert in the m-sized matrices * @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); 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 * @brief Linear Factor Graph where all factors are Gaussians
* @author Kai Ni * @author Kai Ni
* @author Christian Potthast * @author Christian Potthast
@ -12,8 +12,8 @@
#include <colamd/colamd.h> #include <colamd/colamd.h>
#include "LinearFactorGraph.h" #include "GaussianFactorGraph.h"
#include "LinearFactorSet.h" #include "GaussianFactorSet.h"
#include "FactorGraph-inl.h" #include "FactorGraph-inl.h"
#include "inference-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) #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
// Explicitly instantiate so we don't have to include everywhere // Explicitly instantiate so we don't have to include everywhere
template class FactorGraph<LinearFactor>; template class FactorGraph<GaussianFactor>;
/* ************************************************************************* */ /* ************************************************************************* */
LinearFactorGraph::LinearFactorGraph(const GaussianBayesNet& CBN) : GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) :
FactorGraph<LinearFactor> (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; set<string> separator;
BOOST_FOREACH(sharedFactor factor,factors_) BOOST_FOREACH(sharedFactor factor,factors_)
@ -43,13 +43,13 @@ set<string> LinearFactorGraph::find_separator(const string& key) const
/* ************************************************************************* */ /* ************************************************************************* */
ConditionalGaussian::shared_ptr ConditionalGaussian::shared_ptr
LinearFactorGraph::eliminateOne(const std::string& key) { GaussianFactorGraph::eliminateOne(const std::string& key) {
return gtsam::eliminateOne<LinearFactor,ConditionalGaussian>(*this, key); return gtsam::eliminateOne<GaussianFactor,ConditionalGaussian>(*this, key);
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianBayesNet GaussianBayesNet
LinearFactorGraph::eliminate(const Ordering& ordering) GaussianFactorGraph::eliminate(const Ordering& ordering)
{ {
GaussianBayesNet chordalBayesNet; // empty GaussianBayesNet chordalBayesNet; // empty
BOOST_FOREACH(string key, ordering) { 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 // eliminate all nodes in the given ordering -> chordal Bayes net
GaussianBayesNet chordalBayesNet = eliminate(ordering); GaussianBayesNet chordalBayesNet = eliminate(ordering);
@ -71,7 +71,7 @@ VectorConfig LinearFactorGraph::optimize(const Ordering& ordering)
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<GaussianBayesNet> boost::shared_ptr<GaussianBayesNet>
LinearFactorGraph::eliminate_(const Ordering& ordering) GaussianFactorGraph::eliminate_(const Ordering& ordering)
{ {
boost::shared_ptr<GaussianBayesNet> chordalBayesNet(new GaussianBayesNet); // empty boost::shared_ptr<GaussianBayesNet> chordalBayesNet(new GaussianBayesNet); // empty
BOOST_FOREACH(string key, ordering) { BOOST_FOREACH(string key, ordering) {
@ -83,23 +83,23 @@ LinearFactorGraph::eliminate_(const Ordering& ordering)
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<VectorConfig> boost::shared_ptr<VectorConfig>
LinearFactorGraph::optimize_(const Ordering& ordering) { GaussianFactorGraph::optimize_(const Ordering& ordering) {
return boost::shared_ptr<VectorConfig>(new VectorConfig(optimize(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++){ for(const_iterator factor=lfg.factors_.begin(); factor!=lfg.factors_.end(); factor++){
push_back(*factor); push_back(*factor);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
LinearFactorGraph LinearFactorGraph::combine2(const LinearFactorGraph& lfg1, GaussianFactorGraph GaussianFactorGraph::combine2(const GaussianFactorGraph& lfg1,
const LinearFactorGraph& lfg2) { const GaussianFactorGraph& lfg2) {
// create new linear factor graph equal to the first one // create new linear factor graph equal to the first one
LinearFactorGraph fg = lfg1; GaussianFactorGraph fg = lfg1;
// add the second factors_ in the graph // add the second factors_ in the graph
for (const_iterator factor = lfg2.factors_.begin(); factor 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; Dimensions result;
BOOST_FOREACH(sharedFactor factor,factors_) { BOOST_FOREACH(sharedFactor factor,factors_) {
Dimensions vs = factor->dimensions(); 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 // start with this factor graph
LinearFactorGraph result = *this; GaussianFactorGraph result = *this;
// find all variables and their dimensions // find all variables and their dimensions
Dimensions vs = dimensions(); Dimensions vs = dimensions();
@ -135,29 +135,29 @@ LinearFactorGraph LinearFactorGraph::add_priors(double sigma) const {
FOREACH_PAIR(key,dim,vs) { FOREACH_PAIR(key,dim,vs) {
Matrix A = eye(dim); Matrix A = eye(dim);
Vector b = zero(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); result.push_back(prior);
} }
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
pair<Matrix,Vector> LinearFactorGraph::matrix(const Ordering& ordering) const { pair<Matrix,Vector> GaussianFactorGraph::matrix(const Ordering& ordering) const {
// get all factors // get all factors
LinearFactorSet found; GaussianFactorSet found;
BOOST_FOREACH(sharedFactor factor,factors_) BOOST_FOREACH(sharedFactor factor,factors_)
found.push_back(factor); found.push_back(factor);
// combine them // combine them
LinearFactor lf(found); GaussianFactor lf(found);
// Return Matrix and Vector // Return Matrix and Vector
return lf.matrix(ordering); return lf.matrix(ordering);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix LinearFactorGraph::sparse(const Ordering& ordering) const { Matrix GaussianFactorGraph::sparse(const Ordering& ordering) const {
// return values // return values
list<int> I,J; 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 * @brief Linear Factor Graph where all factors are Gaussians
* @author Kai Ni * @author Kai Ni
* @author Christian Potthast * @author Christian Potthast
* @author Alireza Fathi * @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 // \callgraph
@ -14,7 +14,7 @@
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include "FactorGraph.h" #include "FactorGraph.h"
#include "LinearFactor.h" #include "GaussianFactor.h"
#include "GaussianBayesNet.h" // needed for MATLAB toolbox !! #include "GaussianBayesNet.h" // needed for MATLAB toolbox !!
namespace gtsam { namespace gtsam {
@ -23,22 +23,22 @@ namespace gtsam {
/** /**
* A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. * A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
* Factor == LinearFactor * Factor == GaussianFactor
* VectorConfig = A configuration of vectors * VectorConfig = A configuration of vectors
* Most of the time, linear factor graphs arise by linearizing a non-linear factor graph. * 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: public:
/** /**
* Default constructor * 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 */ /** unnormalized error */
double error(const VectorConfig& c) const { double error(const VectorConfig& c) const {
@ -93,14 +93,14 @@ namespace gtsam {
* @param const &lfg2 Linear factor graph * @param const &lfg2 Linear factor graph
* @return a new combined factor graph * @return a new combined factor graph
*/ */
static LinearFactorGraph combine2(const LinearFactorGraph& lfg1, static GaussianFactorGraph combine2(const GaussianFactorGraph& lfg1,
const LinearFactorGraph& lfg2); const GaussianFactorGraph& lfg2);
/** /**
* combine two factor graphs * combine two factor graphs
* @param *lfg Linear factor graph * @param *lfg Linear factor graph
*/ */
void combine(const LinearFactorGraph &lfg); void combine(const GaussianFactorGraph &lfg);
/** /**
* Find all variables and their dimensions * Find all variables and their dimensions
@ -112,7 +112,7 @@ namespace gtsam {
* Add zero-mean i.i.d. Gaussian prior terms to each variable * Add zero-mean i.i.d. Gaussian prior terms to each variable
* @param sigma Standard deviation of Gaussian * @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 * 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 * @brief Utility class: an STL set of linear factors, basically a wrappable typedef
* @author Frank Dellaert * @author Frank Dellaert
*/ */
@ -8,14 +8,14 @@
#include <set> #include <set>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include "LinearFactor.h" #include "GaussianFactor.h"
namespace gtsam { namespace gtsam {
class LinearFactor; class GaussianFactor;
// We use a vector not a an STL set, to get predictable ordering across platforms // We use a vector not a an STL set, to get predictable ordering across platforms
struct LinearFactorSet : std::vector<boost::shared_ptr<LinearFactor> > { struct GaussianFactorSet : std::vector<boost::shared_ptr<GaussianFactor> > {
LinearFactorSet() {} GaussianFactorSet() {}
}; };
} }

26
cpp/Makefile.am
View File

@ -84,29 +84,29 @@ testSymbolicBayesNet_SOURCES = $(example) testSymbolicBayesNet.cpp
testSymbolicBayesNet_LDADD = libgtsam.la testSymbolicBayesNet_LDADD = libgtsam.la
# Gaussian inference # Gaussian inference
headers += LinearFactorSet.h headers += GaussianFactorSet.h
sources += VectorConfig.cpp LinearFactor.cpp LinearFactorGraph.cpp ConditionalGaussian.cpp GaussianBayesNet.cpp sources += VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp ConditionalGaussian.cpp GaussianBayesNet.cpp
check_PROGRAMS += testVectorConfig testLinearFactor testLinearFactorGraph testConditionalGaussian testGaussianBayesNet check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testConditionalGaussian testGaussianBayesNet
testVectorConfig_SOURCES = testVectorConfig.cpp testVectorConfig_SOURCES = testVectorConfig.cpp
testVectorConfig_LDADD = libgtsam.la testVectorConfig_LDADD = libgtsam.la
testLinearFactor_SOURCES = $(example) testLinearFactor.cpp testGaussianFactor_SOURCES = $(example) testGaussianFactor.cpp
testLinearFactor_LDADD = libgtsam.la testGaussianFactor_LDADD = libgtsam.la
testLinearFactorGraph_SOURCES = $(example) testLinearFactorGraph.cpp testGaussianFactorGraph_SOURCES = $(example) testGaussianFactorGraph.cpp
testLinearFactorGraph_LDADD = libgtsam.la testGaussianFactorGraph_LDADD = libgtsam.la
testConditionalGaussian_SOURCES = $(example) testConditionalGaussian.cpp testConditionalGaussian_SOURCES = $(example) testConditionalGaussian.cpp
testConditionalGaussian_LDADD = libgtsam.la testConditionalGaussian_LDADD = libgtsam.la
testGaussianBayesNet_SOURCES = $(example) testGaussianBayesNet.cpp testGaussianBayesNet_SOURCES = $(example) testGaussianBayesNet.cpp
testGaussianBayesNet_LDADD = libgtsam.la testGaussianBayesNet_LDADD = libgtsam.la
# not the correct way, I'm sure: Kai ? # not the correct way, I'm sure: Kai ?
timeLinearFactor: timeLinearFactor.cpp timeGaussianFactor: timeGaussianFactor.cpp
timeLinearFactor: CXXFLAGS += -I /opt/local/include timeGaussianFactor: CXXFLAGS += -I /opt/local/include
timeLinearFactor: LDFLAGS += -L.libs -lgtsam timeGaussianFactor: LDFLAGS += -L.libs -lgtsam
# not the correct way, I'm sure: Kai ? # not the correct way, I'm sure: Kai ?
timeLinearFactorGraph: timeLinearFactorGraph.cpp timeGaussianFactorGraph: timeGaussianFactorGraph.cpp
timeLinearFactorGraph: CXXFLAGS += -I /opt/local/include -I .. timeGaussianFactorGraph: CXXFLAGS += -I /opt/local/include -I ..
timeLinearFactorGraph: LDFLAGS += SmallExample.o -L.libs -lgtsam -L../CppUnitLite -lCppUnitLite timeGaussianFactorGraph: LDFLAGS += smallExample.o -L.libs -lgtsam -L../CppUnitLite -lCppUnitLite
# Nonlinear inference # Nonlinear inference
headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h 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' * 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 * but only in relevant part, from row j onwards
* If called from householder_ does actually more work as first j columns * 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. * 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 { void NonlinearFactor1::print(const string& s) const {
cout << "NonLinearFactor1 " << s << endl; cout << "NonGaussianFactor1 " << s << endl;
cout << "h : " << (void*)h_ << endl; cout << "h : " << (void*)h_ << endl;
cout << "key: " << key_ << endl; cout << "key: " << key_ << endl;
cout << "H : " << (void*)H_ << 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 // get argument 1 from config
Vector x1 = c[key_]; 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 // Right-hand-side b = error(c) = (z - h(x1))/sigma
Vector b = (z_ - h_(x1)); 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; return p;
} }
@ -77,7 +77,7 @@ NonlinearFactor2::NonlinearFactor2(const Vector& z,
/* ************************************************************************* */ /* ************************************************************************* */
void NonlinearFactor2::print(const string& s) const { void NonlinearFactor2::print(const string& s) const {
cout << "NonLinearFactor2 " << s << endl; cout << "NonGaussianFactor2 " << s << endl;
cout << "h : " << (void*)h_ << endl; cout << "h : " << (void*)h_ << endl;
cout << "key1: " << key1_ << endl; cout << "key1: " << key1_ << endl;
cout << "H2 : " << (void*)H2_ << 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 // get arguments from config
Vector x1 = c[key1_]; Vector x1 = c[key1_];
Vector x2 = c[key2_]; 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 // Right-hand-side b = (z - h(x))/sigma
Vector b = (z_ - h_(x1, x2)); 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; return p;
} }

18
cpp/NonlinearFactor.h
View File

@ -20,7 +20,7 @@
#include "Factor.h" #include "Factor.h"
#include "Vector.h" #include "Vector.h"
#include "Matrix.h" #include "Matrix.h"
#include "LinearFactor.h" #include "GaussianFactor.h"
/** /**
* Base Class * Base Class
@ -29,9 +29,9 @@
namespace gtsam { namespace gtsam {
// forward declaration of LinearFactor // forward declaration of GaussianFactor
//class LinearFactor; //class GaussianFactor;
//typedef boost::shared_ptr<LinearFactor> shared_ptr; //typedef boost::shared_ptr<GaussianFactor> shared_ptr;
/** /**
* Nonlinear factor which assumes Gaussian noise on a measurement * Nonlinear factor which assumes Gaussian noise on a measurement
@ -66,7 +66,7 @@ namespace gtsam {
/** print */ /** print */
void print(const std::string& s = "") const { void print(const std::string& s = "") const {
std::cout << "NonLinearFactor " << s << std::endl; std::cout << "NonGaussianFactor " << s << std::endl;
gtsam::print(z_, " z = "); gtsam::print(z_, " z = ");
std::cout << " sigma = " << sigma_ << std::endl; std::cout << " sigma = " << sigma_ << std::endl;
} }
@ -81,8 +81,8 @@ namespace gtsam {
/** Vector of errors */ /** Vector of errors */
virtual Vector error_vector(const Config& c) const = 0; virtual Vector error_vector(const Config& c) const = 0;
/** linearize to a LinearFactor */ /** linearize to a GaussianFactor */
virtual boost::shared_ptr<LinearFactor> linearize(const Config& c) const = 0; virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const = 0;
/** get functions */ /** get functions */
double sigma() const {return sigma_;} double sigma() const {return sigma_;}
@ -145,7 +145,7 @@ namespace gtsam {
} }
/** linearize a non-linearFactor1 to get a linearFactor1 */ /** 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 */ /** 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 #pragma once
#include "LinearFactorGraph.h" #include "GaussianFactorGraph.h"
#include "NonlinearFactorGraph.h" #include "NonlinearFactorGraph.h"
namespace gtsam { namespace gtsam {
@ -28,21 +28,21 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Config> template<class Config>
LinearFactorGraph NonlinearFactorGraph<Config>::linearize( GaussianFactorGraph NonlinearFactorGraph<Config>::linearize(
const Config& config) const { const Config& config) const {
// TODO speed up the function either by returning a pointer or by // TODO speed up the function either by returning a pointer or by
// returning the linearisation as a second argument and returning // returning the linearisation as a second argument and returning
// the reference // the reference
// create an empty linear FG // create an empty linear FG
LinearFactorGraph linearFG; GaussianFactorGraph linearFG;
typedef typename FactorGraph<NonlinearFactor<Config> >::const_iterator typedef typename FactorGraph<NonlinearFactor<Config> >::const_iterator
const_iterator; const_iterator;
// linearize all factors // linearize all factors
for (const_iterator factor = this->factors_.begin(); factor for (const_iterator factor = this->factors_.begin(); factor
< this->factors_.end(); 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); linearFG.push_back(lf);
} }

6
cpp/NonlinearFactorGraph.h
View File

@ -11,13 +11,13 @@
#pragma once #pragma once
#include "NonlinearFactor.h" #include "NonlinearFactor.h"
#include "LinearFactorGraph.h" #include "GaussianFactorGraph.h"
namespace gtsam { namespace gtsam {
/** /**
* A non-linear factor graph is templated on a configuration, but the factor type * 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. * 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 * 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 * 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 * 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 // linearize the non-linear graph around the current config
// which gives a linear optimization problem in the tangent space // 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 // solve for the optimal displacement in the tangent space
VectorConfig delta = linear.optimize(*ordering_); VectorConfig delta = linear.optimize(*ordering_);
@ -118,13 +118,13 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class C> template<class G, class C>
NonlinearOptimizer<G, C> NonlinearOptimizer<G, C>::try_lambda( 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) if (verbosity >= TRYLAMBDA)
cout << "trying lambda = " << lambda_ << endl; cout << "trying lambda = " << lambda_ << endl;
// add prior-factors // add prior-factors
LinearFactorGraph damped = linear.add_priors(1.0/sqrt(lambda_)); GaussianFactorGraph damped = linear.add_priors(1.0/sqrt(lambda_));
if (verbosity >= DAMPED) if (verbosity >= DAMPED)
damped.print("damped"); damped.print("damped");
@ -167,7 +167,7 @@ namespace gtsam {
cout << "lambda = " << lambda_ << endl; cout << "lambda = " << lambda_ << endl;
// linearize all factors once // linearize all factors once
LinearFactorGraph linear = graph_->linearize(*config_); GaussianFactorGraph linear = graph_->linearize(*config_);
if (verbosity >= LINEAR) if (verbosity >= LINEAR)
linear.print("linear"); linear.print("linear");

2
cpp/NonlinearOptimizer.h
View File

@ -62,7 +62,7 @@ namespace gtsam {
double lambda_; double lambda_;
// Recursively try to do tempered Gauss-Newton steps until we succeed // 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; verbosityLevel verbosity, double factor) const;
public: public:

2
cpp/Testable.h
View File

@ -32,7 +32,7 @@ namespace gtsam {
/** /**
* equality up to tolerance * 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 * equals is not supposed to print out *anything*, just return true|false
*/ */
virtual bool equals(const Derived& expected, double tol) const = 0; 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 // get arguments from config
Pose3 pose = c.cameraPose(cameraFrameNumber_); Pose3 pose = c.cameraPose(cameraFrameNumber_);
@ -74,8 +74,8 @@ LinearFactor::shared_ptr VSLAMFactor::linearize(const VSLAMConfig& c) const
Vector b = this->z_ - h; Vector b = this->z_ - h;
// Make new linearfactor, divide by sigma // Make new linearfactor, divide by sigma
LinearFactor::shared_ptr GaussianFactor::shared_ptr
p(new LinearFactor(cameraFrameName_, Dh1, landmarkName_, Dh2, b, this->sigma_)); p(new GaussianFactor(cameraFrameName_, Dh1, landmarkName_, Dh2, b, this->sigma_));
return p; return p;
} }

4
cpp/VSLAMFactor.h
View File

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

30
cpp/gtsam.h
View File

@ -18,23 +18,23 @@ class VectorConfig {
void clear(); void clear();
}; };
class LinearFactorSet { class GaussianFactorSet {
LinearFactorSet(); GaussianFactorSet();
void push_back(LinearFactor* factor); void push_back(GaussianFactor* factor);
}; };
class LinearFactor { class GaussianFactor {
LinearFactor(string key1, GaussianFactor(string key1,
Matrix A1, Matrix A1,
Vector b_in, Vector b_in,
double sigma); double sigma);
LinearFactor(string key1, GaussianFactor(string key1,
Matrix A1, Matrix A1,
string key2, string key2,
Matrix A2, Matrix A2,
Vector b_in, Vector b_in,
double sigma); double sigma);
LinearFactor(string key1, GaussianFactor(string key1,
Matrix A1, Matrix A1,
string key2, string key2,
Matrix A2, Matrix A2,
@ -48,7 +48,7 @@ class LinearFactor {
double error(const VectorConfig& c) const; double error(const VectorConfig& c) const;
bool involves(string key) const; bool involves(string key) const;
void print(string s) 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; pair<Matrix,Vector> matrix(const Ordering& ordering) const;
}; };
@ -86,15 +86,15 @@ class GaussianBayesNet {
void push_front(ConditionalGaussian* conditional); void push_front(ConditionalGaussian* conditional);
}; };
class LinearFactorGraph { class GaussianFactorGraph {
LinearFactorGraph(); GaussianFactorGraph();
size_t size() const; size_t size() const;
void push_back(LinearFactor* ptr_f); void push_back(GaussianFactor* ptr_f);
double error(const VectorConfig& c) const; double error(const VectorConfig& c) const;
double probPrime(const VectorConfig& c) const; double probPrime(const VectorConfig& c) const;
void print(string s) 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); ConditionalGaussian* eliminateOne(string key);
GaussianBayesNet* eliminate_(const Ordering& ordering); GaussianBayesNet* eliminate_(const Ordering& ordering);
@ -128,7 +128,7 @@ class Point3 {
class Point2Prior { class Point2Prior {
Point2Prior(Vector mu, double sigma, string key); Point2Prior(Vector mu, double sigma, string key);
Vector error_vector(const VectorConfig& c) const; Vector error_vector(const VectorConfig& c) const;
LinearFactor* linearize(const VectorConfig& c) const; GaussianFactor* linearize(const VectorConfig& c) const;
double sigma(); double sigma();
Vector measurement(); Vector measurement();
double error(const VectorConfig& c) const; double error(const VectorConfig& c) const;
@ -138,7 +138,7 @@ class Point2Prior {
class Simulated2DOdometry { class Simulated2DOdometry {
Simulated2DOdometry(Vector odo, double sigma, string key, string key2); Simulated2DOdometry(Vector odo, double sigma, string key, string key2);
Vector error_vector(const VectorConfig& c) const; Vector error_vector(const VectorConfig& c) const;
LinearFactor* linearize(const VectorConfig& c) const; GaussianFactor* linearize(const VectorConfig& c) const;
double sigma(); double sigma();
Vector measurement(); Vector measurement();
double error(const VectorConfig& c) const; double error(const VectorConfig& c) const;
@ -148,7 +148,7 @@ class Simulated2DOdometry {
class Simulated2DMeasurement { class Simulated2DMeasurement {
Simulated2DMeasurement(Vector odo, double sigma, string key, string key2); Simulated2DMeasurement(Vector odo, double sigma, string key, string key2);
Vector error_vector(const VectorConfig& c) const; Vector error_vector(const VectorConfig& c) const;
LinearFactor* linearize(const VectorConfig& c) const; GaussianFactor* linearize(const VectorConfig& c) const;
double sigma(); double sigma();
Vector measurement(); Vector measurement();
double error(const VectorConfig& c) const; 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. // version that returns a more specific GaussianBayesNet.
// Note, you will need to include this file to instantiate the function. // 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 # basic
sources = Vector.cpp svdcmp.cpp Matrix.cpp numericalDerivative.cpp Ordering.cpp sources = Vector.cpp svdcmp.cpp Matrix.cpp numericalDerivative.cpp Ordering.cpp
# nodes # nodes
sources += FGConfig.cpp LinearFactor.cpp ConditionalGaussian.cpp NonlinearFactor.cpp sources += FGConfig.cpp GaussianFactor.cpp ConditionalGaussian.cpp NonlinearFactor.cpp
# graphs # graphs
sources += FactorGraph.cpp LinearFactorGraph.cpp NonlinearFactorGraph.cpp ChordalBayesNet.cpp sources += FactorGraph.cpp GaussianFactorGraph.cpp NonlinearFactorGraph.cpp ChordalBayesNet.cpp
# geometry # geometry
sources += Point2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp sources += Point2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
@ -109,8 +109,8 @@ tests: unit-tests timing-tests
clean-tests: clean-tests:
-rm -rf $(executables) -rm -rf $(executables)
# make a version of timeLinearFactor instrumented for Saturn profiler # make a version of timeGaussianFactor instrumented for Saturn profiler
saturn: timeLinearFactor saturn: timeGaussianFactor
saturn: CXXFLAGS += -finstrument-functions saturn: CXXFLAGS += -finstrument-functions
saturn: LDLIBS += -lSaturn saturn: LDLIBS += -lSaturn

56
cpp/smallExample.cpp
View File

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

12
cpp/smallExample.h
View File

@ -49,7 +49,7 @@ namespace gtsam {
* create a linear factor graph * create a linear factor graph
* The non-linear graph above evaluated at NoisyConfig * The non-linear graph above evaluated at NoisyConfig
*/ */
LinearFactorGraph createLinearFactorGraph(); GaussianFactorGraph createGaussianFactorGraph();
/** /**
* create small Chordal Bayes Net x <- y * create small Chordal Bayes Net x <- y
@ -66,7 +66,7 @@ namespace gtsam {
* Create a Kalman smoother by linearizing a non-linear factor graph * Create a Kalman smoother by linearizing a non-linear factor graph
* @param T number of time-steps * @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 * Creates a simple constrained graph with one linear factor and
* one binary equality constraint that sets x = y * one binary equality constraint that sets x = y
*/ */
LinearFactorGraph createSimpleConstraintGraph(); GaussianFactorGraph createSimpleConstraintGraph();
VectorConfig createSimpleConstraintConfig(); VectorConfig createSimpleConstraintConfig();
/** /**
* Creates a simple constrained graph with one linear factor and * Creates a simple constrained graph with one linear factor and
* one binary constraint. * one binary constraint.
*/ */
LinearFactorGraph createSingleConstraintGraph(); GaussianFactorGraph createSingleConstraintGraph();
VectorConfig createSingleConstraintConfig(); VectorConfig createSingleConstraintConfig();
/** /**
* Creates a constrained graph with a linear factor and two * Creates a constrained graph with a linear factor and two
* binary constraints that share a node * binary constraints that share a node
*/ */
LinearFactorGraph createMultiConstraintGraph(); GaussianFactorGraph createMultiConstraintGraph();
VectorConfig createMultiConstraintConfig(); VectorConfig createMultiConstraintConfig();
/** /**
@ -117,7 +117,7 @@ namespace gtsam {
/** /**
* Create small example constrained factor graph * Create small example constrained factor graph
*/ */
//LinearFactorGraph createConstrainedLinearFactorGraph(); //GaussianFactorGraph createConstrainedGaussianFactorGraph();
/** /**
* Create small example constrained nonlinear factor graph * 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 ) TEST( BayesTree, linear_smoother_shortcuts )
{ {
// Create smoother with 7 nodes // Create smoother with 7 nodes
LinearFactorGraph smoother = createSmoother(7); GaussianFactorGraph smoother = createSmoother(7);
Ordering ordering; Ordering ordering;
for (int t = 1; t <= 7; t++) for (int t = 1; t <= 7; t++)
ordering.push_back(symbol('x', t)); ordering.push_back(symbol('x', t));
@ -110,12 +110,12 @@ TEST( BayesTree, linear_smoother_shortcuts )
// Check the conditional P(Root|Root) // Check the conditional P(Root|Root)
GaussianBayesNet empty; GaussianBayesNet empty;
Gaussian::sharedClique R = bayesTree.root(); 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(assert_equal(empty,actual1,1e-4));
// Check the conditional P(C2|Root) // Check the conditional P(C2|Root)
Gaussian::sharedClique C2 = bayesTree["x5"]; 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(assert_equal(empty,actual2,1e-4));
// Check the conditional P(C3|Root) // Check the conditional P(C3|Root)
@ -124,7 +124,7 @@ TEST( BayesTree, linear_smoother_shortcuts )
GaussianBayesNet expected3; GaussianBayesNet expected3;
push_front(expected3,"x5", zero(2), eye(2), "x6", A56, sigma3); push_front(expected3,"x5", zero(2), eye(2), "x6", A56, sigma3);
Gaussian::sharedClique C3 = bayesTree["x4"]; 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(assert_equal(expected3,actual3,1e-4));
// Check the conditional P(C4|Root) // Check the conditional P(C4|Root)
@ -133,7 +133,7 @@ TEST( BayesTree, linear_smoother_shortcuts )
GaussianBayesNet expected4; GaussianBayesNet expected4;
push_front(expected4,"x4", zero(2), eye(2), "x6", A46, sigma4); push_front(expected4,"x4", zero(2), eye(2), "x6", A46, sigma4);
Gaussian::sharedClique C4 = bayesTree["x3"]; Gaussian::sharedClique C4 = bayesTree["x3"];
GaussianBayesNet actual4 = C4->shortcut<LinearFactor>(R); GaussianBayesNet actual4 = C4->shortcut<GaussianFactor>(R);
CHECK(assert_equal(expected4,actual4,1e-4)); CHECK(assert_equal(expected4,actual4,1e-4));
} }
@ -159,7 +159,7 @@ TEST( BayesTree, linear_smoother_shortcuts )
TEST( BayesTree, balanced_smoother_marginals ) TEST( BayesTree, balanced_smoother_marginals )
{ {
// Create smoother with 7 nodes // Create smoother with 7 nodes
LinearFactorGraph smoother = createSmoother(7); GaussianFactorGraph smoother = createSmoother(7);
Ordering ordering; Ordering ordering;
ordering += "x1","x3","x5","x7","x2","x6","x4"; ordering += "x1","x3","x5","x7","x2","x6","x4";
@ -178,27 +178,27 @@ TEST( BayesTree, balanced_smoother_marginals )
// Check marginal on x1 // Check marginal on x1
GaussianBayesNet expected1 = simpleGaussian("x1", zero(2), sigmax1); 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(assert_equal(expected1,actual1,1e-4));
// Check marginal on x2 // Check marginal on x2
GaussianBayesNet expected2 = simpleGaussian("x2", zero(2), sigmax2); 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(assert_equal(expected2,actual2,1e-4));
// Check marginal on x3 // Check marginal on x3
GaussianBayesNet expected3 = simpleGaussian("x3", zero(2), sigmax3); 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(assert_equal(expected3,actual3,1e-4));
// Check marginal on x4 // Check marginal on x4
GaussianBayesNet expected4 = simpleGaussian("x4", zero(2), sigmax4); 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(assert_equal(expected4,actual4,1e-4));
// Check marginal on x7 (should be equal to x1) // Check marginal on x7 (should be equal to x1)
GaussianBayesNet expected7 = simpleGaussian("x7", zero(2), sigmax7); 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)); CHECK(assert_equal(expected7,actual7,1e-4));
} }
@ -206,7 +206,7 @@ TEST( BayesTree, balanced_smoother_marginals )
TEST( BayesTree, balanced_smoother_shortcuts ) TEST( BayesTree, balanced_smoother_shortcuts )
{ {
// Create smoother with 7 nodes // Create smoother with 7 nodes
LinearFactorGraph smoother = createSmoother(7); GaussianFactorGraph smoother = createSmoother(7);
Ordering ordering; Ordering ordering;
ordering += "x1","x3","x5","x7","x2","x6","x4"; ordering += "x1","x3","x5","x7","x2","x6","x4";
@ -217,19 +217,19 @@ TEST( BayesTree, balanced_smoother_shortcuts )
// Check the conditional P(Root|Root) // Check the conditional P(Root|Root)
GaussianBayesNet empty; GaussianBayesNet empty;
Gaussian::sharedClique R = bayesTree.root(); 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(assert_equal(empty,actual1,1e-4));
// Check the conditional P(C2|Root) // Check the conditional P(C2|Root)
Gaussian::sharedClique C2 = bayesTree["x3"]; 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(assert_equal(empty,actual2,1e-4));
// Check the conditional P(C3|Root), which should be equal to P(x2|x4) // Check the conditional P(C3|Root), which should be equal to P(x2|x4)
ConditionalGaussian::shared_ptr p_x2_x4 = chordalBayesNet["x2"]; ConditionalGaussian::shared_ptr p_x2_x4 = chordalBayesNet["x2"];
GaussianBayesNet expected3; expected3.push_back(p_x2_x4); GaussianBayesNet expected3; expected3.push_back(p_x2_x4);
Gaussian::sharedClique C3 = bayesTree["x1"]; Gaussian::sharedClique C3 = bayesTree["x1"];
GaussianBayesNet actual3 = C3->shortcut<LinearFactor>(R); GaussianBayesNet actual3 = C3->shortcut<GaussianFactor>(R);
CHECK(assert_equal(expected3,actual3,1e-4)); CHECK(assert_equal(expected3,actual3,1e-4));
} }
@ -237,7 +237,7 @@ TEST( BayesTree, balanced_smoother_shortcuts )
TEST( BayesTree, balanced_smoother_clique_marginals ) TEST( BayesTree, balanced_smoother_clique_marginals )
{ {
// Create smoother with 7 nodes // Create smoother with 7 nodes
LinearFactorGraph smoother = createSmoother(7); GaussianFactorGraph smoother = createSmoother(7);
Ordering ordering; Ordering ordering;
ordering += "x1","x3","x5","x7","x2","x6","x4"; ordering += "x1","x3","x5","x7","x2","x6","x4";
@ -251,8 +251,8 @@ TEST( BayesTree, balanced_smoother_clique_marginals )
Matrix A12 = (-0.5)*eye(2); Matrix A12 = (-0.5)*eye(2);
push_front(expected,"x1", zero(2), eye(2), "x2", A12, sigma); push_front(expected,"x1", zero(2), eye(2), "x2", A12, sigma);
Gaussian::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"]; Gaussian::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"];
FactorGraph<LinearFactor> marginal = C3->marginal<LinearFactor>(R); FactorGraph<GaussianFactor> marginal = C3->marginal<GaussianFactor>(R);
GaussianBayesNet actual = eliminate<LinearFactor,ConditionalGaussian>(marginal,C3->keys()); GaussianBayesNet actual = eliminate<GaussianFactor,ConditionalGaussian>(marginal,C3->keys());
CHECK(assert_equal(expected,actual,1e-4)); CHECK(assert_equal(expected,actual,1e-4));
} }
@ -260,7 +260,7 @@ TEST( BayesTree, balanced_smoother_clique_marginals )
TEST( BayesTree, balanced_smoother_joint ) TEST( BayesTree, balanced_smoother_joint )
{ {
// Create smoother with 7 nodes // Create smoother with 7 nodes
LinearFactorGraph smoother = createSmoother(7); GaussianFactorGraph smoother = createSmoother(7);
Ordering ordering; Ordering ordering;
ordering += "x1","x3","x5","x7","x2","x6","x4"; 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) // Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
GaussianBayesNet expected1 = simpleGaussian("x7", zero(2), sigmax7); GaussianBayesNet expected1 = simpleGaussian("x7", zero(2), sigmax7);
push_front(expected1,"x1", zero(2), eye(2), "x7", A, sigma); 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(assert_equal(expected1,actual1,1e-4));
// Check the joint density P(x7,x1) factored as P(x7|x1)P(x1) // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
GaussianBayesNet expected2 = simpleGaussian("x1", zero(2), sigmax1); GaussianBayesNet expected2 = simpleGaussian("x1", zero(2), sigmax1);
push_front(expected2,"x7", zero(2), eye(2), "x1", A, sigma); 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(assert_equal(expected2,actual2,1e-4));
// Check the joint density P(x1,x4), i.e. with a root variable // 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); Vector sigma14 = repeat(2, 0.784465);
Matrix A14 = (-0.0769231)*eye(2); Matrix A14 = (-0.0769231)*eye(2);
push_front(expected3,"x1", zero(2), eye(2), "x4", A14, sigma14); 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(assert_equal(expected3,actual3,1e-4));
// Check the joint density P(x4,x1), i.e. with a root variable, factored the other way // 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); Vector sigma41 = repeat(2, 0.668096);
Matrix A41 = (-0.055794)*eye(2); Matrix A41 = (-0.055794)*eye(2);
push_front(expected4,"x4", zero(2), eye(2), "x1", A41, sigma41); 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)); 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 * @brief Unit tests for Linear Factor
* @author Christian Potthast * @author Christian Potthast
* @author Frank Dellaert * @author Frank Dellaert
@ -24,7 +24,7 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactor, linearFactor ) TEST( GaussianFactor, linearFactor )
{ {
double sigma = 0.1; double sigma = 0.1;
@ -39,24 +39,24 @@ TEST( LinearFactor, linearFactor )
Vector b(2); Vector b(2);
b(0) = 0.2 ; b(1) = -0.1; 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 // create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// get the factor "f2" from the factor graph // 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 if the two factors are the same
CHECK(assert_equal(expected,*lf)); CHECK(assert_equal(expected,*lf));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactor, keys ) TEST( GaussianFactor, keys )
{ {
// get the factor "f2" from the small linear factor graph // get the factor "f2" from the small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
LinearFactor::shared_ptr lf = fg[1]; GaussianFactor::shared_ptr lf = fg[1];
list<string> expected; list<string> expected;
expected.push_back("x1"); expected.push_back("x1");
expected.push_back("x2"); 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 // get the factor "f2" from the small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// Check a single factor // Check a single factor
Dimensions expected; Dimensions expected;
@ -77,11 +77,11 @@ TEST( LinearFactor, dimensions )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactor, getDim ) TEST( GaussianFactor, getDim )
{ {
// get a factor // get a factor
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
LinearFactor::shared_ptr factor = fg[0]; GaussianFactor::shared_ptr factor = fg[0];
// get the size of a variable // get the size of a variable
size_t actual = factor->getDim("x1"); 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 // create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// get two factors from it and insert the factors into a vector // 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[4 - 1]);
lfg.push_back(fg[2 - 1]); lfg.push_back(fg[2 - 1]);
// combine in a factor // combine in a factor
LinearFactor combined(lfg); GaussianFactor combined(lfg);
// sigmas // sigmas
double sigma2 = 0.1; double sigma2 = 0.1;
@ -142,7 +142,7 @@ TEST( LinearFactor, combine )
meas.push_back(make_pair("x2", Ax2)); meas.push_back(make_pair("x2", Ax2));
meas.push_back(make_pair("l1", Al1)); meas.push_back(make_pair("l1", Al1));
meas.push_back(make_pair("x1", Ax1)); meas.push_back(make_pair("x1", Ax1));
LinearFactor expected(meas, b2, sigmas); GaussianFactor expected(meas, b2, sigmas);
CHECK(assert_equal(expected,combined)); CHECK(assert_equal(expected,combined));
} }
@ -154,33 +154,33 @@ TEST( NonlinearFactorGraph, combine2){
A11(1,0) = 0; A11(1,1) = 1; A11(1,0) = 0; A11(1,1) = 1;
Vector b(2); Vector b(2);
b(0) = 2; b(1) = -1; 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; double sigma2 = 0.5;
A11(0,0) = 1; A11(0,1) = 0; A11(0,0) = 1; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = -1; A11(1,0) = 0; A11(1,1) = -1;
b(0) = 4 ; b(1) = -5; 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; double sigma3 = 0.25;
A11(0,0) = 1; A11(0,1) = 0; A11(0,0) = 1; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = -1; A11(1,0) = 0; A11(1,1) = -1;
b(0) = 3 ; b(1) = -88; 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 // TODO: find a real sigma value for this example
double sigma4 = 0.1; double sigma4 = 0.1;
A11(0,0) = 6; A11(0,1) = 0; A11(0,0) = 6; A11(0,1) = 0;
A11(1,0) = 0; A11(1,1) = 7; A11(1,0) = 0; A11(1,1) = 7;
b(0) = 5 ; b(1) = -6; 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(f1);
lfg.push_back(f2); lfg.push_back(f2);
lfg.push_back(f3); lfg.push_back(f3);
lfg.push_back(f4); lfg.push_back(f4);
LinearFactor combined(lfg); GaussianFactor combined(lfg);
Vector sigmas = Vector_(8, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3, sigma4, sigma4); Vector sigmas = Vector_(8, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3, sigma4, sigma4);
Matrix A22(8,2); Matrix A22(8,2);
@ -198,19 +198,19 @@ TEST( NonlinearFactorGraph, combine2){
vector<pair<string, Matrix> > meas; vector<pair<string, Matrix> > meas;
meas.push_back(make_pair("x1", A22)); meas.push_back(make_pair("x1", A22));
LinearFactor expected(meas, exb, sigmas); GaussianFactor expected(meas, exb, sigmas);
CHECK(assert_equal(expected,combined)); CHECK(assert_equal(expected,combined));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactor, linearFactorN){ TEST( GaussianFactor, linearFactorN){
vector<LinearFactor::shared_ptr> f; vector<GaussianFactor::shared_ptr> f;
f.push_back(LinearFactor::shared_ptr(new LinearFactor("x1", Matrix_(2,2, f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", Matrix_(2,2,
1.0, 0.0, 1.0, 0.0,
0.0, 1.0), 0.0, 1.0),
Vector_(2, Vector_(2,
10.0, 5.0), 1))); 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, -10.0, 0.0,
0.0, -10.0), 0.0, -10.0),
"x2", Matrix_(2,2, "x2", Matrix_(2,2,
@ -218,7 +218,7 @@ TEST( LinearFactor, linearFactorN){
0.0, 10.0), 0.0, 10.0),
Vector_(2, Vector_(2,
1.0, -2.0), 1))); 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, -10.0, 0.0,
0.0, -10.0), 0.0, -10.0),
"x3", Matrix_(2,2, "x3", Matrix_(2,2,
@ -226,7 +226,7 @@ TEST( LinearFactor, linearFactorN){
0.0, 10.0), 0.0, 10.0),
Vector_(2, Vector_(2,
1.5, -1.5), 1))); 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, -10.0, 0.0,
0.0, -10.0), 0.0, -10.0),
"x4", Matrix_(2,2, "x4", Matrix_(2,2,
@ -235,7 +235,7 @@ TEST( LinearFactor, linearFactorN){
Vector_(2, Vector_(2,
2.0, -1.0), 1))); 2.0, -1.0), 1)));
LinearFactor combinedFactor(f); GaussianFactor combinedFactor(f);
vector<pair<string, Matrix> > combinedMeasurement; vector<pair<string, Matrix> > combinedMeasurement;
combinedMeasurement.push_back(make_pair("x1", Matrix_(8,2, combinedMeasurement.push_back(make_pair("x1", Matrix_(8,2,
@ -277,18 +277,18 @@ TEST( LinearFactor, linearFactorN){
Vector b = Vector_(8, Vector b = Vector_(8,
10.0, 5.0, 1.0, -2.0, 1.5, -1.5, 2.0, -1.0); 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)); CHECK(combinedFactor.equals(expected));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactor, error ) TEST( GaussianFactor, error )
{ {
// create a small linear factor graph // create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// get the first factor from the factor graph // 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 // check the error of the first factor with noisy config
VectorConfig cfg = createZeroDelta(); VectorConfig cfg = createZeroDelta();
@ -300,22 +300,22 @@ TEST( LinearFactor, error )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactor, eliminate ) TEST( GaussianFactor, eliminate )
{ {
// create a small linear factor graph // create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// get two factors from it and insert the factors into a vector // 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[4 - 1]);
lfg.push_back(fg[2 - 1]); lfg.push_back(fg[2 - 1]);
// combine in a factor // combine in a factor
LinearFactor combined(lfg); GaussianFactor combined(lfg);
// eliminate the combined factor // eliminate the combined factor
ConditionalGaussian::shared_ptr actualCG; ConditionalGaussian::shared_ptr actualCG;
LinearFactor::shared_ptr actualLF; GaussianFactor::shared_ptr actualLF;
boost::tie(actualCG,actualLF) = combined.eliminate("x2"); boost::tie(actualCG,actualLF) = combined.eliminate("x2");
// create expected Conditional Gaussian // create expected Conditional Gaussian
@ -357,7 +357,7 @@ TEST( LinearFactor, eliminate )
// the RHS // the RHS
Vector b1(2); b1(0) = 0.0; b1(1) = 0.2; 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 if the result matches
CHECK(assert_equal(expectedCG,*actualCG,1e-4)); CHECK(assert_equal(expectedCG,*actualCG,1e-4));
@ -366,7 +366,7 @@ TEST( LinearFactor, eliminate )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactor, eliminate2 ) TEST( GaussianFactor, eliminate2 )
{ {
// sigmas // sigmas
double sigma1 = 0.2; double sigma1 = 0.2;
@ -400,11 +400,11 @@ TEST( LinearFactor, eliminate2 )
vector<pair<string, Matrix> > meas; vector<pair<string, Matrix> > meas;
meas.push_back(make_pair("x2", Ax2)); meas.push_back(make_pair("x2", Ax2));
meas.push_back(make_pair("l1x1", Al1x1)); meas.push_back(make_pair("l1x1", Al1x1));
LinearFactor combined(meas, b2, sigmas); GaussianFactor combined(meas, b2, sigmas);
// eliminate the combined factor // eliminate the combined factor
ConditionalGaussian::shared_ptr actualCG; ConditionalGaussian::shared_ptr actualCG;
LinearFactor::shared_ptr actualLF; GaussianFactor::shared_ptr actualLF;
boost::tie(actualCG,actualLF) = combined.eliminate("x2"); boost::tie(actualCG,actualLF) = combined.eliminate("x2");
// create expected Conditional Gaussian // create expected Conditional Gaussian
@ -435,7 +435,7 @@ TEST( LinearFactor, eliminate2 )
// the RHS // the RHS
Vector b1(2); b1(0) = 0.0; b1(1) = 0.894427; 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 if the result matches
CHECK(assert_equal(expectedCG,*actualCG,1e-4)); 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; VectorConfig c;
double actual = f.error(c); double actual = f.error(c);
CHECK(actual==0.0); CHECK(actual==0.0);
} }
//* ************************************************************************* */ //* ************************************************************************* */
TEST( LinearFactor, eliminate_empty ) TEST( GaussianFactor, eliminate_empty )
{ {
// create an empty factor // create an empty factor
LinearFactor f; GaussianFactor f;
// eliminate the empty factor // eliminate the empty factor
ConditionalGaussian::shared_ptr actualCG; ConditionalGaussian::shared_ptr actualCG;
LinearFactor::shared_ptr actualLF; GaussianFactor::shared_ptr actualLF;
boost::tie(actualCG,actualLF) = f.eliminate("x2"); boost::tie(actualCG,actualLF) = f.eliminate("x2");
// expected Conditional Gaussian is just a parent-less node with P(x)=1 // expected Conditional Gaussian is just a parent-less node with P(x)=1
ConditionalGaussian expectedCG("x2"); ConditionalGaussian expectedCG("x2");
// expected remaining factor is still empty :-) // expected remaining factor is still empty :-)
LinearFactor expectedLF; GaussianFactor expectedLF;
// check if the result matches // check if the result matches
CHECK(actualCG->equals(expectedCG)); CHECK(actualCG->equals(expectedCG));
@ -474,21 +474,21 @@ TEST( LinearFactor, eliminate_empty )
} }
//* ************************************************************************* */ //* ************************************************************************* */
TEST( LinearFactor, empty ) TEST( GaussianFactor, empty )
{ {
// create an empty factor // create an empty factor
LinearFactor f; GaussianFactor f;
CHECK(f.empty()==true); CHECK(f.empty()==true);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactor, matrix ) TEST( GaussianFactor, matrix )
{ {
// create a small linear factor graph // create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// get the factor "f2" from the factor graph // 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 // render with a given ordering
Ordering ord; Ordering ord;
@ -507,13 +507,13 @@ TEST( LinearFactor, matrix )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactor, matrix_aug ) TEST( GaussianFactor, matrix_aug )
{ {
// create a small linear factor graph // create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// get the factor "f2" from the factor graph // 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 // render with a given ordering
Ordering ord; Ordering ord;
@ -538,13 +538,13 @@ void print(const list<T>& i) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactor, sparse ) TEST( GaussianFactor, sparse )
{ {
// create a small linear factor graph // create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// get the factor "f2" from the factor graph // 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 // render with a given ordering
Ordering ord; Ordering ord;
@ -567,13 +567,13 @@ TEST( LinearFactor, sparse )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactor, sparse2 ) TEST( GaussianFactor, sparse2 )
{ {
// create a small linear factor graph // create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// get the factor "f2" from the factor graph // 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 // render with a given ordering
Ordering ord; Ordering ord;
@ -596,15 +596,15 @@ TEST( LinearFactor, sparse2 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactor, size ) TEST( GaussianFactor, size )
{ {
// create a linear factor graph // create a linear factor graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// get some factors from the graph // get some factors from the graph
boost::shared_ptr<LinearFactor> factor1 = fg[0]; boost::shared_ptr<GaussianFactor> factor1 = fg[0];
boost::shared_ptr<LinearFactor> factor2 = fg[1]; boost::shared_ptr<GaussianFactor> factor2 = fg[1];
boost::shared_ptr<LinearFactor> factor3 = fg[2]; boost::shared_ptr<GaussianFactor> factor3 = fg[2];
CHECK(factor1->size() == 1); CHECK(factor1->size() == 1);
CHECK(factor2->size() == 2); CHECK(factor2->size() == 2);
@ -612,7 +612,7 @@ TEST( LinearFactor, size )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian ) TEST( GaussianFactor, CONSTRUCTOR_ConditionalGaussian )
{ {
Matrix R11 = Matrix_(2,2, Matrix R11 = Matrix_(2,2,
1.00, 0.00, 1.00, 0.00,
@ -629,24 +629,24 @@ TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian )
sigmas(1) = 0.29907; sigmas(1) = 0.29907;
ConditionalGaussian::shared_ptr CG(new ConditionalGaussian("x2",d,R11,"l1x1",S12,sigmas)); ConditionalGaussian::shared_ptr CG(new ConditionalGaussian("x2",d,R11,"l1x1",S12,sigmas));
LinearFactor actualLF(CG); GaussianFactor actualLF(CG);
// actualLF.print(); // 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)); CHECK(assert_equal(expectedLF,actualLF,1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST ( LinearFactor, constraint_eliminate1 ) TEST ( GaussianFactor, constraint_eliminate1 )
{ {
// construct a linear constraint // construct a linear constraint
Vector v(2); v(0)=1.2; v(1)=3.4; Vector v(2); v(0)=1.2; v(1)=3.4;
string key = "x0"; string key = "x0";
LinearFactor lc(key, eye(2), v, 0.0); GaussianFactor lc(key, eye(2), v, 0.0);
// eliminate it // eliminate it
ConditionalGaussian::shared_ptr actualCG; ConditionalGaussian::shared_ptr actualCG;
LinearFactor::shared_ptr actualLF; GaussianFactor::shared_ptr actualLF;
boost::tie(actualCG,actualLF) = lc.eliminate("x0"); boost::tie(actualCG,actualLF) = lc.eliminate("x0");
// verify linear factor // verify linear factor
@ -659,7 +659,7 @@ TEST ( LinearFactor, constraint_eliminate1 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST ( LinearFactor, constraint_eliminate2 ) TEST ( GaussianFactor, constraint_eliminate2 )
{ {
// Construct a linear constraint // Construct a linear constraint
// RHS // RHS
@ -675,15 +675,15 @@ TEST ( LinearFactor, constraint_eliminate2 )
A2(0,0) = 1.0 ; A2(0,1) = 2.0; A2(0,0) = 1.0 ; A2(0,1) = 2.0;
A2(1,0) = 2.0 ; A2(1,1) = 4.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 // eliminate x and verify results
ConditionalGaussian::shared_ptr actualCG; ConditionalGaussian::shared_ptr actualCG;
LinearFactor::shared_ptr actualLF; GaussianFactor::shared_ptr actualLF;
boost::tie(actualCG, actualLF) = lc.eliminate("x"); boost::tie(actualCG, actualLF) = lc.eliminate("x");
// LF should be null // LF should be null
LinearFactor expectedLF; GaussianFactor expectedLF;
CHECK(assert_equal(*actualLF, expectedLF)); CHECK(assert_equal(*actualLF, expectedLF));
// verify CG // 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 // This test shows that ordering matters if there are non-invertable
// blocks, as this example can be eliminated if x is first, but not // 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(0,0) = 1.0 ; A2(0,1) = 2.0;
A2(1,0) = 2.0 ; A2(1,1) = 4.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 // eliminate y from original graph
// NOTE: this will throw an exception, as // NOTE: this will throw an exception, as
// the leading matrix is rank deficient // the leading matrix is rank deficient
ConditionalGaussian::shared_ptr actualCG; ConditionalGaussian::shared_ptr actualCG;
LinearFactor::shared_ptr actualLF; GaussianFactor::shared_ptr actualLF;
try { try {
boost::tie(actualCG, actualLF) = lc.eliminate("y"); boost::tie(actualCG, actualLF) = lc.eliminate("y");
CHECK(false); 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 * @brief Unit tests for Linear Factor Graph
* @author Christian Potthast * @author Christian Potthast
**/ **/
@ -26,19 +26,19 @@ using namespace gtsam;
double tol=1e-4; 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(); GaussianFactorGraph fg = createGaussianFactorGraph();
LinearFactorGraph fg2 = createLinearFactorGraph(); GaussianFactorGraph fg2 = createGaussianFactorGraph();
CHECK(fg.equals(fg2)); CHECK(fg.equals(fg2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactorGraph, error ) TEST( GaussianFactorGraph, error )
{ {
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
VectorConfig cfg = createZeroDelta(); VectorConfig cfg = createZeroDelta();
// note the error is the same as in testNonlinearFactorGraph as a // note the error is the same as in testNonlinearFactorGraph as a
@ -51,9 +51,9 @@ TEST( LinearFactorGraph, error )
/* ************************************************************************* */ /* ************************************************************************* */
/* unit test for find seperator */ /* 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> separator = fg.find_separator("x2");
set<string> expected; 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 // create a small example for a linear factor graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// create sigmas // create sigmas
double sigma1 = 0.1; double sigma1 = 0.1;
@ -79,7 +79,7 @@ TEST( LinearFactorGraph, combine_factors_x1 )
Vector sigmas = Vector_(6, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3); Vector sigmas = Vector_(6, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3);
// combine all factors // combine all factors
LinearFactor::shared_ptr actual = removeAndCombineFactors(fg,"x1"); GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,"x1");
// the expected linear factor // the expected linear factor
Matrix Al1 = Matrix_(6,2, 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("l1", Al1));
meas.push_back(make_pair("x1", Ax1)); meas.push_back(make_pair("x1", Ax1));
meas.push_back(make_pair("x2", Ax2)); meas.push_back(make_pair("x2", Ax2));
LinearFactor expected(meas, b, sigmas); GaussianFactor expected(meas, b, sigmas);
//LinearFactor expected("l1", Al1, "x1", Ax1, "x2", Ax2, b); //GaussianFactor expected("l1", Al1, "x1", Ax1, "x2", Ax2, b);
// check if the two factors are the same // check if the two factors are the same
CHECK(assert_equal(expected,*actual)); CHECK(assert_equal(expected,*actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactorGraph, combine_factors_x2 ) TEST( GaussianFactorGraph, combine_factors_x2 )
{ {
// create a small example for a linear factor graph // create a small example for a linear factor graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// determine sigmas // determine sigmas
double sigma1 = 0.1; double sigma1 = 0.1;
@ -141,7 +141,7 @@ TEST( LinearFactorGraph, combine_factors_x2 )
Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2); Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2);
// combine all factors // combine all factors
LinearFactor::shared_ptr actual = removeAndCombineFactors(fg,"x2"); GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,"x2");
// the expected linear factor // the expected linear factor
Matrix Al1 = Matrix_(4,2, 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("l1", Al1));
meas.push_back(make_pair("x1", Ax1)); meas.push_back(make_pair("x1", Ax1));
meas.push_back(make_pair("x2", Ax2)); 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 if the two factors are the same
CHECK(assert_equal(expected,*actual)); 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"); ConditionalGaussian::shared_ptr actual = fg.eliminateOne("x1");
// create expected Conditional Gaussian // 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"); ConditionalGaussian::shared_ptr actual = fg.eliminateOne("x2");
// create expected Conditional Gaussian // 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"); ConditionalGaussian::shared_ptr actual = fg.eliminateOne("l1");
// create expected Conditional Gaussian // create expected Conditional Gaussian
@ -230,7 +230,7 @@ TEST( LinearFactorGraph, eliminateOne_l1 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactorGraph, eliminateAll ) TEST( GaussianFactorGraph, eliminateAll )
{ {
// create expected Chordal bayes Net // create expected Chordal bayes Net
Matrix I = eye(2); 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); push_front(expected,"x2",d3, I,"l1", (-0.2)*I, "x1", (-0.8)*I, sigma3);
// Check one ordering // Check one ordering
LinearFactorGraph fg1 = createLinearFactorGraph(); GaussianFactorGraph fg1 = createGaussianFactorGraph();
Ordering ordering; Ordering ordering;
ordering += "x2","l1","x1"; ordering += "x2","l1","x1";
GaussianBayesNet actual = fg1.eliminate(ordering); GaussianBayesNet actual = fg1.eliminate(ordering);
@ -253,28 +253,28 @@ TEST( LinearFactorGraph, eliminateAll )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactorGraph, add_priors ) TEST( GaussianFactorGraph, add_priors )
{ {
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
LinearFactorGraph actual = fg.add_priors(3); GaussianFactorGraph actual = fg.add_priors(3);
LinearFactorGraph expected = createLinearFactorGraph(); GaussianFactorGraph expected = createGaussianFactorGraph();
Matrix A = eye(2); Matrix A = eye(2);
Vector b = zero(2); Vector b = zero(2);
double sigma = 3.0; double sigma = 3.0;
expected.push_back(LinearFactor::shared_ptr(new LinearFactor("l1",A,b,sigma))); expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("l1",A,b,sigma)));
expected.push_back(LinearFactor::shared_ptr(new LinearFactor("x1",A,b,sigma))); expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1",A,b,sigma)));
expected.push_back(LinearFactor::shared_ptr(new LinearFactor("x2",A,b,sigma))); expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2",A,b,sigma)));
CHECK(assert_equal(expected,actual)); // Fails CHECK(assert_equal(expected,actual)); // Fails
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactorGraph, copying ) TEST( GaussianFactorGraph, copying )
{ {
// Create a graph // Create a graph
LinearFactorGraph actual = createLinearFactorGraph(); GaussianFactorGraph actual = createGaussianFactorGraph();
// Copy the graph ! // Copy the graph !
LinearFactorGraph copy = actual; GaussianFactorGraph copy = actual;
// now eliminate the copy // now eliminate the copy
Ordering ord1; Ordering ord1;
@ -282,17 +282,17 @@ TEST( LinearFactorGraph, copying )
GaussianBayesNet actual1 = copy.eliminate(ord1); GaussianBayesNet actual1 = copy.eliminate(ord1);
// Create the same graph, but not by copying // Create the same graph, but not by copying
LinearFactorGraph expected = createLinearFactorGraph(); GaussianFactorGraph expected = createGaussianFactorGraph();
// and check that original is still the same graph // and check that original is still the same graph
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactorGraph, matrix ) TEST( GaussianFactorGraph, matrix )
{ {
// Create a graph // Create a graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// render with a given ordering // render with a given ordering
Ordering ord; Ordering ord;
@ -318,10 +318,10 @@ TEST( LinearFactorGraph, matrix )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactorGraph, sparse ) TEST( GaussianFactorGraph, sparse )
{ {
// create a small linear factor graph // create a small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// render with a given ordering // render with a given ordering
Ordering ord; 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 // render with a given ordering
Ordering ord; Ordering ord;
ord += "x2","l1","x1"; ord += "x2","l1","x1";
GaussianBayesNet CBN = fg.eliminate(ord); GaussianBayesNet CBN = fg.eliminate(ord);
// True LinearFactorGraph // True GaussianFactorGraph
LinearFactorGraph fg2(CBN); GaussianFactorGraph fg2(CBN);
GaussianBayesNet CBN2 = fg2.eliminate(ord); GaussianBayesNet CBN2 = fg2.eliminate(ord);
CHECK(assert_equal(CBN,CBN2)); CHECK(assert_equal(CBN,CBN2));
// Base FactorGraph only // Base FactorGraph only
FactorGraph<LinearFactor> fg3(CBN); FactorGraph<GaussianFactor> fg3(CBN);
GaussianBayesNet CBN3 = gtsam::eliminate<LinearFactor,ConditionalGaussian>(fg3,ord); GaussianBayesNet CBN3 = gtsam::eliminate<GaussianFactor,ConditionalGaussian>(fg3,ord);
CHECK(assert_equal(CBN,CBN3)); CHECK(assert_equal(CBN,CBN3));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactorGraph, GET_ORDERING) TEST( GaussianFactorGraph, GET_ORDERING)
{ {
Ordering expected; Ordering expected;
expected += "l1","x1","x2"; expected += "l1","x1","x2";
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
Ordering actual = fg.getOrdering(); Ordering actual = fg.getOrdering();
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactorGraph, OPTIMIZE ) TEST( GaussianFactorGraph, OPTIMIZE )
{ {
// create a graph // create a graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// create an ordering // create an ordering
Ordering ord = fg.getOrdering(); 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 // create a test graph
LinearFactorGraph fg1 = createLinearFactorGraph(); GaussianFactorGraph fg1 = createGaussianFactorGraph();
// create another factor graph // create another factor graph
LinearFactorGraph fg2 = createLinearFactorGraph(); GaussianFactorGraph fg2 = createGaussianFactorGraph();
// get sizes // get sizes
int size1 = fg1.size(); 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 // create a test graph
LinearFactorGraph fg1 = createLinearFactorGraph(); GaussianFactorGraph fg1 = createGaussianFactorGraph();
// create another factor graph // create another factor graph
LinearFactorGraph fg2 = createLinearFactorGraph(); GaussianFactorGraph fg2 = createGaussianFactorGraph();
// get sizes // get sizes
int size1 = fg1.size(); int size1 = fg1.size();
int size2 = fg2.size(); int size2 = fg2.size();
// combine them // combine them
LinearFactorGraph fg3 = LinearFactorGraph::combine2(fg1, fg2); GaussianFactorGraph fg3 = GaussianFactorGraph::combine2(fg1, fg2);
CHECK(size1+size2 == fg3.size()); 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 // create a test graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// ask for all factor indices connected to x1 // ask for all factor indices connected to x1
list<int> x1_factors = fg.factors("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 // create the graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// We expect to remove these three factors: 0, 1, 2 // We expect to remove these three factors: 0, 1, 2
LinearFactor::shared_ptr f0 = fg[0]; GaussianFactor::shared_ptr f0 = fg[0];
LinearFactor::shared_ptr f1 = fg[1]; GaussianFactor::shared_ptr f1 = fg[1];
LinearFactor::shared_ptr f2 = fg[2]; GaussianFactor::shared_ptr f2 = fg[2];
// call the function // call the function
vector<LinearFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1"); vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1");
// Check the factors // Check the factors
CHECK(f0==factors[0]); CHECK(f0==factors[0]);
@ -474,18 +474,18 @@ TEST( LinearFactorGraph, findAndRemoveFactors )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactorGraph, findAndRemoveFactors_twice ) TEST( GaussianFactorGraph, findAndRemoveFactors_twice )
{ {
// create the graph // create the graph
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
// We expect to remove these three factors: 0, 1, 2 // We expect to remove these three factors: 0, 1, 2
LinearFactor::shared_ptr f0 = fg[0]; GaussianFactor::shared_ptr f0 = fg[0];
LinearFactor::shared_ptr f1 = fg[1]; GaussianFactor::shared_ptr f1 = fg[1];
LinearFactor::shared_ptr f2 = fg[2]; GaussianFactor::shared_ptr f2 = fg[2];
// call the function // call the function
vector<LinearFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1"); vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors("x1");
// Check the factors // Check the factors
CHECK(f0==factors[0]); 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()); LONGS_EQUAL(3,fg1.size());
LinearFactorGraph fg2 = createSmoother(3); GaussianFactorGraph fg2 = createSmoother(3);
LONGS_EQUAL(5,fg2.size()); LONGS_EQUAL(5,fg2.size());
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactorGraph, variables ) TEST( GaussianFactorGraph, variables )
{ {
LinearFactorGraph fg = createLinearFactorGraph(); GaussianFactorGraph fg = createGaussianFactorGraph();
Dimensions expected; Dimensions expected;
insert(expected)("l1", 2)("x1", 2)("x2", 2); insert(expected)("l1", 2)("x1", 2)("x2", 2);
Dimensions actual = fg.dimensions(); 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; Ordering expected;
expected += "l1","x1","x2"; expected += "l1","x1","x2";
CHECK(assert_equal(expected,fg.keys())); 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 // get a graph with a constraint in it
LinearFactorGraph fg = createSimpleConstraintGraph(); GaussianFactorGraph fg = createSimpleConstraintGraph();
// eliminate and solve // eliminate and solve
Ordering ord; 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 // get a graph with a constraint in it
LinearFactorGraph fg = createSingleConstraintGraph(); GaussianFactorGraph fg = createSingleConstraintGraph();
// eliminate and solve // eliminate and solve
Ordering ord; 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 // get a graph with a constraint in it
LinearFactorGraph fg = createSingleConstraintGraph(); GaussianFactorGraph fg = createSingleConstraintGraph();
// eliminate and solve // eliminate and solve
Ordering ord; 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 // get a graph with a constraint in it
LinearFactorGraph fg = createMultiConstraintGraph(); GaussianFactorGraph fg = createMultiConstraintGraph();
// eliminate and solve // eliminate and solve
Ordering ord; 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 // get a graph with a constraint in it
LinearFactorGraph fg = createMultiConstraintGraph(); GaussianFactorGraph fg = createMultiConstraintGraph();
// eliminate and solve // eliminate and solve
Ordering ord; 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 // 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()); LONGS_EQUAL(5,fg2.size());
// eliminate // eliminate
Ordering ordering; Ordering ordering;
GaussianBayesNet bayesNet = fg2.eliminate(ordering); GaussianBayesNet bayesNet = fg2.eliminate(ordering);
bayesNet.print("bayesNet"); bayesNet.print("bayesNet");
FactorGraph<LinearFactor> p_x3 = marginalize<LinearFactor,ConditionalGaussian>(bayesNet, Ordering("x3")); FactorGraph<GaussianFactor> p_x3 = marginalize<GaussianFactor,ConditionalGaussian>(bayesNet, Ordering("x3"));
FactorGraph<LinearFactor> p_x1 = marginalize<LinearFactor,ConditionalGaussian>(bayesNet, Ordering("x1")); FactorGraph<GaussianFactor> p_x1 = marginalize<GaussianFactor,ConditionalGaussian>(bayesNet, Ordering("x1"));
CHECK(assert_equal(p_x1,p_x3)); // should be the same because of symmetry 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" // create and marginalize a small Bayes net on "x"
GaussianBayesNet cbn = createSmallGaussianBayesNet(); GaussianBayesNet cbn = createSmallGaussianBayesNet();
Ordering keys("x"); 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 // 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 // expected is just scalar Gaussian on x
GaussianBayesNet expected = scalarGaussian("x",4,sqrt(2)); 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; typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared_nlf;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonLinearFactor, equals ) TEST( NonGaussianFactor, equals )
{ {
double sigma = 1.0; double sigma = 1.0;
@ -40,7 +40,7 @@ TEST( NonLinearFactor, equals )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonLinearFactor, equals2 ) TEST( NonGaussianFactor, equals2 )
{ {
// create a non linear factor graph // create a non linear factor graph
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
@ -54,7 +54,7 @@ TEST( NonLinearFactor, equals2 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonLinearFactor, NonlinearFactor ) TEST( NonGaussianFactor, NonlinearFactor )
{ {
// create a non linear factor graph // create a non linear factor graph
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
@ -81,7 +81,7 @@ TEST( NonLinearFactor, NonlinearFactor )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonLinearFactor, linearize_f1 ) TEST( NonGaussianFactor, linearize_f1 )
{ {
// Grab a non-linear factor // Grab a non-linear factor
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
@ -90,16 +90,16 @@ TEST( NonLinearFactor, linearize_f1 )
// We linearize at noisy config from SmallExample // We linearize at noisy config from SmallExample
VectorConfig c = createNoisyConfig(); VectorConfig c = createNoisyConfig();
LinearFactor::shared_ptr actual = nlf->linearize(c); GaussianFactor::shared_ptr actual = nlf->linearize(c);
LinearFactorGraph lfg = createLinearFactorGraph(); GaussianFactorGraph lfg = createGaussianFactorGraph();
LinearFactor::shared_ptr expected = lfg[0]; GaussianFactor::shared_ptr expected = lfg[0];
CHECK(expected->equals(*actual)); CHECK(expected->equals(*actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonLinearFactor, linearize_f2 ) TEST( NonGaussianFactor, linearize_f2 )
{ {
// Grab a non-linear factor // Grab a non-linear factor
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
@ -108,16 +108,16 @@ TEST( NonLinearFactor, linearize_f2 )
// We linearize at noisy config from SmallExample // We linearize at noisy config from SmallExample
VectorConfig c = createNoisyConfig(); VectorConfig c = createNoisyConfig();
LinearFactor::shared_ptr actual = nlf->linearize(c); GaussianFactor::shared_ptr actual = nlf->linearize(c);
LinearFactorGraph lfg = createLinearFactorGraph(); GaussianFactorGraph lfg = createGaussianFactorGraph();
LinearFactor::shared_ptr expected = lfg[1]; GaussianFactor::shared_ptr expected = lfg[1];
CHECK(expected->equals(*actual)); CHECK(expected->equals(*actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonLinearFactor, linearize_f3 ) TEST( NonGaussianFactor, linearize_f3 )
{ {
// Grab a non-linear factor // Grab a non-linear factor
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
@ -126,16 +126,16 @@ TEST( NonLinearFactor, linearize_f3 )
// We linearize at noisy config from SmallExample // We linearize at noisy config from SmallExample
VectorConfig c = createNoisyConfig(); VectorConfig c = createNoisyConfig();
LinearFactor::shared_ptr actual = nlf->linearize(c); GaussianFactor::shared_ptr actual = nlf->linearize(c);
LinearFactorGraph lfg = createLinearFactorGraph(); GaussianFactorGraph lfg = createGaussianFactorGraph();
LinearFactor::shared_ptr expected = lfg[2]; GaussianFactor::shared_ptr expected = lfg[2];
CHECK(expected->equals(*actual)); CHECK(expected->equals(*actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonLinearFactor, linearize_f4 ) TEST( NonGaussianFactor, linearize_f4 )
{ {
// Grab a non-linear factor // Grab a non-linear factor
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
@ -144,16 +144,16 @@ TEST( NonLinearFactor, linearize_f4 )
// We linearize at noisy config from SmallExample // We linearize at noisy config from SmallExample
VectorConfig c = createNoisyConfig(); VectorConfig c = createNoisyConfig();
LinearFactor::shared_ptr actual = nlf->linearize(c); GaussianFactor::shared_ptr actual = nlf->linearize(c);
LinearFactorGraph lfg = createLinearFactorGraph(); GaussianFactorGraph lfg = createGaussianFactorGraph();
LinearFactor::shared_ptr expected = lfg[3]; GaussianFactor::shared_ptr expected = lfg[3];
CHECK(expected->equals(*actual)); CHECK(expected->equals(*actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonLinearFactor, size ) TEST( NonGaussianFactor, size )
{ {
// create a non linear factor graph // create a non linear factor graph
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();

4
cpp/testNonlinearFactorGraph.cpp
View File

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

2
cpp/testSymbolicBayesNet.cpp
View File

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

18
cpp/testSymbolicFactorGraph.cpp
View File

@ -40,7 +40,7 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph )
expected.push_back(f4); expected.push_back(f4);
// construct it from the factor graph graph // construct it from the factor graph graph
LinearFactorGraph factorGraph = createLinearFactorGraph(); GaussianFactorGraph factorGraph = createGaussianFactorGraph();
SymbolicFactorGraph actual(factorGraph); SymbolicFactorGraph actual(factorGraph);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
@ -50,7 +50,7 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph )
TEST( SymbolicFactorGraph, findAndRemoveFactors ) TEST( SymbolicFactorGraph, findAndRemoveFactors )
{ {
// construct it from the factor graph graph // construct it from the factor graph graph
LinearFactorGraph factorGraph = createLinearFactorGraph(); GaussianFactorGraph factorGraph = createGaussianFactorGraph();
SymbolicFactorGraph actual(factorGraph); SymbolicFactorGraph actual(factorGraph);
SymbolicFactor::shared_ptr f1 = actual[0]; SymbolicFactor::shared_ptr f1 = actual[0];
SymbolicFactor::shared_ptr f3 = actual[2]; SymbolicFactor::shared_ptr f3 = actual[2];
@ -70,7 +70,7 @@ TEST( SymbolicFactorGraph, findAndRemoveFactors )
TEST( SymbolicFactorGraph, factors) TEST( SymbolicFactorGraph, factors)
{ {
// create a test graph // create a test graph
LinearFactorGraph factorGraph = createLinearFactorGraph(); GaussianFactorGraph factorGraph = createGaussianFactorGraph();
SymbolicFactorGraph fg(factorGraph); SymbolicFactorGraph fg(factorGraph);
// ask for all factor indices connected to x1 // 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 // create a test graph
LinearFactorGraph factorGraph = createLinearFactorGraph(); GaussianFactorGraph factorGraph = createGaussianFactorGraph();
SymbolicFactorGraph fg(factorGraph); SymbolicFactorGraph fg(factorGraph);
// combine all factors connected to x1 // combine all factors connected to x1
@ -104,10 +104,10 @@ TEST( LinearFactorGraph, removeAndCombineFactors )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactorGraph, eliminateOne ) TEST( GaussianFactorGraph, eliminateOne )
{ {
// create a test graph // create a test graph
LinearFactorGraph factorGraph = createLinearFactorGraph(); GaussianFactorGraph factorGraph = createGaussianFactorGraph();
SymbolicFactorGraph fg(factorGraph); SymbolicFactorGraph fg(factorGraph);
// eliminate // eliminate
@ -120,7 +120,7 @@ TEST( LinearFactorGraph, eliminateOne )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactorGraph, eliminate ) TEST( GaussianFactorGraph, eliminate )
{ {
// create expected Chordal bayes Net // create expected Chordal bayes Net
SymbolicConditional::shared_ptr x2(new SymbolicConditional("x2", "l1", "x1")); SymbolicConditional::shared_ptr x2(new SymbolicConditional("x2", "l1", "x1"));
@ -133,7 +133,7 @@ TEST( LinearFactorGraph, eliminate )
expected.push_back(x1); expected.push_back(x1);
// create a test graph // create a test graph
LinearFactorGraph factorGraph = createLinearFactorGraph(); GaussianFactorGraph factorGraph = createGaussianFactorGraph();
SymbolicFactorGraph fg(factorGraph); SymbolicFactorGraph fg(factorGraph);
// eliminate it // 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 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.); 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.); Vector b = Vector_(2,3.,0.);
LinearFactor expected("l1", Al1, "x1", Ax1, b, 1); GaussianFactor expected("l1", Al1, "x1", Ax1, b, 1);
LinearFactor::shared_ptr actual = factor->linearize(config); GaussianFactor::shared_ptr actual = factor->linearize(config);
CHECK(assert_equal(expected,*actual,1e-3)); CHECK(assert_equal(expected,*actual,1e-3));
// linearize graph // linearize graph
VSLAMGraph graph; VSLAMGraph graph;
graph.push_back(factor); graph.push_back(factor);
LinearFactorGraph expected_lfg; GaussianFactorGraph expected_lfg;
expected_lfg.push_back(actual); expected_lfg.push_back(actual);
LinearFactorGraph actual_lfg = graph.linearize(config); GaussianFactorGraph actual_lfg = graph.linearize(config);
CHECK(assert_equal(expected_lfg,actual_lfg)); CHECK(assert_equal(expected_lfg,actual_lfg));
// exmap on a config // exmap on a config

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

@ -1,6 +1,6 @@
/** /**
* @file timeLinearFactor.cpp * @file timeGaussianFactor.cpp
* @brief time LinearFactor.eliminate * @brief time GaussianFactor.eliminate
* @author Alireza Fathi * @author Alireza Fathi
*/ */
@ -12,7 +12,7 @@ using namespace std;
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include "Matrix.h" #include "Matrix.h"
#include "LinearFactor.h" #include "GaussianFactor.h"
using namespace gtsam; using namespace gtsam;
@ -66,11 +66,11 @@ int main()
b2(6) = 2; b2(6) = 2;
b2(7) = -1; b2(7) = -1;
LinearFactor combined("x2", Ax2, "l1", Al1, "x1", Ax1, b2); GaussianFactor combined("x2", Ax2, "l1", Al1, "x1", Ax1, b2);
long timeLog = clock(); long timeLog = clock();
int n = 1000000; int n = 1000000;
ConditionalGaussian::shared_ptr conditional; ConditionalGaussian::shared_ptr conditional;
LinearFactor::shared_ptr factor; GaussianFactor::shared_ptr factor;
for(int i = 0; i < n; i++) for(int i = 0; i < n; i++)
boost::tie(conditional,factor) = combined.eliminate("x2"); 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 * @brief Time elimination with simple Kalman Smoothing example
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <time.h> #include <time.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include "SmallExample.h" #include "smallExample.h"
#include "Ordering.h" #include "Ordering.h"
using namespace std; using namespace std;
@ -15,7 +15,7 @@ using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
// Create a Kalman smoother for t=1:T and optimize // Create a Kalman smoother for t=1:T and optimize
double timeKalmanSmoother(int T) { double timeKalmanSmoother(int T) {
LinearFactorGraph smoother = createSmoother(T); GaussianFactorGraph smoother = createSmoother(T);
Ordering ordering; Ordering ordering;
for (int t = 1; t <= T; t++) ordering.push_back(symbol('x',t)); for (int t = 1; t <= T; t++) ordering.push_back(symbol('x',t));
clock_t start = clock(); clock_t start = clock();
@ -26,7 +26,7 @@ double timeKalmanSmoother(int T) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(timeLinearFactorGraph, linearTime) TEST(timeGaussianFactorGraph, linearTime)
{ {
int T = 1000; int T = 1000;
double time1 = timeKalmanSmoother( T); // cout << time1 << endl; double time1 = timeKalmanSmoother( T); // cout << time1 << endl;

12
matlab/createLinearFactorGraph.m
View File

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

4
matlab/run_tests.m
View File

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

8
matlab/testLinearFactor.m
View File

@ -26,12 +26,12 @@ Ax1 = [
% the RHS % the RHS
b2=[-1;1.5;2;-1]; 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 % eliminate the combined factor
% NOT WORKING % NOT WORKING
% this is not working because there is no elimination function for a linear % 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'); %[actualCG,actualLF] = combined.eliminate('x2');
% create expected Conditional Gaussian % create expected Conditional Gaussian
@ -65,10 +65,10 @@ Bx1 = [
% the RHS % the RHS
b1= [0.0;0.894427]; 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 % check if the result matches
% NOT WORKING % 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(~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 %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 % equals
fg = createLinearFactorGraph(); fg = createGaussianFactorGraph();
fg2 = createLinearFactorGraph(); fg2 = createGaussianFactorGraph();
CHECK('equals',fg.equals(fg2)); CHECK('equals',fg.equals(fg2));
%----------------------------------------------------------------------- %-----------------------------------------------------------------------
@ -12,7 +12,7 @@ DOUBLES_EQUAL( 5.625, actual, 1e-9 );
%----------------------------------------------------------------------- %-----------------------------------------------------------------------
% combine_factors_x1 % combine_factors_x1
fg = createLinearFactorGraph(); fg = createGaussianFactorGraph();
actual = fg.combine_factors('x1'); actual = fg.combine_factors('x1');
Al1 = [ Al1 = [
0., 0. 0., 0.
@ -43,22 +43,22 @@ Ax2 = [
b=[-1;-1;2;-1;0;1]; 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)); CHECK('combine_factors_x1', actual.equals(expected,1e-9));
%----------------------------------------------------------------------- %-----------------------------------------------------------------------
% combine_factors_x2 % combine_factors_x2
fg = createLinearFactorGraph(); fg = createGaussianFactorGraph();
actual = fg.combine_factors('x2'); actual = fg.combine_factors('x2');
%----------------------------------------------------------------------- %-----------------------------------------------------------------------
% eliminate_x1 % eliminate_x1
fg = createLinearFactorGraph(); fg = createGaussianFactorGraph();
actual = fg.eliminate_one('x1'); actual = fg.eliminate_one('x1');
%----------------------------------------------------------------------- %-----------------------------------------------------------------------
% eliminate_x2 % eliminate_x2
fg = createLinearFactorGraph(); fg = createGaussianFactorGraph();
actual = fg.eliminate_one('x2'); actual = fg.eliminate_one('x2');
%----------------------------------------------------------------------- %-----------------------------------------------------------------------
@ -91,7 +91,7 @@ expected.insert('l1', cg2);
expected.insert('x2', cg3); expected.insert('x2', cg3);
% Check one ordering % Check one ordering
fg1 = createLinearFactorGraph(); fg1 = createGaussianFactorGraph();
ord1 = Ordering; ord1 = Ordering;
ord1.push_back('x2'); ord1.push_back('x2');
ord1.push_back('l1'); ord1.push_back('l1');
@ -102,7 +102,7 @@ CHECK('eliminateAll', actual1.equals(expected));
%----------------------------------------------------------------------- %-----------------------------------------------------------------------
% matrix % matrix
fg = createLinearFactorGraph(); fg = createGaussianFactorGraph();
ord = Ordering; ord = Ordering;
ord.push_back('x2'); ord.push_back('x2');
ord.push_back('l1'); ord.push_back('l1');