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
/* ************************************************************************* */
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);
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;
/** 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 "ConditionalGaussian.h"
#include "FGConfig.h"
#include "VectorConfig.h"
namespace gtsam {
@ -72,8 +72,8 @@ public:
const_iterator const end() const {return nodes.end();}
/** optimize */
boost::shared_ptr<FGConfig> optimize() const;
boost::shared_ptr<FGConfig> optimize(const boost::shared_ptr<FGConfig> &c) const;
boost::shared_ptr<VectorConfig> optimize() const;
boost::shared_ptr<VectorConfig> optimize(const boost::shared_ptr<VectorConfig> &c) const;
/** print */
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_;
for (map<string, Matrix>::const_iterator it = parents_.begin(); it
!= parents_.end(); it++) {

4
cpp/ConditionalGaussian.h
View File

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

2
cpp/ConstrainedConditionalGaussian.cpp
View File

@ -48,7 +48,7 @@ ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const ConstrainedConditionalGaussian& df) {
}
Vector ConstrainedConditionalGaussian::solve(const FGConfig& x) const {
Vector ConstrainedConditionalGaussian::solve(const VectorConfig& x) const {
// sum the RHS
Vector rhs = d_;
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
* @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);
boost::shared_ptr<FGConfig> newConfig = cbn->optimize();
boost::shared_ptr<VectorConfig> newConfig = cbn->optimize();
return *newConfig;
}

2
cpp/ConstrainedLinearFactorGraph.h
View File

@ -114,7 +114,7 @@ public:
* optimize a linear factor graph
* @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

4
cpp/ConstrainedNonlinearFactorGraph.h
View File

