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

Renamed FGConfig to VectorConfig in gtsam, easylib, EasySLAM, and mast.

release/4.3a0
Alex Cunningham 2009-10-14 20:39:59 +00:00
parent 8f20523e7f
commit 7d0a30c20f
39 changed files with 199 additions and 199 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cpp/ChordalBayesNet.cpp
View File

@ -37,17 +37,17 @@ void ChordalBayesNet::erase(const string& key)
/* ************************************************************************* */ /* ************************************************************************* */
// optimize, i.e. return x = inv(R)*d // optimize, i.e. return x = inv(R)*d
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<FGConfig> ChordalBayesNet::optimize() const boost::shared_ptr<VectorConfig> ChordalBayesNet::optimize() const
{ {
boost::shared_ptr<FGConfig> result(new FGConfig); boost::shared_ptr<VectorConfig> result(new VectorConfig);
result = optimize(result); result = optimize(result);
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<FGConfig> ChordalBayesNet::optimize(const boost::shared_ptr<FGConfig> &c) const boost::shared_ptr<VectorConfig> ChordalBayesNet::optimize(const boost::shared_ptr<VectorConfig> &c) const
{ {
boost::shared_ptr<FGConfig> result(new FGConfig); boost::shared_ptr<VectorConfig> result(new VectorConfig);
result = c; result = c;
/** solve each node in turn in topological sort order (parents first)*/ /** solve each node in turn in topological sort order (parents first)*/

6
cpp/ChordalBayesNet.h
View File

@ -14,7 +14,7 @@
#include <boost/serialization/list.hpp> #include <boost/serialization/list.hpp>
#include "ConditionalGaussian.h" #include "ConditionalGaussian.h"
#include "FGConfig.h" #include "VectorConfig.h"
namespace gtsam { namespace gtsam {
@ -72,8 +72,8 @@ public:
const_iterator const end() const {return nodes.end();} const_iterator const end() const {return nodes.end();}
/** optimize */ /** optimize */
boost::shared_ptr<FGConfig> optimize() const; boost::shared_ptr<VectorConfig> optimize() const;
boost::shared_ptr<FGConfig> optimize(const boost::shared_ptr<FGConfig> &c) const; boost::shared_ptr<VectorConfig> optimize(const boost::shared_ptr<VectorConfig> &c) const;
/** print */ /** print */
void print() const; void print() const;

2
cpp/ConditionalGaussian.cpp
View File

@ -62,7 +62,7 @@ void ConditionalGaussian::print(const string &s) const
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector ConditionalGaussian::solve(const FGConfig& x) const { Vector ConditionalGaussian::solve(const VectorConfig& x) const {
Vector rhs = d_; Vector rhs = d_;
for (map<string, Matrix>::const_iterator it = parents_.begin(); it for (map<string, Matrix>::const_iterator it = parents_.begin(); it
!= parents_.end(); it++) { != parents_.end(); it++) {

4
cpp/ConditionalGaussian.h
View File

@ -15,7 +15,7 @@
#include <boost/serialization/map.hpp> #include <boost/serialization/map.hpp>
#include <boost/serialization/shared_ptr.hpp> #include <boost/serialization/shared_ptr.hpp>
#include "Matrix.h" #include "Matrix.h"
#include "FGConfig.h" #include "VectorConfig.h"
#include "Ordering.h" #include "Ordering.h"
namespace gtsam { namespace gtsam {
@ -115,7 +115,7 @@ namespace gtsam {
* @param x configuration in which the parents values (y,z,...) are known * @param x configuration in which the parents values (y,z,...) are known
* @return solution x = R \ (d - Sy - Tz - ...) * @return solution x = R \ (d - Sy - Tz - ...)
*/ */
virtual Vector solve(const FGConfig& x) const; virtual Vector solve(const VectorConfig& x) const;
/** /**
* adds a parent * adds a parent

2
cpp/ConstrainedConditionalGaussian.cpp
View File

@ -48,7 +48,7 @@ ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const ConstrainedConditionalGaussian& df) { const ConstrainedConditionalGaussian& df) {
} }
Vector ConstrainedConditionalGaussian::solve(const FGConfig& x) const { Vector ConstrainedConditionalGaussian::solve(const VectorConfig& x) const {
// sum the RHS // sum the RHS
Vector rhs = d_; Vector rhs = d_;
for (map<string, Matrix>::const_iterator it = parents_.begin(); it for (map<string, Matrix>::const_iterator it = parents_.begin(); it

2
cpp/ConstrainedConditionalGaussian.h
View File

@ -100,7 +100,7 @@ public:
* @param config contains the values for all of the parents * @param config contains the values for all of the parents
* @return the value for this node * @return the value for this node
*/ */
Vector solve(const FGConfig& x) const; Vector solve(const VectorConfig& x) const;
}; };

4
cpp/ConstrainedLinearFactorGraph.cpp
View File

@ -217,9 +217,9 @@ void ConstrainedLinearFactorGraph::update_constraints(const std::string& key,
} }
/* ************************************************************************* */ /* ************************************************************************* */
FGConfig ConstrainedLinearFactorGraph::optimize(const Ordering& ordering) { VectorConfig ConstrainedLinearFactorGraph::optimize(const Ordering& ordering) {
ChordalBayesNet::shared_ptr cbn = eliminate(ordering); ChordalBayesNet::shared_ptr cbn = eliminate(ordering);
boost::shared_ptr<FGConfig> newConfig = cbn->optimize(); boost::shared_ptr<VectorConfig> newConfig = cbn->optimize();
return *newConfig; return *newConfig;
} }

2
cpp/ConstrainedLinearFactorGraph.h
View File

@ -114,7 +114,7 @@ public:
* optimize a linear factor graph * optimize a linear factor graph
* @param ordering fg in order * @param ordering fg in order
*/ */
FGConfig optimize(const Ordering& ordering); VectorConfig optimize(const Ordering& ordering);
/** /**
* Determines if a node has any constraints attached to it * Determines if a node has any constraints attached to it

4
cpp/ConstrainedNonlinearFactorGraph.h
View File

@ -19,8 +19,8 @@ namespace gtsam {
* A nonlinear factor graph with the addition of equality constraints. * A nonlinear factor graph with the addition of equality constraints.
* *
* Templated on factor and configuration type. * Templated on factor and configuration type.
* TODO FD: this is totally wrong: it can only work if Config==FGConfig * TODO FD: this is totally wrong: it can only work if Config==VectorConfig
* as LinearConstraint is only defined for FGConfig * as LinearConstraint is only defined for VectorConfig
* To fix it, we need to think more deeply about this. * To fix it, we need to think more deeply about this.
*/ */
template<class Factor, class Config> template<class Factor, class Config>

2
cpp/Factor.h
View File

@ -40,7 +40,7 @@ namespace gtsam {
* conflicts with efficiency as well, esp. in the case of incomplete * conflicts with efficiency as well, esp. in the case of incomplete
* QR factorization. A solution is still being sought. * QR factorization. A solution is still being sought.
* *
* A Factor is templated on a Config, for example FGConfig is a configuration of * A Factor is templated on a Config, for example VectorConfig is a configuration of
* labeled vectors. This way, we can have factors that might be defined on discrete * labeled vectors. This way, we can have factors that might be defined on discrete
* variables, continuous ones, or a combination of both. It is up to the config to * variables, continuous ones, or a combination of both. It is up to the config to
* provide the appropriate values at the appropriate time. * provide the appropriate values at the appropriate time.

2
cpp/FactorGraph.h
View File

@ -16,7 +16,7 @@
namespace gtsam { namespace gtsam {
class Ordering; class Ordering;
class FGConfig; class VectorConfig;
class LinearFactor; class LinearFactor;
class LinearFactorGraph; class LinearFactorGraph;
class Ordering; class Ordering;

2
cpp/LinearFactor.cpp
View File

@ -22,7 +22,7 @@ typedef pair<const string, Matrix>& mypair;
/* ************************************************************************* */ /* ************************************************************************* */
// 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 FGConfig& c) const { double LinearFactor::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;

4
cpp/LinearFactor.h
View File

@ -32,7 +32,7 @@ class MutableLinearFactor;
* LinearFactor is non-mutable (all methods const!). * LinearFactor 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<FGConfig> { class LinearFactor: public Factor<VectorConfig> {
public: public:
typedef boost::shared_ptr<LinearFactor> shared_ptr; typedef boost::shared_ptr<LinearFactor> shared_ptr;
@ -98,7 +98,7 @@ public:
// Implementing Factor virtual functions // Implementing Factor virtual functions
double error(const FGConfig& c) const; /** 0.5*(A*x-b)'*(A*x-b) */ double error(const VectorConfig& c) const; /** 0.5*(A*x-b)'*(A*x-b) */
void print(const std::string& s = "") const; void print(const std::string& s = "") const;
bool equals(const LinearFactor& lf, double tol = 1e-9) const; bool equals(const LinearFactor& lf, double tol = 1e-9) const;
std::string dump() const { return "";} std::string dump() const { return "";}

4
cpp/LinearFactorGraph.cpp
View File

@ -142,13 +142,13 @@ LinearFactorGraph::eliminate(const Ordering& ordering)
/* ************************************************************************* */ /* ************************************************************************* */
/** optimize the linear factor graph */ /** optimize the linear factor graph */
/* ************************************************************************* */ /* ************************************************************************* */
FGConfig LinearFactorGraph::optimize(const Ordering& ordering) VectorConfig LinearFactorGraph::optimize(const Ordering& ordering)
{ {
// eliminate all nodes in the given ordering -> chordal Bayes net // eliminate all nodes in the given ordering -> chordal Bayes net
ChordalBayesNet::shared_ptr chordalBayesNet = eliminate(ordering); ChordalBayesNet::shared_ptr chordalBayesNet = eliminate(ordering);
// calculate new configuration (using backsubstitution) // calculate new configuration (using backsubstitution)
boost::shared_ptr<FGConfig> newConfig = chordalBayesNet->optimize(); boost::shared_ptr<VectorConfig> newConfig = chordalBayesNet->optimize();
return *newConfig; return *newConfig;
} }

8
cpp/LinearFactorGraph.h
View File

@ -16,7 +16,7 @@
#include "LinearFactor.h" #include "LinearFactor.h"
//#include "Ordering.h" //#include "Ordering.h"
#include "FGConfig.h" #include "VectorConfig.h"
#include "FactorGraph.h" #include "FactorGraph.h"
namespace gtsam { namespace gtsam {
@ -26,10 +26,10 @@ 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 == LinearFactor
* FGConfig = 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, FGConfig> { class LinearFactorGraph : public FactorGraph<LinearFactor, VectorConfig> {
public: public:
/** /**
@ -92,7 +92,7 @@ namespace gtsam {
* optimize a linear factor graph * optimize a linear factor graph
* @param ordering fg in order * @param ordering fg in order
*/ */
FGConfig optimize(const Ordering& ordering); VectorConfig optimize(const Ordering& ordering);
/** /**
* static function that combines two factor graphs * static function that combines two factor graphs

8
cpp/Makefile.am
View File

@ -57,18 +57,18 @@ testVector_LDADD = libgtsam.la
testMatrix_LDADD = libgtsam.la testMatrix_LDADD = libgtsam.la
# nodes # nodes
sources += FGConfig.cpp Ordering.cpp LinearFactor.cpp NonlinearFactor.cpp sources += VectorConfig.cpp Ordering.cpp LinearFactor.cpp NonlinearFactor.cpp
sources += ConditionalGaussian.cpp LinearConstraint.cpp ConstrainedConditionalGaussian.cpp sources += ConditionalGaussian.cpp LinearConstraint.cpp ConstrainedConditionalGaussian.cpp
check_PROGRAMS += testFGConfig testLinearFactor testConditionalGaussian testNonlinearFactor testLinearConstraint testConstrainedConditionalGaussian check_PROGRAMS += testVectorConfig testLinearFactor testConditionalGaussian testNonlinearFactor testLinearConstraint testConstrainedConditionalGaussian
example = smallExample.cpp example = smallExample.cpp
testFGConfig_SOURCES = testFGConfig.cpp testVectorConfig_SOURCES = testVectorConfig.cpp
testLinearFactor_SOURCES = $(example) testLinearFactor.cpp testLinearFactor_SOURCES = $(example) testLinearFactor.cpp
testConditionalGaussian_SOURCES = $(example) testConditionalGaussian.cpp testConditionalGaussian_SOURCES = $(example) testConditionalGaussian.cpp
testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp
testLinearConstraint_SOURCES = $(example) testLinearConstraint.cpp testLinearConstraint_SOURCES = $(example) testLinearConstraint.cpp
testConstrainedConditionalGaussian_SOURCES = testConstrainedConditionalGaussian.cpp testConstrainedConditionalGaussian_SOURCES = testConstrainedConditionalGaussian.cpp
testFGConfig_LDADD = libgtsam.la testVectorConfig_LDADD = libgtsam.la
testLinearFactor_LDADD = libgtsam.la testLinearFactor_LDADD = libgtsam.la
testConditionalGaussian_LDADD = libgtsam.la testConditionalGaussian_LDADD = libgtsam.la
testNonlinearFactor_LDADD = libgtsam.la testNonlinearFactor_LDADD = libgtsam.la

16
cpp/NonlinearFactor.cpp
View File

@ -19,7 +19,7 @@ NonlinearFactor1::NonlinearFactor1(const Vector& z,
Vector (*h)(const Vector&), Vector (*h)(const Vector&),
const string& key1, const string& key1,
Matrix (*H)(const Vector&)) Matrix (*H)(const Vector&))
: NonlinearFactor<FGConfig>(z, sigma), h_(h), key1_(key1), H_(H) : NonlinearFactor<VectorConfig>(z, sigma), h_(h), key1_(key1), H_(H)
{ {
keys_.push_front(key1); keys_.push_front(key1);
} }
@ -30,7 +30,7 @@ void NonlinearFactor1::print(const string& s) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
LinearFactor::shared_ptr NonlinearFactor1::linearize(const FGConfig& c) const { LinearFactor::shared_ptr NonlinearFactor1::linearize(const VectorConfig& c) const {
// get argument 1 from config // get argument 1 from config
Vector x1 = c[key1_]; Vector x1 = c[key1_];
@ -47,10 +47,10 @@ LinearFactor::shared_ptr NonlinearFactor1::linearize(const FGConfig& c) const {
/* ************************************************************************* */ /* ************************************************************************* */
/** http://artis.imag.fr/~Xavier.Decoret/resources/C++/operator==.html */ /** http://artis.imag.fr/~Xavier.Decoret/resources/C++/operator==.html */
/* ************************************************************************* */ /* ************************************************************************* */
bool NonlinearFactor1::equals(const NonlinearFactor<FGConfig>& f, double tol) const { bool NonlinearFactor1::equals(const NonlinearFactor<VectorConfig>& f, double tol) const {
const NonlinearFactor1* p = dynamic_cast<const NonlinearFactor1*> (&f); const NonlinearFactor1* p = dynamic_cast<const NonlinearFactor1*> (&f);
if (p == NULL) return false; if (p == NULL) return false;
return NonlinearFactor<FGConfig>::equals(*p, tol) return NonlinearFactor<VectorConfig>::equals(*p, tol)
&& (h_ == p->h_) && (h_ == p->h_)
&& (key1_== p->key1_) && (key1_== p->key1_)
&& (H_ == p->H_); && (H_ == p->H_);
@ -65,7 +65,7 @@ NonlinearFactor2::NonlinearFactor2(const Vector& z,
const string& key2, const string& key2,
Matrix (*H2)(const Vector&, const Vector&) Matrix (*H2)(const Vector&, const Vector&)
) )
: NonlinearFactor<FGConfig>(z, sigma), h_(h), key1_(key1), H1_(H1), key2_(key2), H2_(H2) : NonlinearFactor<VectorConfig>(z, sigma), h_(h), key1_(key1), H1_(H1), key2_(key2), H2_(H2)
{ {
keys_.push_front(key1); keys_.push_front(key1);
keys_.push_front(key2); keys_.push_front(key2);
@ -77,7 +77,7 @@ void NonlinearFactor2::print(const string& s) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
LinearFactor::shared_ptr NonlinearFactor2::linearize(const FGConfig& c) const { LinearFactor::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_];
@ -94,10 +94,10 @@ LinearFactor::shared_ptr NonlinearFactor2::linearize(const FGConfig& c) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool NonlinearFactor2::equals(const NonlinearFactor<FGConfig>& f, double tol) const { bool NonlinearFactor2::equals(const NonlinearFactor<VectorConfig>& f, double tol) const {
const NonlinearFactor2* p = dynamic_cast<const NonlinearFactor2*> (&f); const NonlinearFactor2* p = dynamic_cast<const NonlinearFactor2*> (&f);
if (p == NULL) return false; if (p == NULL) return false;
return NonlinearFactor<FGConfig>::equals(*p, tol) return NonlinearFactor<VectorConfig>::equals(*p, tol)
&& (h_ == p->h_) && (h_ == p->h_)
&& (key1_ == p->key1_) && (key1_ == p->key1_)
&& (H2_ == p->H1_) && (H2_ == p->H1_)

16
cpp/NonlinearFactor.h
View File

@ -113,7 +113,7 @@ namespace gtsam {
* Note: cannot be serialized as contains function pointers * Note: cannot be serialized as contains function pointers
* Specialized derived classes could do this * Specialized derived classes could do this
*/ */
class NonlinearFactor1 : public NonlinearFactor<FGConfig> { class NonlinearFactor1 : public NonlinearFactor<VectorConfig> {
private: private:
std::string key1_; std::string key1_;
@ -133,15 +133,15 @@ namespace gtsam {
void print(const std::string& s = "") const; void print(const std::string& s = "") const;
/** error function */ /** error function */
inline Vector error_vector(const FGConfig& c) const { inline Vector error_vector(const VectorConfig& c) const {
return z_ - h_(c[key1_]); return z_ - h_(c[key1_]);
} }
/** linearize a non-linearFactor1 to get a linearFactor1 */ /** linearize a non-linearFactor1 to get a linearFactor1 */
boost::shared_ptr<LinearFactor> linearize(const FGConfig& c) const; boost::shared_ptr<LinearFactor> linearize(const VectorConfig& c) const;
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const NonlinearFactor<FGConfig>& f, double tol=1e-9) const; bool equals(const NonlinearFactor<VectorConfig>& f, double tol=1e-9) const;
std::string dump() const {return "";} std::string dump() const {return "";}
}; };
@ -151,7 +151,7 @@ namespace gtsam {
* Note: cannot be serialized as contains function pointers * Note: cannot be serialized as contains function pointers
* Specialized derived classes could do this * Specialized derived classes could do this
*/ */
class NonlinearFactor2 : public NonlinearFactor<FGConfig> { class NonlinearFactor2 : public NonlinearFactor<VectorConfig> {
private: private:
@ -176,15 +176,15 @@ namespace gtsam {
void print(const std::string& s = "") const; void print(const std::string& s = "") const;
/** error function */ /** error function */
inline Vector error_vector(const FGConfig& c) const { inline Vector error_vector(const VectorConfig& c) const {
return z_ - h_(c[key1_], c[key2_]); return z_ - h_(c[key1_], c[key2_]);
} }
/** Linearize a non-linearFactor2 to get a linearFactor2 */ /** Linearize a non-linearFactor2 to get a linearFactor2 */
boost::shared_ptr<LinearFactor> linearize(const FGConfig& c) const; boost::shared_ptr<LinearFactor> linearize(const VectorConfig& c) const;
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const NonlinearFactor<FGConfig>& f, double tol=1e-9) const; bool equals(const NonlinearFactor<VectorConfig>& f, double tol=1e-9) const;
std::string dump() const{return "";}; std::string dump() const{return "";};
}; };

2
cpp/NonlinearFactorGraph.h
View File

@ -21,7 +21,7 @@ namespace gtsam {
* 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
* vector space, the config type will be an FGConfig in that linearized * vector space, the config type will be an VectorConfig in that linearized
*/ */
template<class Config> template<class Config>
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<Config> ,Config> { class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<Config> ,Config> {

6
cpp/NonlinearOptimizer-inl.h
View File

@ -54,7 +54,7 @@ namespace gtsam {
// linearize and optimize // linearize and optimize
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class C> template<class G, class C>
FGConfig NonlinearOptimizer<G, C>::delta() const { VectorConfig NonlinearOptimizer<G, C>::delta() const {
LinearFactorGraph linear = graph_->linearize(*config_); LinearFactorGraph linear = graph_->linearize(*config_);
return linear.optimize(*ordering_); return linear.optimize(*ordering_);
} }
@ -67,7 +67,7 @@ namespace gtsam {
verbosityLevel verbosity) const { verbosityLevel verbosity) const {
// linearize and optimize // linearize and optimize
FGConfig delta = this->delta(); VectorConfig delta = this->delta();
// maybe show output // maybe show output
if (verbosity >= DELTA) if (verbosity >= DELTA)
@ -121,7 +121,7 @@ namespace gtsam {
damped.print("damped"); damped.print("damped");
// solve // solve
FGConfig delta = damped.optimize(*ordering_); VectorConfig delta = damped.optimize(*ordering_);
if (verbosity >= TRYDELTA) if (verbosity >= TRYDELTA)
delta.print("delta"); delta.print("delta");

6
cpp/NonlinearOptimizer.h
View File

@ -10,7 +10,7 @@
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include "NonlinearFactorGraph.h" #include "NonlinearFactorGraph.h"
#include "FGConfig.h" #include "VectorConfig.h"
namespace gtsam { namespace gtsam {
@ -96,9 +96,9 @@ namespace gtsam {
/** /**
* linearize and optimize * linearize and optimize
* Thi returns an FGConfig, i.e., vectors in tangent space of Config * Thi returns an VectorConfig, i.e., vectors in tangent space of Config
*/ */
FGConfig delta() const; VectorConfig delta() const;
/** /**
* Do one Gauss-Newton iteration and return next state * Do one Gauss-Newton iteration and return next state

2
cpp/VSLAMFactor.h
View File

@ -8,7 +8,7 @@
#include "NonlinearFactor.h" #include "NonlinearFactor.h"
#include "LinearFactor.h" #include "LinearFactor.h"
#include "FGConfig.h" #include "VectorConfig.h"
#include "Cal3_S2.h" #include "Cal3_S2.h"
#include "Pose3.h" #include "Pose3.h"

22
cpp/FGConfig.cpp → cpp/VectorConfig.cpp
View File

@ -1,14 +1,14 @@
/** /**
* @file FGConfig.cpp * @file VectorConfig.cpp
* @brief Factor Graph Configuration * @brief Factor Graph Configuration
* @brief fgConfig * @brief VectorConfig
* @author Carlos Nieto * @author Carlos Nieto
* @author Christian Potthast * @author Christian Potthast
*/ */
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include "FGConfig.h" #include "VectorConfig.h"
// trick from some reading group // trick from some reading group
#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)
@ -22,14 +22,14 @@ void check_size(const string& key, const Vector & vj, const Vector & dj) {
cout << "For key \"" << key << "\"" << endl; cout << "For key \"" << key << "\"" << endl;
cout << "vj.size = " << vj.size() << endl; cout << "vj.size = " << vj.size() << endl;
cout << "dj.size = " << dj.size() << endl; cout << "dj.size = " << dj.size() << endl;
throw(std::invalid_argument("FGConfig::+ mismatched dimensions")); throw(std::invalid_argument("VectorConfig::+ mismatched dimensions"));
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
FGConfig FGConfig::exmap(const FGConfig & delta) const VectorConfig VectorConfig::exmap(const VectorConfig & delta) const
{ {
FGConfig newConfig; VectorConfig newConfig;
for (const_iterator it = values.begin(); it!=values.end(); it++) { for (const_iterator it = values.begin(); it!=values.end(); it++) {
string j = it->first; string j = it->first;
const Vector &vj = it->second; const Vector &vj = it->second;
@ -41,19 +41,19 @@ FGConfig FGConfig::exmap(const FGConfig & delta) const
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector FGConfig::get(const std::string& name) const { Vector VectorConfig::get(const std::string& name) const {
const_iterator it = values.find(name); const_iterator it = values.find(name);
if (it==values.end()) { if (it==values.end()) {
print(); print();
cout << "asked for key " << name << endl; cout << "asked for key " << name << endl;
throw(std::invalid_argument("FGConfig::[] invalid key")); throw(std::invalid_argument("VectorConfig::[] invalid key"));
} }
return it->second; return it->second;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void FGConfig::print(const std::string& name) const { void VectorConfig::print(const std::string& name) const {
odprintf("FGConfig %s\n", name.c_str()); odprintf("VectorConfig %s\n", name.c_str());
odprintf("size: %d\n", values.size()); odprintf("size: %d\n", values.size());
string j; Vector v; string j; Vector v;
FOREACH_PAIR(j, v, values) { FOREACH_PAIR(j, v, values) {
@ -63,7 +63,7 @@ void FGConfig::print(const std::string& name) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool FGConfig::equals(const FGConfig& expected, double tol) const { bool VectorConfig::equals(const VectorConfig& expected, double tol) const {
string j; Vector vActual; string j; Vector vActual;
if( values.size() != expected.size() ) goto fail; if( values.size() != expected.size() ) goto fail;

18
cpp/FGConfig.h → cpp/VectorConfig.h
View File

@ -1,5 +1,5 @@
/** /**
* @file FGConfig.h * @file VectorConfig.h
* @brief Factor Graph Configuration * @brief Factor Graph Configuration
* @author Carlos Nieto * @author Carlos Nieto
* @author Christian Potthast * @author Christian Potthast
@ -18,7 +18,7 @@
namespace gtsam { namespace gtsam {
/** Factor Graph Configuration */ /** Factor Graph Configuration */
class FGConfig : public Testable<FGConfig> { class VectorConfig : public Testable<VectorConfig> {
protected: protected:
/** Map from string indices to values */ /** Map from string indices to values */
@ -28,10 +28,10 @@ namespace gtsam {
typedef std::map<std::string, Vector>::iterator iterator; typedef std::map<std::string, Vector>::iterator iterator;
typedef std::map<std::string, Vector>::const_iterator const_iterator; typedef std::map<std::string, Vector>::const_iterator const_iterator;
FGConfig():Testable<FGConfig>() {} VectorConfig():Testable<VectorConfig>() {}
FGConfig(const FGConfig& cfg_in) : Testable<FGConfig>(), values(cfg_in.values) {} VectorConfig(const VectorConfig& cfg_in) : Testable<VectorConfig>(), values(cfg_in.values) {}
virtual ~FGConfig() {} virtual ~VectorConfig() {}
/** return all the nodes in the graph **/ /** return all the nodes in the graph **/
std::vector<std::string> get_names() const { std::vector<std::string> get_names() const {
@ -42,16 +42,16 @@ namespace gtsam {
} }
/** Insert a value into the configuration with a given index */ /** Insert a value into the configuration with a given index */
FGConfig& insert(const std::string& name, const Vector& val) { VectorConfig& insert(const std::string& name, const Vector& val) {
values.insert(std::make_pair(name,val)); values.insert(std::make_pair(name,val));
return *this; return *this;
} }
/** /**
* Add a delta config, needed for use in NonlinearOptimizer * Add a delta config, needed for use in NonlinearOptimizer
* For FGConfig, this is just addition. * For VectorConfig, this is just addition.
*/ */
FGConfig exmap(const FGConfig & delta) const; VectorConfig exmap(const VectorConfig & delta) const;
const_iterator begin() const {return values.begin();} const_iterator begin() const {return values.begin();}
const_iterator end() const {return values.end();} const_iterator end() const {return values.end();}
@ -78,7 +78,7 @@ namespace gtsam {
void print(const std::string& name = "") const; void print(const std::string& name = "") const;
/** equals, for unit testing */ /** equals, for unit testing */
bool equals(const FGConfig& expected, double tol=1e-9) const; bool equals(const VectorConfig& expected, double tol=1e-9) const;
void clear() {values.clear();} void clear() {values.clear();}

36
cpp/gtsam.h
View File

@ -1,11 +1,11 @@
class FGConfig { class VectorConfig {
FGConfig(); VectorConfig();
Vector get(string name) const; Vector get(string name) const;
bool contains(string name) const; bool contains(string name) const;
size_t size() const; size_t size() const;
void insert(string name, Vector val); void insert(string name, Vector val);
void print() const; void print() const;
bool equals(const FGConfig& expected, double tol) const; bool equals(const VectorConfig& expected, double tol) const;
void clear(); void clear();
}; };
@ -33,7 +33,7 @@ class LinearFactor {
bool empty() const; bool empty() const;
Vector get_b() const; Vector get_b() const;
Matrix get_A(string key) const; Matrix get_A(string key) const;
double error(const FGConfig& c) const; double error(const VectorConfig& c) const;
bool involves(string key) const; bool involves(string key) const;
void print() const; void print() const;
bool equals(const LinearFactor& lf, double tol) const; bool equals(const LinearFactor& lf, double tol) const;
@ -55,7 +55,7 @@ class ConditionalGaussian {
string name2, string name2,
Matrix T); Matrix T);
void print() const; void print() const;
Vector solve(const FGConfig& x); Vector solve(const VectorConfig& x);
void add(string key, Matrix S); void add(string key, Matrix S);
bool equals(const ConditionalGaussian &cg) const; bool equals(const ConditionalGaussian &cg) const;
}; };
@ -70,7 +70,7 @@ class ChordalBayesNet {
ChordalBayesNet(); ChordalBayesNet();
void insert(string name, ConditionalGaussian* node); void insert(string name, ConditionalGaussian* node);
ConditionalGaussian* get(string name); ConditionalGaussian* get(string name);
FGConfig* optimize(); VectorConfig* optimize();
void print() const; void print() const;
bool equals(const ChordalBayesNet& cbn) const; bool equals(const ChordalBayesNet& cbn) const;
pair<Matrix,Vector> matrix() const; pair<Matrix,Vector> matrix() const;
@ -81,12 +81,12 @@ class LinearFactorGraph {
size_t size() const; size_t size() const;
void push_back(LinearFactor* ptr_f); void push_back(LinearFactor* ptr_f);
double error(const FGConfig& c) const; double error(const VectorConfig& c) const;
double probPrime(const FGConfig& c) const; double probPrime(const VectorConfig& c) const;
void print() const; void print() const;
bool equals(const LinearFactorGraph& lfgraph) const; bool equals(const LinearFactorGraph& lfgraph) const;
FGConfig optimize(const Ordering& ordering); VectorConfig optimize(const Ordering& ordering);
LinearFactor* combine_factors(string key); LinearFactor* combine_factors(string key);
ConditionalGaussian* eliminate_one(string key); ConditionalGaussian* eliminate_one(string key);
ChordalBayesNet* eliminate(const Ordering& ordering); ChordalBayesNet* eliminate(const Ordering& ordering);
@ -117,31 +117,31 @@ class Point3 {
class Point2Prior { class Point2Prior {
Point2Prior(Vector mu, double sigma, string key); Point2Prior(Vector mu, double sigma, string key);
Vector error_vector(const FGConfig& c) const; Vector error_vector(const VectorConfig& c) const;
LinearFactor* linearize(const FGConfig& c) const; LinearFactor* linearize(const VectorConfig& c) const;
double get_sigma(); double get_sigma();
Vector get_measurement(); Vector get_measurement();
double error(const FGConfig& c) const; double error(const VectorConfig& c) const;
void print() const; void print() const;
}; };
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 FGConfig& c) const; Vector error_vector(const VectorConfig& c) const;
LinearFactor* linearize(const FGConfig& c) const; LinearFactor* linearize(const VectorConfig& c) const;
double get_sigma(); double get_sigma();
Vector get_measurement(); Vector get_measurement();
double error(const FGConfig& c) const; double error(const VectorConfig& c) const;
void print() const; void print() const;
}; };
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 FGConfig& c) const; Vector error_vector(const VectorConfig& c) const;
LinearFactor* linearize(const FGConfig& c) const; LinearFactor* linearize(const VectorConfig& c) const;
double get_sigma(); double get_sigma();
Vector get_measurement(); Vector get_measurement();
double error(const FGConfig& c) const; double error(const VectorConfig& c) const;
void print() const; void print() const;
}; };

42
cpp/smallExample.cpp
View File

@ -26,7 +26,7 @@ using namespace std;
namespace gtsam { namespace gtsam {
typedef boost::shared_ptr<NonlinearFactor<FGConfig> > shared; typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared;
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedNonlinearFactorGraph() { boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedNonlinearFactorGraph() {
@ -66,12 +66,12 @@ ExampleNonlinearFactorGraph createNonlinearFactorGraph() {
} }
/* ************************************************************************* */ /* ************************************************************************* */
FGConfig createConfig() VectorConfig createConfig()
{ {
Vector v_x1(2); v_x1(0) = 0.; v_x1(1) = 0.; Vector v_x1(2); v_x1(0) = 0.; v_x1(1) = 0.;
Vector v_x2(2); v_x2(0) = 1.5; v_x2(1) = 0.; Vector v_x2(2); v_x2(0) = 1.5; v_x2(1) = 0.;
Vector v_l1(2); v_l1(0) = 0.; v_l1(1) = -1.; Vector v_l1(2); v_l1(0) = 0.; v_l1(1) = -1.;
FGConfig c; VectorConfig c;
c.insert("x1", v_x1); c.insert("x1", v_x1);
c.insert("x2", v_x2); c.insert("x2", v_x2);
c.insert("l1", v_l1); c.insert("l1", v_l1);
@ -79,12 +79,12 @@ FGConfig createConfig()
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<const FGConfig> sharedNoisyConfig() boost::shared_ptr<const VectorConfig> sharedNoisyConfig()
{ {
Vector v_x1(2); v_x1(0) = 0.1; v_x1(1) = 0.1; Vector v_x1(2); v_x1(0) = 0.1; v_x1(1) = 0.1;
Vector v_x2(2); v_x2(0) = 1.4; v_x2(1) = 0.2; Vector v_x2(2); v_x2(0) = 1.4; v_x2(1) = 0.2;
Vector v_l1(2); v_l1(0) = 0.1; v_l1(1) = -1.1; Vector v_l1(2); v_l1(0) = 0.1; v_l1(1) = -1.1;
boost::shared_ptr<FGConfig> c(new FGConfig); boost::shared_ptr<VectorConfig> c(new VectorConfig);
c->insert("x1", v_x1); c->insert("x1", v_x1);
c->insert("x2", v_x2); c->insert("x2", v_x2);
c->insert("l1", v_l1); c->insert("l1", v_l1);
@ -92,16 +92,16 @@ boost::shared_ptr<const FGConfig> sharedNoisyConfig()
} }
/* ************************************************************************* */ /* ************************************************************************* */
FGConfig createNoisyConfig() { VectorConfig createNoisyConfig() {
return *sharedNoisyConfig(); return *sharedNoisyConfig();
} }
/* ************************************************************************* */ /* ************************************************************************* */
FGConfig createCorrectDelta() { VectorConfig createCorrectDelta() {
Vector v_x1(2); v_x1(0) = -0.1; v_x1(1) = -0.1; Vector v_x1(2); v_x1(0) = -0.1; v_x1(1) = -0.1;
Vector v_x2(2); v_x2(0) = 0.1; v_x2(1) = -0.2; Vector v_x2(2); v_x2(0) = 0.1; v_x2(1) = -0.2;
Vector v_l1(2); v_l1(0) = -0.1; v_l1(1) = 0.1; Vector v_l1(2); v_l1(0) = -0.1; v_l1(1) = 0.1;
FGConfig c; VectorConfig c;
c.insert("x1", v_x1); c.insert("x1", v_x1);
c.insert("x2", v_x2); c.insert("x2", v_x2);
c.insert("l1", v_l1); c.insert("l1", v_l1);
@ -109,11 +109,11 @@ FGConfig createCorrectDelta() {
} }
/* ************************************************************************* */ /* ************************************************************************* */
FGConfig createZeroDelta() { VectorConfig createZeroDelta() {
Vector v_x1(2); v_x1(0) = 0; v_x1(1) = 0; Vector v_x1(2); v_x1(0) = 0; v_x1(1) = 0;
Vector v_x2(2); v_x2(0) = 0; v_x2(1) = 0; Vector v_x2(2); v_x2(0) = 0; v_x2(1) = 0;
Vector v_l1(2); v_l1(0) = 0; v_l1(1) = 0; Vector v_l1(2); v_l1(0) = 0; v_l1(1) = 0;
FGConfig c; VectorConfig c;
c.insert("x1", v_x1); c.insert("x1", v_x1);
c.insert("x2", v_x2); c.insert("x2", v_x2);
c.insert("l1", v_l1); c.insert("l1", v_l1);
@ -123,7 +123,7 @@ FGConfig createZeroDelta() {
/* ************************************************************************* */ /* ************************************************************************* */
LinearFactorGraph createLinearFactorGraph() LinearFactorGraph createLinearFactorGraph()
{ {
FGConfig c = createNoisyConfig(); VectorConfig c = createNoisyConfig();
// Create // Create
LinearFactorGraph fg; LinearFactorGraph fg;
@ -333,9 +333,9 @@ ConstrainedLinearFactorGraph createMultiConstraintGraph() {
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
// ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig> , FGConfig> createConstrainedNonlinearFactorGraph() { // ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorConfig> , VectorConfig> createConstrainedNonlinearFactorGraph() {
// ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig> , FGConfig> graph; // ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorConfig> , VectorConfig> graph;
// FGConfig c = createConstrainedConfig(); // VectorConfig c = createConstrainedConfig();
// //
// // equality constraint for initial pose // // equality constraint for initial pose
// LinearConstraint::shared_ptr f1(new LinearConstraint(c["x0"], "x0")); // LinearConstraint::shared_ptr f1(new LinearConstraint(c["x0"], "x0"));
@ -349,9 +349,9 @@ ConstrainedLinearFactorGraph createMultiConstraintGraph() {
// } // }
/* ************************************************************************* */ /* ************************************************************************* */
//FGConfig createConstrainedConfig() //VectorConfig createConstrainedConfig()
//{ //{
// FGConfig config; // VectorConfig config;
// //
// Vector x0(2); x0(0)=1.0; x0(1)=2.0; // Vector x0(2); x0(0)=1.0; x0(1)=2.0;
// config.insert("x0", x0); // config.insert("x0", x0);
@ -363,9 +363,9 @@ ConstrainedLinearFactorGraph createMultiConstraintGraph() {
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
//FGConfig createConstrainedLinConfig() //VectorConfig createConstrainedLinConfig()
//{ //{
// FGConfig config; // VectorConfig config;
// //
// Vector x0(2); x0(0)=1.0; x0(1)=2.0; // value doesn't actually matter // Vector x0(2); x0(0)=1.0; x0(1)=2.0; // value doesn't actually matter
// config.insert("x0", x0); // config.insert("x0", x0);
@ -377,9 +377,9 @@ ConstrainedLinearFactorGraph createMultiConstraintGraph() {
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
//FGConfig createConstrainedCorrectDelta() //VectorConfig createConstrainedCorrectDelta()
//{ //{
// FGConfig config; // VectorConfig config;
// //
// Vector x0(2); x0(0)=0.; x0(1)=0.; // Vector x0(2); x0(0)=0.; x0(1)=0.;
// config.insert("x0", x0); // config.insert("x0", x0);
@ -394,7 +394,7 @@ ConstrainedLinearFactorGraph createMultiConstraintGraph() {
//ConstrainedChordalBayesNet createConstrainedChordalBayesNet() //ConstrainedChordalBayesNet createConstrainedChordalBayesNet()
//{ //{
// ConstrainedChordalBayesNet cbn; // ConstrainedChordalBayesNet cbn;
// FGConfig c = createConstrainedConfig(); // VectorConfig c = createConstrainedConfig();
// //
// // add regular conditional gaussian - no parent // // add regular conditional gaussian - no parent
// Matrix R = eye(2); // Matrix R = eye(2);

22
cpp/smallExample.h
View File

@ -16,13 +16,13 @@
#include "NonlinearFactorGraph.h" #include "NonlinearFactorGraph.h"
#include "ChordalBayesNet.h" #include "ChordalBayesNet.h"
#include "LinearFactorGraph.h" #include "LinearFactorGraph.h"
#include "FGConfig.h" #include "VectorConfig.h"
// \namespace // \namespace
namespace gtsam { namespace gtsam {
typedef NonlinearFactorGraph<FGConfig> ExampleNonlinearFactorGraph; typedef NonlinearFactorGraph<VectorConfig> ExampleNonlinearFactorGraph;
/** /**
* Create small example for non-linear factor graph * Create small example for non-linear factor graph
@ -34,23 +34,23 @@ namespace gtsam {
* Create configuration to go with it * Create configuration to go with it
* The ground truth configuration for the example above * The ground truth configuration for the example above
*/ */
FGConfig createConfig(); VectorConfig createConfig();
/** /**
* create a noisy configuration for a nonlinear factor graph * create a noisy configuration for a nonlinear factor graph
*/ */
boost::shared_ptr<const FGConfig> sharedNoisyConfig(); boost::shared_ptr<const VectorConfig> sharedNoisyConfig();
FGConfig createNoisyConfig(); VectorConfig createNoisyConfig();
/** /**
* Zero delta config * Zero delta config
*/ */
FGConfig createZeroDelta(); VectorConfig createZeroDelta();
/** /**
* Delta config that, when added to noisyConfig, returns the ground truth * Delta config that, when added to noisyConfig, returns the ground truth
*/ */
FGConfig createCorrectDelta(); VectorConfig createCorrectDelta();
/** /**
* create a linear factor graph * create a linear factor graph
@ -93,17 +93,17 @@ namespace gtsam {
* Create configuration for constrained example * Create configuration for constrained example
* This is the ground truth version * This is the ground truth version
*/ */
//FGConfig createConstrainedConfig(); //VectorConfig createConstrainedConfig();
/** /**
* Create a noisy configuration for linearization * Create a noisy configuration for linearization
*/ */
//FGConfig createConstrainedLinConfig(); //VectorConfig createConstrainedLinConfig();
/** /**
* Create the correct delta configuration * Create the correct delta configuration
*/ */
//FGConfig createConstrainedCorrectDelta(); //VectorConfig createConstrainedCorrectDelta();
/** /**
* Create small example constrained factor graph * Create small example constrained factor graph
@ -113,6 +113,6 @@ namespace gtsam {
/** /**
* Create small example constrained nonlinear factor graph * Create small example constrained nonlinear factor graph
*/ */
// ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig>,FGConfig> // ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorConfig>,VectorConfig>
// createConstrainedNonlinearFactorGraph(); // createConstrainedNonlinearFactorGraph();
} }

4
cpp/testChordalBayesNet.cpp
View File

@ -69,9 +69,9 @@ TEST( ChordalBayesNet, optimize )
{ {
// optimize small Bayes Net // optimize small Bayes Net
ChordalBayesNet cbn = createSmallChordalBayesNet(); ChordalBayesNet cbn = createSmallChordalBayesNet();
boost::shared_ptr<FGConfig> actual = cbn.optimize(); boost::shared_ptr<VectorConfig> actual = cbn.optimize();
FGConfig expected; VectorConfig expected;
Vector x(1), y(1); Vector x(1), y(1);
x(0) = 4; y(0) = 5; x(0) = 4; y(0) = 5;
expected.insert("x",x); expected.insert("x",x);

2
cpp/testConditionalGaussian.cpp
View File

@ -80,7 +80,7 @@ TEST( ConditionalGaussian, solve )
Vector sl1(2); Vector sl1(2);
sl1(0) = 0.5; sl1(1) = 0.8; sl1(0) = 0.5; sl1(1) = 0.8;
FGConfig solution; VectorConfig solution;
solution.insert("x1", sx1); solution.insert("x1", sx1);
solution.insert("l1", sl1); solution.insert("l1", sl1);

10
cpp/testConstrainedConditionalGaussian.cpp
View File

@ -18,7 +18,7 @@ TEST (ConstrainedConditionalGaussian, basic_unary1 )
// check unary constructor that doesn't require an R matrix // check unary constructor that doesn't require an R matrix
// assumed identity matrix // assumed identity matrix
ConstrainedConditionalGaussian unary(v); ConstrainedConditionalGaussian unary(v);
FGConfig fg; VectorConfig fg;
fg.insert("x1", v); fg.insert("x1", v);
CHECK(assert_equal(v, unary.solve(fg))); CHECK(assert_equal(v, unary.solve(fg)));
@ -33,7 +33,7 @@ TEST (ConstrainedConditionalGaussian, basic_unary2 )
Matrix A = eye(2) * 10; Matrix A = eye(2) * 10;
ConstrainedConditionalGaussian unary(10*v, A); ConstrainedConditionalGaussian unary(10*v, A);
FGConfig fg; VectorConfig fg;
fg.insert("x1", v); fg.insert("x1", v);
CHECK(assert_equal(v, unary.solve(fg))); CHECK(assert_equal(v, unary.solve(fg)));
@ -52,7 +52,7 @@ TEST (ConstrainedConditionalGaussian, basic_unary3 )
Vector rhs = A*v; Vector rhs = A*v;
ConstrainedConditionalGaussian unary(rhs, A); ConstrainedConditionalGaussian unary(rhs, A);
FGConfig fg; VectorConfig fg;
fg.insert("x1", v); fg.insert("x1", v);
CHECK(assert_equal(v, unary.solve(fg))); CHECK(assert_equal(v, unary.solve(fg)));
@ -79,7 +79,7 @@ TEST (ConstrainedConditionalGaussian, basic_binary1 )
Vector y = Vector_(2, 1.0, 2.0); Vector y = Vector_(2, 1.0, 2.0);
FGConfig fg; VectorConfig fg;
fg.insert("x1", y); fg.insert("x1", y);
Vector expected = Vector_(2, -3.3333, 0.6667); Vector expected = Vector_(2, -3.3333, 0.6667);
@ -113,7 +113,7 @@ TEST (ConstrainedConditionalGaussian, basic_ternary1 )
Vector y = Vector_(2, 1.0, 2.0); Vector y = Vector_(2, 1.0, 2.0);
Vector z = Vector_(2, 1.0, -1.0); Vector z = Vector_(2, 1.0, -1.0);
FGConfig fg; VectorConfig fg;
fg.insert("x1", y); fg.insert("x1", y);
fg.insert("x2", z); fg.insert("x2", z);

30
cpp/testConstrainedLinearFactorGraph.cpp
View File

@ -67,9 +67,9 @@ TEST( ConstrainedLinearFactorGraph, optimize )
Ordering ord; Ordering ord;
ord.push_back("y"); ord.push_back("y");
ord.push_back("x"); ord.push_back("x");
FGConfig actual = fg.optimize(ord); VectorConfig actual = fg.optimize(ord);
FGConfig expected; VectorConfig expected;
expected.insert("x", Vector_(2, 1.0, -1.0)); expected.insert("x", Vector_(2, 1.0, -1.0));
expected.insert("y", Vector_(2, 0.2, 0.1)); expected.insert("y", Vector_(2, 0.2, 0.1));
@ -88,9 +88,9 @@ TEST( ConstrainedLinearFactorGraph, optimize2 )
Ordering ord; Ordering ord;
ord.push_back("x"); ord.push_back("x");
ord.push_back("y"); ord.push_back("y");
FGConfig actual = fg.optimize(ord); VectorConfig actual = fg.optimize(ord);
FGConfig expected; VectorConfig expected;
expected.insert("x", Vector_(2, 1.0, -1.0)); expected.insert("x", Vector_(2, 1.0, -1.0));
expected.insert("y", Vector_(2, 0.2, 0.1)); expected.insert("y", Vector_(2, 0.2, 0.1));
@ -239,7 +239,7 @@ TEST( ConstrainedLinearFactorGraph, eliminate_multi_constraint )
CHECK(cg3->size() == 0); CHECK(cg3->size() == 0);
// solve piecewise // solve piecewise
FGConfig actual; VectorConfig actual;
Vector act_z = cg3->solve(actual); Vector act_z = cg3->solve(actual);
actual.insert("z", act_z); actual.insert("z", act_z);
CHECK(assert_equal(act_z, Vector_(2, -4.0, 5.0), 1e-4)); CHECK(assert_equal(act_z, Vector_(2, -4.0, 5.0), 1e-4));
@ -261,10 +261,10 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint )
ord.push_back("y"); ord.push_back("y");
ord.push_back("z"); ord.push_back("z");
FGConfig actual = fg.optimize(ord); VectorConfig actual = fg.optimize(ord);
// verify // verify
FGConfig expected; VectorConfig expected;
expected.insert("x", Vector_(2, -2.0, 2.0)); expected.insert("x", Vector_(2, -2.0, 2.0));
expected.insert("y", Vector_(2, -0.1, 0.4)); expected.insert("y", Vector_(2, -0.1, 0.4));
expected.insert("z", Vector_(2, -4.0, 5.0)); expected.insert("z", Vector_(2, -4.0, 5.0));
@ -363,7 +363,7 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint )
// ConstrainedLinearFactorGraph fg1 = createConstrainedLinearFactorGraph(); // ConstrainedLinearFactorGraph fg1 = createConstrainedLinearFactorGraph();
// ConstrainedLinearFactorGraph fg2 = createConstrainedLinearFactorGraph(); // ConstrainedLinearFactorGraph fg2 = createConstrainedLinearFactorGraph();
// //
// FGConfig expected = createConstrainedConfig(); // VectorConfig expected = createConstrainedConfig();
// //
// Ordering ord1; // Ordering ord1;
// ord1.push_back("x0"); // ord1.push_back("x0");
@ -373,8 +373,8 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint )
// ord2.push_back("x1"); // ord2.push_back("x1");
// ord2.push_back("x0"); // ord2.push_back("x0");
// //
// FGConfig actual1 = fg1.optimize(ord1); // VectorConfig actual1 = fg1.optimize(ord1);
// FGConfig actual2 = fg2.optimize(ord2); // VectorConfig actual2 = fg2.optimize(ord2);
// //
// CHECK(actual1.equals(expected)); // CHECK(actual1.equals(expected));
// CHECK(actual1.equals(actual2)); // CHECK(actual1.equals(actual2));
@ -383,7 +383,7 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint )
//TEST (ConstrainedLinearFactorGraph, eliminate ) //TEST (ConstrainedLinearFactorGraph, eliminate )
//{ //{
// ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph(); // ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph();
// FGConfig c = createConstrainedConfig(); // VectorConfig c = createConstrainedConfig();
// //
// Ordering ord1; // Ordering ord1;
// ord1.push_back("x0"); // ord1.push_back("x0");
@ -417,9 +417,9 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint )
// ord.push_back("x1"); // ord.push_back("x1");
// ord.push_back("x2"); // ord.push_back("x2");
// //
// FGConfig actual = clfg.optimize(ord); // VectorConfig actual = clfg.optimize(ord);
// //
// FGConfig expected = lfg.optimize(ord); // should be identical to regular lfg optimize // VectorConfig expected = lfg.optimize(ord); // should be identical to regular lfg optimize
// //
// CHECK(actual.equals(expected)); // CHECK(actual.equals(expected));
//} //}
@ -455,7 +455,7 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint )
//// ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph(); //// ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph();
//// ConstrainedConditionalGaussian::shared_ptr actual = fg.eliminate_constraint("x0"); //// ConstrainedConditionalGaussian::shared_ptr actual = fg.eliminate_constraint("x0");
//// ////
//// FGConfig c = createConstrainedConfig(); //// VectorConfig c = createConstrainedConfig();
//// ConstrainedConditionalGaussian::shared_ptr expected(new ConstrainedConditionalGaussian);//(c["x0"], "x0")); //// ConstrainedConditionalGaussian::shared_ptr expected(new ConstrainedConditionalGaussian);//(c["x0"], "x0"));
//// ////
//// CHECK(assert_equal(*actual, *expected)); // check output for correct delta function //// CHECK(assert_equal(*actual, *expected)); // check output for correct delta function
@ -496,7 +496,7 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint )
// CHECK(actual->size() == 1); // remaining factor will be unary // CHECK(actual->size() == 1); // remaining factor will be unary
// //
// // verify values // // verify values
// FGConfig c = createConstrainedConfig(); // VectorConfig c = createConstrainedConfig();
// Vector exp_v = c["x1"]; // Vector exp_v = c["x1"];
// Matrix A = actual->get_A("x1"); // Matrix A = actual->get_A("x1");
// Vector b = actual->get_b(); // Vector b = actual->get_b();

12
cpp/testConstrainedNonlinearFactorGraph.cpp
View File

@ -12,8 +12,8 @@
using namespace gtsam; using namespace gtsam;
typedef boost::shared_ptr<NonlinearFactor<FGConfig> > shared; typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared;
typedef ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig>,FGConfig> TestGraph; typedef ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorConfig>,VectorConfig> TestGraph;
//TEST( TestGraph, equals ) //TEST( TestGraph, equals )
//{ //{
@ -47,7 +47,7 @@ typedef ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig>,FGConfig> Test
// ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); // ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
// TestGraph cfg(nfg); // TestGraph cfg(nfg);
// //
// FGConfig initial = createNoisyConfig(); // VectorConfig initial = createNoisyConfig();
// ConstrainedLinearFactorGraph linearized = cfg.linearize(initial); // ConstrainedLinearFactorGraph linearized = cfg.linearize(initial);
// LinearFactorGraph lfg = createLinearFactorGraph(); // LinearFactorGraph lfg = createLinearFactorGraph();
// ConstrainedLinearFactorGraph expected(lfg); // ConstrainedLinearFactorGraph expected(lfg);
@ -68,11 +68,11 @@ typedef ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig>,FGConfig> Test
//TEST( TestGraph, linearize_and_solve ) //TEST( TestGraph, linearize_and_solve )
//{ //{
// TestGraph nfg = createConstrainedNonlinearFactorGraph(); // TestGraph nfg = createConstrainedNonlinearFactorGraph();
// FGConfig lin = createConstrainedLinConfig(); // VectorConfig lin = createConstrainedLinConfig();
// ConstrainedLinearFactorGraph actual_lfg = nfg.linearize(lin); // ConstrainedLinearFactorGraph actual_lfg = nfg.linearize(lin);
// FGConfig actual = actual_lfg.optimize(actual_lfg.getOrdering()); // VectorConfig actual = actual_lfg.optimize(actual_lfg.getOrdering());
// //
// FGConfig expected = createConstrainedCorrectDelta(); // VectorConfig expected = createConstrainedCorrectDelta();
// CHECK(actual.equals(expected)); // CHECK(actual.equals(expected));
//} //}

6
cpp/testLinearConstraint.cpp
View File

@ -151,7 +151,7 @@ TEST ( LinearConstraint, eliminate1 )
ConstrainedConditionalGaussian::shared_ptr ccg = lc.eliminate(key); ConstrainedConditionalGaussian::shared_ptr ccg = lc.eliminate(key);
// solve the ccg to get a value // solve the ccg to get a value
FGConfig fg; VectorConfig fg;
CHECK(assert_equal(ccg->solve(fg), v)); CHECK(assert_equal(ccg->solve(fg), v));
} }
@ -176,7 +176,7 @@ TEST ( LinearConstraint, eliminate2 )
Vector y = Vector_(2, 1.0, 2.0); Vector y = Vector_(2, 1.0, 2.0);
FGConfig fg1; VectorConfig fg1;
fg1.insert("y", y); fg1.insert("y", y);
Vector expected = Vector_(2, -3.3333, 0.6667); Vector expected = Vector_(2, -3.3333, 0.6667);
@ -186,7 +186,7 @@ TEST ( LinearConstraint, eliminate2 )
CHECK(assert_equal(expected, actual->solve(fg1), 1e-4)); CHECK(assert_equal(expected, actual->solve(fg1), 1e-4));
// eliminate y to test thrown error // eliminate y to test thrown error
FGConfig fg2; VectorConfig fg2;
fg2.insert("x", expected); fg2.insert("x", expected);
actual = lc.eliminate("y"); actual = lc.eliminate("y");
try { try {

4
cpp/testLinearFactor.cpp
View File

@ -172,7 +172,7 @@ TEST( LinearFactor, error )
LinearFactor::shared_ptr lf = fg[0]; LinearFactor::shared_ptr lf = fg[0];
// check the error of the first factor with nosiy config // check the error of the first factor with nosiy config
FGConfig cfg = createZeroDelta(); VectorConfig cfg = createZeroDelta();
// calculate the error from the factor "f1" // calculate the error from the factor "f1"
// note the error is the same as in testNonlinearFactor // note the error is the same as in testNonlinearFactor
@ -330,7 +330,7 @@ TEST( LinearFactor, eliminate2 )
TEST( LinearFactor, default_error ) TEST( LinearFactor, default_error )
{ {
MutableLinearFactor f; MutableLinearFactor f;
FGConfig c; VectorConfig c;
double actual = f.error(c); double actual = f.error(c);
CHECK(actual==0.0); CHECK(actual==0.0);
} }

6
cpp/testLinearFactorGraph.cpp
View File

@ -31,7 +31,7 @@ TEST( LinearFactorGraph, equals ){
TEST( LinearFactorGraph, error ) TEST( LinearFactorGraph, error )
{ {
LinearFactorGraph fg = createLinearFactorGraph(); LinearFactorGraph fg = createLinearFactorGraph();
FGConfig 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
// zero delta config in the linear graph is equivalent to noisy in // zero delta config in the linear graph is equivalent to noisy in
@ -484,10 +484,10 @@ TEST( LinearFactorGraph, OPTIMIZE )
Ordering ord = fg.getOrdering(); Ordering ord = fg.getOrdering();
// optimize the graph // optimize the graph
FGConfig actual = fg.optimize(ord); VectorConfig actual = fg.optimize(ord);
// verify // verify
FGConfig expected = createCorrectDelta(); VectorConfig expected = createCorrectDelta();
CHECK(actual.equals(expected)); CHECK(actual.equals(expected));
} }

14
cpp/testNonlinearFactor.cpp
View File

@ -17,7 +17,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
typedef boost::shared_ptr<NonlinearFactor<FGConfig> > shared_nlf; typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared_nlf;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonLinearFactor, NonlinearFactor ) TEST( NonLinearFactor, NonlinearFactor )
@ -26,7 +26,7 @@ TEST( NonLinearFactor, NonlinearFactor )
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
// create a configuration for the non linear factor graph // create a configuration for the non linear factor graph
FGConfig cfg = createNoisyConfig(); VectorConfig cfg = createNoisyConfig();
// get the factor "f1" from the factor graph // get the factor "f1" from the factor graph
shared_nlf factor = fg[0]; shared_nlf factor = fg[0];
@ -55,7 +55,7 @@ TEST( NonLinearFactor, linearize_f1 )
boost::static_pointer_cast<NonlinearFactor1>(nfg[0]); boost::static_pointer_cast<NonlinearFactor1>(nfg[0]);
// We linearize at noisy config from SmallExample // We linearize at noisy config from SmallExample
FGConfig c = createNoisyConfig(); VectorConfig c = createNoisyConfig();
LinearFactor::shared_ptr actual = nlf->linearize(c); LinearFactor::shared_ptr actual = nlf->linearize(c);
LinearFactorGraph lfg = createLinearFactorGraph(); LinearFactorGraph lfg = createLinearFactorGraph();
@ -73,7 +73,7 @@ TEST( NonLinearFactor, linearize_f2 )
boost::static_pointer_cast<NonlinearFactor1>(nfg[1]); boost::static_pointer_cast<NonlinearFactor1>(nfg[1]);
// We linearize at noisy config from SmallExample // We linearize at noisy config from SmallExample
FGConfig c = createNoisyConfig(); VectorConfig c = createNoisyConfig();
LinearFactor::shared_ptr actual = nlf->linearize(c); LinearFactor::shared_ptr actual = nlf->linearize(c);
LinearFactorGraph lfg = createLinearFactorGraph(); LinearFactorGraph lfg = createLinearFactorGraph();
@ -91,7 +91,7 @@ TEST( NonLinearFactor, linearize_f3 )
boost::static_pointer_cast<NonlinearFactor1>(nfg[2]); boost::static_pointer_cast<NonlinearFactor1>(nfg[2]);
// We linearize at noisy config from SmallExample // We linearize at noisy config from SmallExample
FGConfig c = createNoisyConfig(); VectorConfig c = createNoisyConfig();
LinearFactor::shared_ptr actual = nlf->linearize(c); LinearFactor::shared_ptr actual = nlf->linearize(c);
LinearFactorGraph lfg = createLinearFactorGraph(); LinearFactorGraph lfg = createLinearFactorGraph();
@ -109,7 +109,7 @@ TEST( NonLinearFactor, linearize_f4 )
boost::static_pointer_cast<NonlinearFactor1>(nfg[3]); boost::static_pointer_cast<NonlinearFactor1>(nfg[3]);
// We linearize at noisy config from SmallExample // We linearize at noisy config from SmallExample
FGConfig c = createNoisyConfig(); VectorConfig c = createNoisyConfig();
LinearFactor::shared_ptr actual = nlf->linearize(c); LinearFactor::shared_ptr actual = nlf->linearize(c);
LinearFactorGraph lfg = createLinearFactorGraph(); LinearFactorGraph lfg = createLinearFactorGraph();
@ -125,7 +125,7 @@ TEST( NonLinearFactor, size )
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
// create a configuration for the non linear factor graph // create a configuration for the non linear factor graph
FGConfig cfg = createNoisyConfig(); VectorConfig cfg = createNoisyConfig();
// get some factors from the graph // get some factors from the graph
shared_nlf factor1 = fg[0]; shared_nlf factor1 = fg[0];

8
cpp/testNonlinearFactorGraph.cpp
View File

@ -33,11 +33,11 @@ TEST( ExampleNonlinearFactorGraph, error )
{ {
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
FGConfig c1 = createConfig(); VectorConfig c1 = createConfig();
double actual1 = fg.error(c1); double actual1 = fg.error(c1);
DOUBLES_EQUAL( 0.0, actual1, 1e-9 ); DOUBLES_EQUAL( 0.0, actual1, 1e-9 );
FGConfig c2 = createNoisyConfig(); VectorConfig c2 = createNoisyConfig();
double actual2 = fg.error(c2); double actual2 = fg.error(c2);
DOUBLES_EQUAL( 5.625, actual2, 1e-9 ); DOUBLES_EQUAL( 5.625, actual2, 1e-9 );
} }
@ -46,7 +46,7 @@ TEST( ExampleNonlinearFactorGraph, error )
TEST( ExampleNonlinearFactorGraph, probPrime ) TEST( ExampleNonlinearFactorGraph, probPrime )
{ {
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
FGConfig cfg = createConfig(); VectorConfig cfg = createConfig();
// evaluate the probability of the factor graph // evaluate the probability of the factor graph
double actual = fg.probPrime(cfg); double actual = fg.probPrime(cfg);
@ -58,7 +58,7 @@ TEST( ExampleNonlinearFactorGraph, probPrime )
TEST( ExampleNonlinearFactorGraph, linearize ) TEST( ExampleNonlinearFactorGraph, linearize )
{ {
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
FGConfig initial = createNoisyConfig(); VectorConfig initial = createNoisyConfig();
LinearFactorGraph linearized = fg.linearize(initial); LinearFactorGraph linearized = fg.linearize(initial);
LinearFactorGraph expected = createLinearFactorGraph(); LinearFactorGraph expected = createLinearFactorGraph();
CHECK(expected.equals(linearized)); CHECK(expected.equals(linearized));

16
cpp/testNonlinearOptimizer.cpp
View File

@ -17,7 +17,7 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
typedef NonlinearOptimizer<ExampleNonlinearFactorGraph,FGConfig> Optimizer; typedef NonlinearOptimizer<ExampleNonlinearFactorGraph,VectorConfig> Optimizer;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearOptimizer, delta ) TEST( NonlinearOptimizer, delta )
@ -27,7 +27,7 @@ TEST( NonlinearOptimizer, delta )
// Expected configuration is the difference between the noisy config // Expected configuration is the difference between the noisy config
// and the ground-truth config. One step only because it's linear ! // and the ground-truth config. One step only because it's linear !
FGConfig expected; VectorConfig expected;
Vector dl1(2); Vector dl1(2);
dl1(0) = -0.1; dl1(0) = -0.1;
dl1(1) = 0.1; dl1(1) = 0.1;
@ -47,7 +47,7 @@ TEST( NonlinearOptimizer, delta )
ord1.push_back("l1"); ord1.push_back("l1");
ord1.push_back("x1"); ord1.push_back("x1");
Optimizer optimizer1(fg, ord1, initial); Optimizer optimizer1(fg, ord1, initial);
FGConfig actual1 = optimizer1.delta(); VectorConfig actual1 = optimizer1.delta();
CHECK(assert_equal(actual1,expected)); CHECK(assert_equal(actual1,expected));
// Check another // Check another
@ -56,7 +56,7 @@ TEST( NonlinearOptimizer, delta )
ord2.push_back("x2"); ord2.push_back("x2");
ord2.push_back("l1"); ord2.push_back("l1");
Optimizer optimizer2(fg, ord2, initial); Optimizer optimizer2(fg, ord2, initial);
FGConfig actual2 = optimizer2.delta(); VectorConfig actual2 = optimizer2.delta();
CHECK(assert_equal(actual2,expected)); CHECK(assert_equal(actual2,expected));
// And yet another... // And yet another...
@ -65,7 +65,7 @@ TEST( NonlinearOptimizer, delta )
ord3.push_back("x1"); ord3.push_back("x1");
ord3.push_back("x2"); ord3.push_back("x2");
Optimizer optimizer3(fg, ord3, initial); Optimizer optimizer3(fg, ord3, initial);
FGConfig actual3 = optimizer3.delta(); VectorConfig actual3 = optimizer3.delta();
CHECK(assert_equal(actual3,expected)); CHECK(assert_equal(actual3,expected));
} }
@ -77,7 +77,7 @@ TEST( NonlinearOptimizer, iterateLM )
// config far from minimum // config far from minimum
Vector x0 = Vector_(1, 3.0); Vector x0 = Vector_(1, 3.0);
boost::shared_ptr<FGConfig> config(new FGConfig); boost::shared_ptr<VectorConfig> config(new VectorConfig);
config->insert("x", x0); config->insert("x", x0);
// ordering // ordering
@ -103,13 +103,13 @@ TEST( NonlinearOptimizer, optimize )
// test error at minimum // test error at minimum
Vector xstar = Vector_(1, 0.0); Vector xstar = Vector_(1, 0.0);
FGConfig cstar; VectorConfig cstar;
cstar.insert("x", xstar); cstar.insert("x", xstar);
DOUBLES_EQUAL(0.0,fg.error(cstar),0.0); DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);
// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
Vector x0 = Vector_(1, 3.0); Vector x0 = Vector_(1, 3.0);
boost::shared_ptr<FGConfig> c0(new FGConfig); boost::shared_ptr<VectorConfig> c0(new VectorConfig);
c0->insert("x", x0); c0->insert("x", x0);
DOUBLES_EQUAL(199.0,fg.error(*c0),1e-3); DOUBLES_EQUAL(199.0,fg.error(*c0),1e-3);

34
cpp/testFGConfig.cpp → cpp/testVectorConfig.cpp
View File

@ -1,5 +1,5 @@
/** /**
* @file testFGConfig.cpp * @file testVectorConfig.cpp
* @brief Unit tests for Factor Graph Configuration * @brief Unit tests for Factor Graph Configuration
* @author Carlos Nieto * @author Carlos Nieto
**/ **/
@ -17,27 +17,27 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include "Matrix.h" #include "Matrix.h"
#include "FGConfig.h" #include "VectorConfig.h"
#include "smallExample.cpp" #include "smallExample.cpp"
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( FGConfig, equals ) TEST( VectorConfig, equals )
{ {
FGConfig expected; VectorConfig expected;
Vector v = Vector_(3, 5.0, 6.0, 7.0); Vector v = Vector_(3, 5.0, 6.0, 7.0);
expected.insert("a",v); expected.insert("a",v);
FGConfig actual; VectorConfig actual;
actual.insert("a",v); actual.insert("a",v);
CHECK(actual.equals(expected)); CHECK(actual.equals(expected));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( FGConfig, contains) TEST( VectorConfig, contains)
{ {
FGConfig fg; VectorConfig fg;
Vector v = Vector_(3, 5.0, 6.0, 7.0); Vector v = Vector_(3, 5.0, 6.0, 7.0);
fg.insert("ali", v); fg.insert("ali", v);
CHECK(fg.contains("ali")); CHECK(fg.contains("ali"));
@ -45,34 +45,34 @@ TEST( FGConfig, contains)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( FGConfig, plus) TEST( VectorConfig, plus)
{ {
FGConfig fg; VectorConfig fg;
Vector vx = Vector_(3, 5.0, 6.0, 7.0), vy = Vector_(2, 8.0, 9.0); Vector vx = Vector_(3, 5.0, 6.0, 7.0), vy = Vector_(2, 8.0, 9.0);
fg.insert("x", vx).insert("y",vy); fg.insert("x", vx).insert("y",vy);
FGConfig delta; VectorConfig delta;
Vector dx = Vector_(3, 1.0, 1.0, 1.0), dy = Vector_(2, -1.0, -1.0); Vector dx = Vector_(3, 1.0, 1.0, 1.0), dy = Vector_(2, -1.0, -1.0);
delta.insert("x", dx).insert("y",dy); delta.insert("x", dx).insert("y",dy);
FGConfig expected; VectorConfig expected;
Vector wx = Vector_(3, 6.0, 7.0, 8.0), wy = Vector_(2, 7.0, 8.0); Vector wx = Vector_(3, 6.0, 7.0, 8.0), wy = Vector_(2, 7.0, 8.0);
expected.insert("x", wx).insert("y",wy); expected.insert("x", wx).insert("y",wy);
// functional // functional
FGConfig actual = fg.exmap(delta); VectorConfig actual = fg.exmap(delta);
CHECK(actual.equals(expected)); CHECK(actual.equals(expected));
} }
/* ************************************************************************* */ /* ************************************************************************* */
#ifdef HAVE_BOOST_SERIALIZATION #ifdef HAVE_BOOST_SERIALIZATION
TEST( FGConfig, serialize) TEST( VectorConfig, serialize)
{ {
//DEBUG: //DEBUG:
cout << "FGConfig: Running Serialization Test" << endl; cout << "VectorConfig: Running Serialization Test" << endl;
//create an FGConfig //create an VectorConfig
FGConfig fg = createConfig(); VectorConfig fg = createConfig();
//serialize the config //serialize the config
std::ostringstream in_archive_stream; std::ostringstream in_archive_stream;
@ -83,7 +83,7 @@ TEST( FGConfig, serialize)
//deserialize the config //deserialize the config
std::istringstream out_archive_stream(serialized_fgc); std::istringstream out_archive_stream(serialized_fgc);
boost::archive::text_iarchive out_archive(out_archive_stream); boost::archive::text_iarchive out_archive(out_archive_stream);
FGConfig output; VectorConfig output;
out_archive >> output; out_archive >> output;
//check for equality //check for equality