@ -19,8 +19,8 @@ namespace gtsam {
* A nonlinear factor graph with the addition of equality constraints.
*
* Templated on factor and configuration type.
* TODO FD: this is totally wrong: it can only work if Config==FGConfig
* as LinearConstraint is only defined for FGConfig
* TODO FD: this is totally wrong: it can only work if Config==VectorConfig
* as LinearConstraint is only defined for VectorConfig
* To fix it, we need to think more deeply about this.
*/
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
* 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
* variables, continuous ones, or a combination of both. It is up to the config to
* provide the appropriate values at the appropriate time.

2
cpp/FactorGraph.h
View File

@ -16,7 +16,7 @@
namespace gtsam {
class Ordering;
class FGConfig;
class VectorConfig;
class LinearFactor;
class LinearFactorGraph;
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
double LinearFactor::error(const FGConfig& c) const {
double LinearFactor::error(const VectorConfig& c) const {
if (empty()) return 0;
Vector e = b;
string j; Matrix Aj;

4
cpp/LinearFactor.h
View File

@ -32,7 +32,7 @@ class MutableLinearFactor;
* LinearFactor is non-mutable (all methods const!).
* The factor value is exp(-0.5*||Ax-b||^2)
*/
class LinearFactor: public Factor<FGConfig> {
class LinearFactor: public Factor<VectorConfig> {
public:
typedef boost::shared_ptr<LinearFactor> shared_ptr;
@ -98,7 +98,7 @@ public:
// 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;
bool equals(const LinearFactor& lf, double tol = 1e-9) const;
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 */
/* ************************************************************************* */
FGConfig LinearFactorGraph::optimize(const Ordering& ordering)
VectorConfig LinearFactorGraph::optimize(const Ordering& ordering)
{
// eliminate all nodes in the given ordering -> chordal Bayes net
ChordalBayesNet::shared_ptr chordalBayesNet = eliminate(ordering);
// calculate new configuration (using backsubstitution)
boost::shared_ptr<FGConfig> newConfig = chordalBayesNet->optimize();
boost::shared_ptr<VectorConfig> newConfig = chordalBayesNet->optimize();
return *newConfig;
}

8
cpp/LinearFactorGraph.h
View File

@ -16,7 +16,7 @@
#include "LinearFactor.h"
//#include "Ordering.h"
#include "FGConfig.h"
#include "VectorConfig.h"
#include "FactorGraph.h"
namespace gtsam {
@ -26,10 +26,10 @@ namespace gtsam {
/**
* A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
* 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.
*/
class LinearFactorGraph : public FactorGraph<LinearFactor, FGConfig> {
class LinearFactorGraph : public FactorGraph<LinearFactor, VectorConfig> {
public:
/**
@ -92,7 +92,7 @@ namespace gtsam {
* optimize a linear factor graph
* @param ordering fg in order
*/
FGConfig optimize(const Ordering& ordering);
VectorConfig optimize(const Ordering& ordering);
/**
* 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
# 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
check_PROGRAMS += testFGConfig testLinearFactor testConditionalGaussian testNonlinearFactor testLinearConstraint testConstrainedConditionalGaussian
check_PROGRAMS += testVectorConfig testLinearFactor testConditionalGaussian testNonlinearFactor testLinearConstraint testConstrainedConditionalGaussian
example = smallExample.cpp
testFGConfig_SOURCES = testFGConfig.cpp
testVectorConfig_SOURCES = testVectorConfig.cpp
testLinearFactor_SOURCES = $(example) testLinearFactor.cpp
testConditionalGaussian_SOURCES = $(example) testConditionalGaussian.cpp
testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp
testLinearConstraint_SOURCES = $(example) testLinearConstraint.cpp
testConstrainedConditionalGaussian_SOURCES = testConstrainedConditionalGaussian.cpp
testFGConfig_LDADD = libgtsam.la
testVectorConfig_LDADD = libgtsam.la
testLinearFactor_LDADD = libgtsam.la
testConditionalGaussian_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&),
const string& key1,
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);
}
@ -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
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 */
/* ************************************************************************* */
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);
if (p == NULL) return false;
return NonlinearFactor<FGConfig>::equals(*p, tol)
return NonlinearFactor<VectorConfig>::equals(*p, tol)
&& (h_ == p->h_)
&& (key1_== p->key1_)
&& (H_ == p->H_);
@ -65,7 +65,7 @@ NonlinearFactor2::NonlinearFactor2(const Vector& z,
const string& key2,
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(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
Vector x1 = c[key1_];
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);
if (p == NULL) return false;
return NonlinearFactor<FGConfig>::equals(*p, tol)
return NonlinearFactor<VectorConfig>::equals(*p, tol)
&& (h_ == p->h_)
&& (key1_ == p->key1_)
&& (H2_ == p->H1_)

16
cpp/NonlinearFactor.h
View File

@ -113,7 +113,7 @@ namespace gtsam {
* Note: cannot be serialized as contains function pointers
* Specialized derived classes could do this
*/
class NonlinearFactor1 : public NonlinearFactor<FGConfig> {
class NonlinearFactor1 : public NonlinearFactor<VectorConfig> {
private:
std::string key1_;
@ -133,15 +133,15 @@ namespace gtsam {
void print(const std::string& s = "") const;
/** error function */
inline Vector error_vector(const FGConfig& c) const {
inline Vector error_vector(const VectorConfig& c) const {
return z_ - h_(c[key1_]);
}
/** 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 */
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 "";}
};
@ -151,7 +151,7 @@ namespace gtsam {
* Note: cannot be serialized as contains function pointers
* Specialized derived classes could do this
*/
class NonlinearFactor2 : public NonlinearFactor<FGConfig> {
class NonlinearFactor2 : public NonlinearFactor<VectorConfig> {
private:
@ -176,15 +176,15 @@ namespace gtsam {
void print(const std::string& s = "") const;
/** 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_]);
}
/** 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 */
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 "";};
};

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.
* 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
* 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>
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<Config> ,Config> {

6
cpp/NonlinearOptimizer-inl.h
View File

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

6
cpp/NonlinearOptimizer.h
View File

@ -10,7 +10,7 @@
#include <boost/shared_ptr.hpp>
#include "NonlinearFactorGraph.h"
#include "FGConfig.h"
#include "VectorConfig.h"
namespace gtsam {
@ -96,9 +96,9 @@ namespace gtsam {
/**
* 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

2
cpp/VSLAMFactor.h
View File

@ -8,7 +8,7 @@
#include "NonlinearFactor.h"
#include "LinearFactor.h"
#include "FGConfig.h"
#include "VectorConfig.h"
#include "Cal3_S2.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 fgConfig
* @brief VectorConfig
* @author Carlos Nieto
* @author Christian Potthast
*/
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include "FGConfig.h"
#include "VectorConfig.h"
// trick from some reading group
#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 << "vj.size = " << vj.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++) {
string j = it->first;
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);
if (it==values.end()) {
print();
cout << "asked for key " << name << endl;
throw(std::invalid_argument("FGConfig::[] invalid key"));
throw(std::invalid_argument("VectorConfig::[] invalid key"));
}
return it->second;
}
/* ************************************************************************* */
void FGConfig::print(const std::string& name) const {
odprintf("FGConfig %s\n", name.c_str());
void VectorConfig::print(const std::string& name) const {
odprintf("VectorConfig %s\n", name.c_str());
odprintf("size: %d\n", values.size());
string j; Vector v;
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;
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
* @author Carlos Nieto
* @author Christian Potthast
@ -18,7 +18,7 @@
namespace gtsam {
/** Factor Graph Configuration */
class FGConfig : public Testable<FGConfig> {
class VectorConfig : public Testable<VectorConfig> {
protected:
/** 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>::const_iterator const_iterator;
FGConfig():Testable<FGConfig>() {}
FGConfig(const FGConfig& cfg_in) : Testable<FGConfig>(), values(cfg_in.values) {}
VectorConfig():Testable<VectorConfig>() {}
VectorConfig(const VectorConfig& cfg_in) : Testable<VectorConfig>(), values(cfg_in.values) {}
virtual ~FGConfig() {}
virtual ~VectorConfig() {}
/** return all the nodes in the graph **/
std::vector<std::string> get_names() const {
@ -42,16 +42,16 @@ namespace gtsam {
}
/** 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));
return *this;
}
/**
* 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 end() const {return values.end();}
@ -78,7 +78,7 @@ namespace gtsam {
void print(const std::string& name = "") const;
/** 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();}

36
cpp/gtsam.h
View File

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

42
cpp/smallExample.cpp
View File

@ -26,7 +26,7 @@ using namespace std;
namespace gtsam {
typedef boost::shared_ptr<NonlinearFactor<FGConfig> > shared;
typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared;
/* ************************************************************************* */
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_x2(2); v_x2(0) = 1.5; v_x2(1) = 0.;
Vector v_l1(2); v_l1(0) = 0.; v_l1(1) = -1.;
FGConfig c;
VectorConfig c;
c.insert("x1", v_x1);
c.insert("x2", v_x2);
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_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;
boost::shared_ptr<FGConfig> c(new FGConfig);
boost::shared_ptr<VectorConfig> c(new VectorConfig);
c->insert("x1", v_x1);
c->insert("x2", v_x2);
c->insert("l1", v_l1);
@ -92,16 +92,16 @@ boost::shared_ptr<const FGConfig> sharedNoisyConfig()
}
/* ************************************************************************* */
FGConfig createNoisyConfig() {
VectorConfig createNoisyConfig() {
return *sharedNoisyConfig();
}
/* ************************************************************************* */
FGConfig createCorrectDelta() {
VectorConfig createCorrectDelta() {
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_l1(2); v_l1(0) = -0.1; v_l1(1) = 0.1;
FGConfig c;
VectorConfig c;
c.insert("x1", v_x1);
c.insert("x2", v_x2);
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_x2(2); v_x2(0) = 0; v_x2(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("x2", v_x2);
c.insert("l1", v_l1);
@ -123,7 +123,7 @@ FGConfig createZeroDelta() {
/* ************************************************************************* */
LinearFactorGraph createLinearFactorGraph()
{
FGConfig c = createNoisyConfig();
VectorConfig c = createNoisyConfig();
// Create
LinearFactorGraph fg;
@ -333,9 +333,9 @@ ConstrainedLinearFactorGraph createMultiConstraintGraph() {
//}
/* ************************************************************************* */
// ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig> , FGConfig> createConstrainedNonlinearFactorGraph() {
// ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig> , FGConfig> graph;
// FGConfig c = createConstrainedConfig();
// ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorConfig> , VectorConfig> createConstrainedNonlinearFactorGraph() {
// ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorConfig> , VectorConfig> graph;
// VectorConfig c = createConstrainedConfig();
//
// // equality constraint for initial pose
// 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;
// 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
// 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.;
// config.insert("x0", x0);
@ -394,7 +394,7 @@ ConstrainedLinearFactorGraph createMultiConstraintGraph() {
//ConstrainedChordalBayesNet createConstrainedChordalBayesNet()
//{
// ConstrainedChordalBayesNet cbn;
// FGConfig c = createConstrainedConfig();
// VectorConfig c = createConstrainedConfig();
//
// // add regular conditional gaussian - no parent
// Matrix R = eye(2);

22
cpp/smallExample.h
View File

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

4
cpp/testChordalBayesNet.cpp
View File

@ -69,9 +69,9 @@ TEST( ChordalBayesNet, optimize )
{
// optimize small Bayes Net
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);
x(0) = 4; y(0) = 5;
expected.insert("x",x);

2
cpp/testConditionalGaussian.cpp
View File

@ -80,7 +80,7 @@ TEST( ConditionalGaussian, solve )
Vector sl1(2);
sl1(0) = 0.5; sl1(1) = 0.8;
FGConfig solution;
VectorConfig solution;
solution.insert("x1", sx1);
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
// assumed identity matrix
ConstrainedConditionalGaussian unary(v);
FGConfig fg;
VectorConfig fg;
fg.insert("x1", v);
CHECK(assert_equal(v, unary.solve(fg)));
@ -33,7 +33,7 @@ TEST (ConstrainedConditionalGaussian, basic_unary2 )
Matrix A = eye(2) * 10;
ConstrainedConditionalGaussian unary(10*v, A);
FGConfig fg;
VectorConfig fg;
fg.insert("x1", v);
CHECK(assert_equal(v, unary.solve(fg)));
@ -52,7 +52,7 @@ TEST (ConstrainedConditionalGaussian, basic_unary3 )
Vector rhs = A*v;
ConstrainedConditionalGaussian unary(rhs, A);
FGConfig fg;
VectorConfig fg;
fg.insert("x1", v);
CHECK(assert_equal(v, unary.solve(fg)));
@ -79,7 +79,7 @@ TEST (ConstrainedConditionalGaussian, basic_binary1 )
Vector y = Vector_(2, 1.0, 2.0);
FGConfig fg;
VectorConfig fg;
fg.insert("x1", y);
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 z = Vector_(2, 1.0, -1.0);
FGConfig fg;
VectorConfig fg;
fg.insert("x1", y);
fg.insert("x2", z);

30
cpp/testConstrainedLinearFactorGraph.cpp
View File

@ -67,9 +67,9 @@ TEST( ConstrainedLinearFactorGraph, optimize )
Ordering ord;
ord.push_back("y");
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("y", Vector_(2, 0.2, 0.1));
@ -88,9 +88,9 @@ TEST( ConstrainedLinearFactorGraph, optimize2 )
Ordering ord;
ord.push_back("x");
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("y", Vector_(2, 0.2, 0.1));
@ -239,7 +239,7 @@ TEST( ConstrainedLinearFactorGraph, eliminate_multi_constraint )
CHECK(cg3->size() == 0);
// solve piecewise
FGConfig actual;
VectorConfig actual;
Vector act_z = cg3->solve(actual);
actual.insert("z", act_z);
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("z");
FGConfig actual = fg.optimize(ord);
VectorConfig actual = fg.optimize(ord);
// verify
FGConfig expected;
VectorConfig expected;
expected.insert("x", Vector_(2, -2.0, 2.0));
expected.insert("y", Vector_(2, -0.1, 0.4));
expected.insert("z", Vector_(2, -4.0, 5.0));
@ -363,7 +363,7 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint )
// ConstrainedLinearFactorGraph fg1 = createConstrainedLinearFactorGraph();
// ConstrainedLinearFactorGraph fg2 = createConstrainedLinearFactorGraph();
//
// FGConfig expected = createConstrainedConfig();
// VectorConfig expected = createConstrainedConfig();
//
// Ordering ord1;
// ord1.push_back("x0");
@ -373,8 +373,8 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint )
// ord2.push_back("x1");
// ord2.push_back("x0");
//
// FGConfig actual1 = fg1.optimize(ord1);
// FGConfig actual2 = fg2.optimize(ord2);
// VectorConfig actual1 = fg1.optimize(ord1);
// VectorConfig actual2 = fg2.optimize(ord2);
//
// CHECK(actual1.equals(expected));
// CHECK(actual1.equals(actual2));
@ -383,7 +383,7 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint )
//TEST (ConstrainedLinearFactorGraph, eliminate )
//{
// ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph();
// FGConfig c = createConstrainedConfig();
// VectorConfig c = createConstrainedConfig();
//
// Ordering ord1;
// ord1.push_back("x0");
@ -417,9 +417,9 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint )
// ord.push_back("x1");
// 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));
//}
@ -455,7 +455,7 @@ TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint )
//// ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph();
//// ConstrainedConditionalGaussian::shared_ptr actual = fg.eliminate_constraint("x0");
////
//// FGConfig c = createConstrainedConfig();
//// VectorConfig c = createConstrainedConfig();
//// ConstrainedConditionalGaussian::shared_ptr expected(new ConstrainedConditionalGaussian);//(c["x0"], "x0"));
////
//// 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
//
// // verify values
// FGConfig c = createConstrainedConfig();
// VectorConfig c = createConstrainedConfig();
// Vector exp_v = c["x1"];
// Matrix A = actual->get_A("x1");
// Vector b = actual->get_b();

12
cpp/testConstrainedNonlinearFactorGraph.cpp
View File

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

6
cpp/testLinearConstraint.cpp
View File

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

4
cpp/testLinearFactor.cpp
View File

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

6
cpp/testLinearFactorGraph.cpp
View File

@ -31,7 +31,7 @@ TEST( LinearFactorGraph, equals ){
TEST( LinearFactorGraph, error )
{
LinearFactorGraph fg = createLinearFactorGraph();
FGConfig cfg = createZeroDelta();
VectorConfig cfg = createZeroDelta();
// note the error is the same as in testNonlinearFactorGraph as a
// zero delta config in the linear graph is equivalent to noisy in
@ -484,10 +484,10 @@ TEST( LinearFactorGraph, OPTIMIZE )
Ordering ord = fg.getOrdering();
// optimize the graph
FGConfig actual = fg.optimize(ord);
VectorConfig actual = fg.optimize(ord);
// verify
FGConfig expected = createCorrectDelta();
VectorConfig expected = createCorrectDelta();
CHECK(actual.equals(expected));
}

14
cpp/testNonlinearFactor.cpp
View File

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

8
cpp/testNonlinearFactorGraph.cpp
View File

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

16
cpp/testNonlinearOptimizer.cpp
View File

@ -17,7 +17,7 @@ using namespace std;
using namespace gtsam;
typedef NonlinearOptimizer<ExampleNonlinearFactorGraph,FGConfig> Optimizer;
typedef NonlinearOptimizer<ExampleNonlinearFactorGraph,VectorConfig> Optimizer;
/* ************************************************************************* */
TEST( NonlinearOptimizer, delta )
@ -27,7 +27,7 @@ TEST( NonlinearOptimizer, delta )
// Expected configuration is the difference between the noisy config
// and the ground-truth config. One step only because it's linear !
FGConfig expected;
VectorConfig expected;
Vector dl1(2);
dl1(0) = -0.1;
dl1(1) = 0.1;
@ -47,7 +47,7 @@ TEST( NonlinearOptimizer, delta )
ord1.push_back("l1");
ord1.push_back("x1");
Optimizer optimizer1(fg, ord1, initial);
FGConfig actual1 = optimizer1.delta();
VectorConfig actual1 = optimizer1.delta();
CHECK(assert_equal(actual1,expected));
// Check another
@ -56,7 +56,7 @@ TEST( NonlinearOptimizer, delta )
ord2.push_back("x2");
ord2.push_back("l1");
Optimizer optimizer2(fg, ord2, initial);
FGConfig actual2 = optimizer2.delta();
VectorConfig actual2 = optimizer2.delta();
CHECK(assert_equal(actual2,expected));
// And yet another...
@ -65,7 +65,7 @@ TEST( NonlinearOptimizer, delta )
ord3.push_back("x1");
ord3.push_back("x2");
Optimizer optimizer3(fg, ord3, initial);
FGConfig actual3 = optimizer3.delta();
VectorConfig actual3 = optimizer3.delta();
CHECK(assert_equal(actual3,expected));
}
@ -77,7 +77,7 @@ TEST( NonlinearOptimizer, iterateLM )
// config far from minimum
Vector x0 = Vector_(1, 3.0);
boost::shared_ptr<FGConfig> config(new FGConfig);
boost::shared_ptr<VectorConfig> config(new VectorConfig);
config->insert("x", x0);
// ordering
@ -103,13 +103,13 @@ TEST( NonlinearOptimizer, optimize )
// test error at minimum
Vector xstar = Vector_(1, 0.0);
FGConfig cstar;
VectorConfig cstar;
cstar.insert("x", xstar);
DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);
// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
Vector x0 = Vector_(1, 3.0);
boost::shared_ptr<FGConfig> c0(new FGConfig);
boost::shared_ptr<VectorConfig> c0(new VectorConfig);
c0->insert("x", x0);
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
* @author Carlos Nieto
**/
@ -17,27 +17,27 @@
#include <CppUnitLite/TestHarness.h>
#include "Matrix.h"
#include "FGConfig.h"
#include "VectorConfig.h"
#include "smallExample.cpp"
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST( FGConfig, equals )
TEST( VectorConfig, equals )
{
FGConfig expected;
VectorConfig expected;
Vector v = Vector_(3, 5.0, 6.0, 7.0);
expected.insert("a",v);
FGConfig actual;
VectorConfig actual;
actual.insert("a",v);
CHECK(actual.equals(expected));
}
/* ************************************************************************* */
TEST( FGConfig, contains)
TEST( VectorConfig, contains)
{
FGConfig fg;
VectorConfig fg;
Vector v = Vector_(3, 5.0, 6.0, 7.0);
fg.insert("ali", v);
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);
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);
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);
expected.insert("x", wx).insert("y",wy);
// functional
FGConfig actual = fg.exmap(delta);
VectorConfig actual = fg.exmap(delta);
CHECK(actual.equals(expected));
}
/* ************************************************************************* */
#ifdef HAVE_BOOST_SERIALIZATION
TEST( FGConfig, serialize)
TEST( VectorConfig, serialize)
{
//DEBUG:
cout << "FGConfig: Running Serialization Test" << endl;
cout << "VectorConfig: Running Serialization Test" << endl;
//create an FGConfig
FGConfig fg = createConfig();
//create an VectorConfig
VectorConfig fg = createConfig();
//serialize the config
std::ostringstream in_archive_stream;
@ -83,7 +83,7 @@ TEST( FGConfig, serialize)
//deserialize the config
std::istringstream out_archive_stream(serialized_fgc);
boost::archive::text_iarchive out_archive(out_archive_stream);
FGConfig output;
VectorConfig output;
out_archive >> output;
//check for equality