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

'''BIG CHANGE''': avoid converting back and to FGConfigs by templating on configuration type. Details: 

* Factors are now templated on the configuration type. Factor Graphs are now templated on the factor type and configuration type.
 * LinearFactor is a factor on an FGConfig.
 * LinearFactorGraph uses LinearFactor and FGConfig.
 * NonLinearFactor is still templated on Config.
 * NonLinearFactorGraph uses NonLinearFactors, but is still templated on Config.
 * Tests and VSLAMFactor have been updated to reflect those changes.
release/4.3a0
Frank Dellaert 2009-10-06 18:25:04 +00:00
parent 34c1bb6f29
commit 989f290c99
27 changed files with 386 additions and 392 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
.cproject
View File

@ -1,4 +1,4 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?> <?fileVersion 4.0.0?>
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage"> <cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
@ -300,7 +300,6 @@
<buildTargets> <buildTargets>
<target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>check</buildTarget> <buildTarget>check</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -308,6 +307,7 @@
</target> </target>
<target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimpleCamera.run</buildTarget> <buildTarget>testSimpleCamera.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -315,14 +315,14 @@
</target> </target>
<target name="testCal3_S2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testCal3_S2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-f local.mk</buildArguments>
<buildTarget>testCal3_S2.run</buildTarget> <buildTarget>testCal3_S2.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>testVSLAMFactor.run</buildTarget> <buildTarget>testVSLAMFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -330,6 +330,7 @@
</target> </target>
<target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testCalibratedCamera.run</buildTarget> <buildTarget>testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -337,7 +338,6 @@
</target> </target>
<target name="testConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>testConditionalGaussian.run</buildTarget> <buildTarget>testConditionalGaussian.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -345,6 +345,7 @@
</target> </target>
<target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose2.run</buildTarget> <buildTarget>testPose2.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -352,14 +353,15 @@
</target> </target>
<target name="testFGConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testFGConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments></buildArguments> <buildArguments>-f local.mk</buildArguments>
<buildTarget>testFGConfig.run</buildTarget> <buildTarget>testFGConfig.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testRot3.run</buildTarget> <buildTarget>testRot3.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -367,7 +369,6 @@
</target> </target>
<target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>testNonlinearOptimizer.run</buildTarget> <buildTarget>testNonlinearOptimizer.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -375,22 +376,45 @@
</target> </target>
<target name="testLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testLinearFactor.run</buildTarget> <buildTarget>testLinearFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testConstrainedNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testConstrainedNonlinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments></buildArguments> <buildArguments/>
<buildTarget>testLinearFactorGraph.run</buildTarget> <buildTarget>testLinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testNonlinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>install</buildTarget> <buildTarget>install</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -398,7 +422,6 @@
</target> </target>
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>clean</buildTarget> <buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -406,7 +429,6 @@
</target> </target>
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>check</buildTarget> <buildTarget>check</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>

45
cpp/ConstrainedNonlinearFactorGraph.cpp
View File

@ -7,48 +7,3 @@
#include "ConstrainedNonlinearFactorGraph.h" #include "ConstrainedNonlinearFactorGraph.h"
namespace gtsam {
ConstrainedNonlinearFactorGraph::ConstrainedNonlinearFactorGraph()
{
}
ConstrainedNonlinearFactorGraph::ConstrainedNonlinearFactorGraph(
const NonlinearFactorGraph& nfg)
: NonlinearFactorGraph(nfg)
{
}
ConstrainedNonlinearFactorGraph::~ConstrainedNonlinearFactorGraph()
{
}
ConstrainedLinearFactorGraph ConstrainedNonlinearFactorGraph::linearize(const FGConfig& config) const
{
ConstrainedLinearFactorGraph ret;
// linearize all nonlinear factors
for(const_iterator factor=factors_.begin(); factor<factors_.end(); factor++){
LinearFactor::shared_ptr lf = (*factor)->linearize(config);
ret.push_back(lf);
}
// linearize the equality factors (set to zero because they are now in delta space)
for(eq_const_iterator e_factor=eq_factors.begin(); e_factor<eq_factors.end(); e_factor++){
EqualityFactor::shared_ptr eq = (*e_factor)->linearize();
ret.push_back_eq(eq);
}
return ret;
}
NonlinearFactorGraph ConstrainedNonlinearFactorGraph::convert() const
{
NonlinearFactorGraph ret;
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> f, factors_)
ret.push_back(f);
return ret;
}
}

55
cpp/ConstrainedNonlinearFactorGraph.h
View File

@ -8,39 +8,78 @@
#ifndef CONSTRAINEDNONLINEARFACTORGRAPH_H_ #ifndef CONSTRAINEDNONLINEARFACTORGRAPH_H_
#define CONSTRAINEDNONLINEARFACTORGRAPH_H_ #define CONSTRAINEDNONLINEARFACTORGRAPH_H_
#include <boost/shared_ptr.hpp>
#include "NonlinearFactorGraph.h" #include "NonlinearFactorGraph.h"
#include "EqualityFactor.h" #include "EqualityFactor.h"
#include "ConstrainedLinearFactorGraph.h" #include "ConstrainedLinearFactorGraph.h"
namespace gtsam { namespace gtsam {
class ConstrainedNonlinearFactorGraph: public NonlinearFactorGraph { /**
* 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 EqualityFactor is only defined for FGConfig
* To fix it, we need to think more deeply about this.
*/
template<class Factor, class Config>
class ConstrainedNonlinearFactorGraph: public FactorGraph<Factor, Config> {
protected: protected:
/** collection of equality factors */ /** collection of equality factors */
std::vector<EqualityFactor::shared_ptr> eq_factors; std::vector<EqualityFactor::shared_ptr> eq_factors;
public: public:
// iterators over equality factors // iterators over equality factors
typedef std::vector<EqualityFactor::shared_ptr>::const_iterator eq_const_iterator; typedef std::vector<EqualityFactor::shared_ptr>::const_iterator eq_const_iterator;
typedef std::vector<EqualityFactor::shared_ptr>::iterator eq_iterator; typedef std::vector<EqualityFactor::shared_ptr>::iterator eq_iterator;
/** /**
* Default constructor * Default constructor
*/ */
ConstrainedNonlinearFactorGraph(); ConstrainedNonlinearFactorGraph() {
}
/** /**
* Copy constructor from regular NLFGs * Copy constructor from regular NLFGs
*/ */
ConstrainedNonlinearFactorGraph(const NonlinearFactorGraph& nfg); ConstrainedNonlinearFactorGraph(const NonlinearFactorGraph<Config>& nfg) :
FactorGraph<Factor, Config> (nfg) {
}
virtual ~ConstrainedNonlinearFactorGraph(); typedef typename boost::shared_ptr<Factor> shared_factor;
typedef typename std::vector<shared_factor>::const_iterator const_iterator;
/** /**
* Linearize a nonlinear graph to produce a linear graph * Linearize a nonlinear graph to produce a linear graph
* Note that equality constraints will just pass through * Note that equality constraints will just pass through
*/ */
ConstrainedLinearFactorGraph linearize(const FGConfig& initial) const; ConstrainedLinearFactorGraph linearize(const Config& config) const {
ConstrainedLinearFactorGraph ret;
// linearize all nonlinear factors
// TODO uncomment
for (const_iterator factor = this->factors_.begin(); factor < this->factors_.end(); factor++) {
LinearFactor::shared_ptr lf = (*factor)->linearize(config);
ret.push_back(lf);
}
// linearize the equality factors (set to zero because they are now in delta space)
for (eq_const_iterator e_factor = eq_factors.begin(); e_factor
< eq_factors.end(); e_factor++) {
EqualityFactor::shared_ptr eq = (*e_factor)->linearize();
ret.push_back_eq(eq);
}
return ret;
}
/**
* Insert a factor into the graph
*/
void push_back(const shared_factor& f) {
FactorGraph<Factor,Config>::push_back(f);
}
/** /**
* Insert a equality factor into the graph * Insert a equality factor into the graph
@ -49,10 +88,6 @@ public:
eq_factors.push_back(eq); eq_factors.push_back(eq);
} }
/**
* converts the graph to a regular nonlinear graph - removes equality constraints
*/
NonlinearFactorGraph convert() const;
}; };
} }

7
cpp/EqualityFactor.cpp
View File

@ -42,13 +42,6 @@ bool EqualityFactor::equals(const EqualityFactor& f, double tol) const
return equal_with_abs_tol(value_, f.get_value(), tol) && key_ == f.get_key(); return equal_with_abs_tol(value_, f.get_value(), tol) && key_ == f.get_key();
} }
bool EqualityFactor::equals(const Factor& f, double tol) const
{
const EqualityFactor* p = dynamic_cast<const EqualityFactor*>(&f);
if (p == NULL) return false;
return equals(f, tol);
}
string EqualityFactor::dump() const string EqualityFactor::dump() const
{ {
string ret = "[" + key_ + "] " + gtsam::dump(value_); string ret = "[" + key_ + "] " + gtsam::dump(value_);

4
cpp/EqualityFactor.h
View File

@ -9,11 +9,12 @@
#define EQUALITYFACTOR_H_ #define EQUALITYFACTOR_H_
#include "Factor.h" #include "Factor.h"
#include "FGConfig.h"
#include "DeltaFunction.h" #include "DeltaFunction.h"
namespace gtsam { namespace gtsam {
class EqualityFactor: public Factor { class EqualityFactor: public Factor<FGConfig> {
public: public:
typedef boost::shared_ptr<EqualityFactor> shared_ptr; typedef boost::shared_ptr<EqualityFactor> shared_ptr;
@ -54,7 +55,6 @@ public:
/** /**
* equality up to tolerance * equality up to tolerance
*/ */
bool equals(const Factor& f, double tol=1e-9) const;
bool equals(const EqualityFactor& f, double tol=1e-9) const; bool equals(const EqualityFactor& f, double tol=1e-9) const;
/** /**

14
cpp/Factor.h
View File

@ -11,10 +11,11 @@
#include <set> #include <set>
#include <list> #include <list>
#include "FGConfig.h" #include <boost/utility.hpp> // for noncopyable
namespace gtsam { namespace gtsam {
/** A combination of a key with a dimension - TODO FD: move, vector specific */
struct Variable { struct Variable {
private: private:
std::string key_; std::string key_;
@ -26,6 +27,7 @@ namespace gtsam {
std::size_t dim() const { return dim_;} std::size_t dim() const { return dim_;}
}; };
/** A set of variables, used to eliminate linear factor factor graphs. TODO FD: move */
class VariableSet : public std::set<Variable> { class VariableSet : public std::set<Variable> {
}; };
@ -37,7 +39,13 @@ namespace gtsam {
* immutable, i.e., practicing functional programming. However, this * immutable, i.e., practicing functional programming. However, this
* 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
* 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.
*/ */
template <class Config>
class Factor : boost::noncopyable class Factor : boost::noncopyable
{ {
public: public:
@ -47,7 +55,7 @@ namespace gtsam {
/** /**
* negative log probability * negative log probability
*/ */
virtual double error(const FGConfig& c) const = 0; virtual double error(const Config& c) const = 0;
/** /**
* print * print
@ -58,8 +66,8 @@ namespace gtsam {
/** /**
* equality up to tolerance * equality up to tolerance
* tricky to implement, see NonLinearFactor1 for an example * tricky to implement, see NonLinearFactor1 for an example
*/
virtual bool equals(const Factor& f, double tol=1e-9) const = 0; virtual bool equals(const Factor& f, double tol=1e-9) const = 0;
*/
virtual std::string dump() const = 0; virtual std::string dump() const = 0;

5
cpp/FactorGraph-inl.h
View File

@ -10,6 +10,7 @@
#pragma once #pragma once
#include <list>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include "Ordering.h" #include "Ordering.h"
@ -69,8 +70,8 @@ Ordering colamd(int n_col, int n_row, int nrNonZeros, const std::map<Key, std::v
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Factor> template<class Factor, class Config>
Ordering FactorGraph<Factor>::getOrdering() const { Ordering FactorGraph<Factor,Config>::getOrdering() const {
// A factor graph is really laid out in row-major format, each factor a row // A factor graph is really laid out in row-major format, each factor a row
// Below, we compute a symbolic matrix stored in sparse columns. // Below, we compute a symbolic matrix stored in sparse columns.

16
cpp/FactorGraph.h
View File

@ -13,18 +13,21 @@
#include <boost/serialization/vector.hpp> #include <boost/serialization/vector.hpp>
#include <boost/serialization/shared_ptr.hpp> #include <boost/serialization/shared_ptr.hpp>
#include "Factor.h"
#include "FGConfig.h"
namespace gtsam { namespace gtsam {
class Ordering; class Ordering;
class FGConfig;
class LinearFactor;
class LinearFactorGraph;
class Ordering;
/** /**
* A factor graph is a bipartite graph with factor nodes connected to variable nodes. * A factor graph is a bipartite graph with factor nodes connected to variable nodes.
* In this class, however, only factor nodes are kept around. * In this class, however, only factor nodes are kept around.
*
* Templated on the type of factors and configuration.
*/ */
template<class Factor> class FactorGraph { template<class Factor, class Config> class FactorGraph {
public: public:
typedef typename boost::shared_ptr<Factor> shared_factor; typedef typename boost::shared_ptr<Factor> shared_factor;
typedef typename std::vector<shared_factor>::iterator iterator; typedef typename std::vector<shared_factor>::iterator iterator;
@ -67,7 +70,7 @@ namespace gtsam {
} }
/** unnormalized error */ /** unnormalized error */
double error(const FGConfig& c) const { double error(const Config& c) const {
double total_error = 0.; double total_error = 0.;
/** iterate over all the factors_ to accumulate the log probabilities */ /** iterate over all the factors_ to accumulate the log probabilities */
for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++) for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++)
@ -77,7 +80,7 @@ namespace gtsam {
} }
/** Unnormalized probability. O(n) */ /** Unnormalized probability. O(n) */
double probPrime(const FGConfig& c) const { double probPrime(const Config& c) const {
return exp(-0.5 * error(c)); return exp(-0.5 * error(c));
} }
@ -120,3 +123,4 @@ namespace gtsam {
} }
}; // FactorGraph }; // FactorGraph
} // namespace gtsam } // namespace gtsam

16
cpp/LinearFactor.cpp
View File

@ -44,15 +44,15 @@ void LinearFactor::print(const string& s) const {
/* ************************************************************************* */ /* ************************************************************************* */
// Check if two linear factors are equal // Check if two linear factors are equal
bool LinearFactor::equals(const Factor& f, double tol) const { bool LinearFactor::equals(const LinearFactor& lf, double tol) const {
const LinearFactor* lf = dynamic_cast<const LinearFactor*>(&f); //const LinearFactor* lf = dynamic_cast<const LinearFactor*>(&f);
if (lf == NULL) return false; //if (lf == NULL) return false;
if (empty()) return (lf->empty()); if (empty()) return (lf.empty());
const_iterator it1 = As.begin(), it2 = lf->As.begin(); const_iterator it1 = As.begin(), it2 = lf.As.begin();
if(As.size() != lf->As.size()) goto fail; if(As.size() != lf.As.size()) goto fail;
for(; it1 != As.end(); it1++, it2++){ for(; it1 != As.end(); it1++, it2++){
const string& j1 = it1->first, j2 = it2->first; const string& j1 = it1->first, j2 = it2->first;
@ -63,7 +63,7 @@ bool LinearFactor::equals(const Factor& f, double tol) const {
goto fail; goto fail;
} }
} }
if( !(::equal_with_abs_tol(b, (lf->b),tol)) ) { if( !(::equal_with_abs_tol(b, (lf.b),tol)) ) {
cout << "RHS disagree" << endl; cout << "RHS disagree" << endl;
goto fail; goto fail;
} }
@ -72,7 +72,7 @@ bool LinearFactor::equals(const Factor& f, double tol) const {
fail: fail:
// they don't match, print out and fail // they don't match, print out and fail
print(); print();
lf->print(); lf.print();
return false; return false;
} }

7
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 { class LinearFactor: public Factor<FGConfig> {
public: public:
typedef boost::shared_ptr<LinearFactor> shared_ptr; typedef boost::shared_ptr<LinearFactor> shared_ptr;
@ -100,7 +100,7 @@ public:
double error(const FGConfig& c) const; /** 0.5*(A*x-b)'*(A*x-b) */ double error(const FGConfig& 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 Factor& 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 "";}
std::size_t size() const { return As.size();} std::size_t size() const { return As.size();}
@ -234,8 +234,7 @@ public:
* @param key the key of the node to be eliminated * @param key the key of the node to be eliminated
* @return a new factor and a conditional gaussian on the eliminated variable * @return a new factor and a conditional gaussian on the eliminated variable
*/ */
std::pair<ConditionalGaussian::shared_ptr, shared_ptr> eliminate( std::pair<ConditionalGaussian::shared_ptr, shared_ptr> eliminate(const std::string& key);
const std::string& key);
/** /**
* Take the factor f, and append to current matrices. Not very general. * Take the factor f, and append to current matrices. Not very general.

18
cpp/LinearFactorGraph.h
View File

@ -15,13 +15,21 @@
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include "LinearFactor.h" #include "LinearFactor.h"
//#include "Ordering.h"
#include "FGConfig.h"
#include "FactorGraph.h" #include "FactorGraph.h"
#include "ChordalBayesNet.h"
namespace gtsam { namespace gtsam {
/** Linear Factor Graph where all factors are Gaussians */ class ChordalBayesNet;
class LinearFactorGraph : public FactorGraph<LinearFactor> {
/**
* A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
* Factor == LinearFactor
* FGConfig = 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> {
public: public:
/** /**
@ -72,13 +80,13 @@ namespace gtsam {
* eliminate factor graph in place(!) in the given order, yielding * eliminate factor graph in place(!) in the given order, yielding
* a chordal Bayes net * a chordal Bayes net
*/ */
ChordalBayesNet::shared_ptr eliminate(const Ordering& ordering); boost::shared_ptr<ChordalBayesNet> eliminate(const Ordering& ordering);
/** /**
* Same as eliminate but allows for passing an incomplete ordering * Same as eliminate but allows for passing an incomplete ordering
* that does not completely eliminate the graph * that does not completely eliminate the graph
*/ */
ChordalBayesNet::shared_ptr eliminate_partially(const Ordering& ordering); boost::shared_ptr<ChordalBayesNet> eliminate_partially(const Ordering& ordering);
/** /**
* optimize a linear factor graph * optimize a linear factor graph

10
cpp/Makefile.am
View File

@ -57,7 +57,8 @@ testVector_LDADD = libgtsam.la
testMatrix_LDADD = libgtsam.la testMatrix_LDADD = libgtsam.la
# nodes # nodes
sources += FGConfig.cpp Ordering.cpp LinearFactor.cpp NonlinearFactor.cpp ConditionalGaussian.cpp EqualityFactor.cpp DeltaFunction.cpp sources += FGConfig.cpp Ordering.cpp LinearFactor.cpp NonlinearFactor.cpp
sources += ConditionalGaussian.cpp EqualityFactor.cpp DeltaFunction.cpp
check_PROGRAMS += testFGConfig testLinearFactor testConditionalGaussian testNonlinearFactor testEqualityFactor testDeltaFunction check_PROGRAMS += testFGConfig testLinearFactor testConditionalGaussian testNonlinearFactor testEqualityFactor testDeltaFunction
example = smallExample.cpp example = smallExample.cpp
testFGConfig_SOURCES = testFGConfig.cpp testFGConfig_SOURCES = testFGConfig.cpp
@ -80,8 +81,8 @@ timeLinearFactor: CXXFLAGS += -I /opt/local/include
timeLinearFactor: LDFLAGS += -L.libs -lgtsam timeLinearFactor: LDFLAGS += -L.libs -lgtsam
# graphs # graphs
sources += ConstrainedChordalBayesNet.cpp ChordalBayesNet.cpp sources += ConstrainedChordalBayesNet.cpp ChordalBayesNet.cpp
sources += LinearFactorGraph.cpp NonlinearFactorGraph.cpp sources += LinearFactorGraph.cpp
sources += ConstrainedNonlinearFactorGraph.cpp ConstrainedLinearFactorGraph.cpp sources += ConstrainedNonlinearFactorGraph.cpp ConstrainedLinearFactorGraph.cpp
check_PROGRAMS += testChordalBayesNet testConstrainedChordalBayesNet testFactorgraph check_PROGRAMS += testChordalBayesNet testConstrainedChordalBayesNet testFactorgraph
check_PROGRAMS += testLinearFactorGraph testNonlinearFactorGraph testNonlinearOptimizer check_PROGRAMS += testLinearFactorGraph testNonlinearFactorGraph testNonlinearOptimizer
@ -146,10 +147,11 @@ testVSLAMFactor_LDADD = libgtsam.la
# The header files will be installed in ~/include/gtsam # The header files will be installed in ~/include/gtsam
headers = gtsam.h Value.h Testable.h Factor.h LinearFactorSet.h Point2Prior.h headers = gtsam.h Value.h Testable.h Factor.h LinearFactorSet.h Point2Prior.h
headers += Simulated2DOdometry.h Simulated2DMeasurement.h smallExample.h headers += Simulated2DOdometry.h Simulated2DMeasurement.h smallExample.h
headers += $(sources:.cpp=.h) headers += $(sources:.cpp=.h)
# templates: # templates:
headers += FactorGraph.h FactorGraph-inl.h headers += FactorGraph.h FactorGraph-inl.h
headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h
# create both dynamic and static libraries # create both dynamic and static libraries

102
cpp/NonlinearFactor.cpp
View File

@ -7,105 +7,97 @@
* @author Christian Potthast * @author Christian Potthast
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include "NonlinearFactor.h" #include "NonlinearFactor.h"
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */
NonlinearFactor::NonlinearFactor(const Vector& z, const double sigma) :
z_(z), sigma_(sigma) {
}
/* ************************************************************************* */ /* ************************************************************************* */
NonlinearFactor1::NonlinearFactor1(const Vector& z, NonlinearFactor1::NonlinearFactor1(const Vector& z,
const double sigma, const double sigma,
Vector (*h)(const Vector&), Vector (*h)(const Vector&),
const string& key1, const string& key1,
Matrix (*H)(const Vector&)) Matrix (*H)(const Vector&))
: NonlinearFactor(z, sigma), h_(h), key1_(key1), H_(H) : NonlinearFactor<FGConfig>(z, sigma), h_(h), key1_(key1), H_(H)
{ {
keys_.push_front(key1); keys_.push_front(key1);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void NonlinearFactor1::print(const string& s) const { void NonlinearFactor1::print(const string& s) const {
cout << "NonLinearFactor1 " << s << endl; cout << "NonLinearFactor1 " << s << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
LinearFactor::shared_ptr NonlinearFactor1::linearize(const FGConfig& c) const LinearFactor::shared_ptr NonlinearFactor1::linearize(const FGConfig& c) const {
{ // get argument 1 from config
// get argument 1 from config Vector x1 = c[key1_];
Vector x1 = c[key1_];
// Jacobian A = H(x1)/sigma // Jacobian A = H(x1)/sigma
Matrix A = H_(x1)/sigma_; Matrix A = H_(x1) / sigma_;
// Right-hand-side b = error(c) = (z - h(x1))/sigma // Right-hand-side b = error(c) = (z - h(x1))/sigma
Vector b = (z_ - h_(x1))/sigma_; Vector b = (z_ - h_(x1)) / sigma_;
LinearFactor::shared_ptr p(new LinearFactor(key1_,A,b)); LinearFactor::shared_ptr p(new LinearFactor(key1_, A, b));
return p; return p;
} }
/* ************************************************************************* */ /* ************************************************************************* */
/** http://artis.imag.fr/~Xavier.Decoret/resources/C++/operator==.html */ /** http://artis.imag.fr/~Xavier.Decoret/resources/C++/operator==.html */
/* ************************************************************************* */ /* ************************************************************************* */
bool NonlinearFactor1::equals(const Factor& f, double tol) const { bool NonlinearFactor1::equals(const NonlinearFactor<FGConfig>& 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::equals(*p,tol) return NonlinearFactor<FGConfig>::equals(*p, tol)
&& (h_ == p->h_) && (h_ == p->h_)
&& (key1_ == p->key1_) && (key1_== p->key1_)
&& (H_ == p->H_); && (H_ == p->H_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
NonlinearFactor2::NonlinearFactor2(const Vector& z, NonlinearFactor2::NonlinearFactor2(const Vector& z,
const double sigma, const double sigma,
Vector (*h)(const Vector&, const Vector&), Vector (*h)(const Vector&, const Vector&),
const string& key1, const string& key1,
Matrix (*H1)(const Vector&, const Vector&), Matrix (*H1)(const Vector&, const Vector&),
const string& key2, const string& key2,
Matrix (*H2)(const Vector&, const Vector&) Matrix (*H2)(const Vector&, const Vector&)
) )
: NonlinearFactor(z, sigma), h_(h), key1_(key1), H1_(H1), key2_(key2), H2_(H2) : NonlinearFactor<FGConfig>(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);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void NonlinearFactor2::print(const string& s) const { void NonlinearFactor2::print(const string& s) const {
cout << "NonLinearFactor2 " << s << endl; cout << "NonLinearFactor2 " << s << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
LinearFactor::shared_ptr NonlinearFactor2::linearize(const FGConfig& c) const LinearFactor::shared_ptr NonlinearFactor2::linearize(const FGConfig& 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_];
// Jacobian A = H(x)/sigma // Jacobian A = H(x)/sigma
Matrix A1 = H1_(x1,x2)/sigma_; Matrix A1 = H1_(x1, x2) / sigma_;
Matrix A2 = H2_(x1,x2)/sigma_; Matrix A2 = H2_(x1, x2) / sigma_;
// Right-hand-side b = (z - h(x))/sigma // Right-hand-side b = (z - h(x))/sigma
Vector b = (z_ - h_(x1,x2))/sigma_; Vector b = (z_ - h_(x1, x2)) / sigma_;
LinearFactor::shared_ptr p(new LinearFactor(key1_,A1,key2_,A2,b)); LinearFactor::shared_ptr p(new LinearFactor(key1_, A1, key2_, A2, b));
return p; return p;
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool NonlinearFactor2::equals(const Factor& f, double tol) const { bool NonlinearFactor2::equals(const NonlinearFactor<FGConfig>& 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::equals(*p,tol) return NonlinearFactor<FGConfig>::equals(*p, tol)
&& (h_ == p->h_) && (h_ == p->h_)
&& (key1_ == p->key1_) && (key1_ == p->key1_)
&& (H2_ == p->H1_) && (H2_ == p->H1_)

37
cpp/NonlinearFactor.h
View File

@ -35,8 +35,12 @@ namespace gtsam {
/** /**
* Nonlinear factor which assumes Gaussian noise on a measurement * Nonlinear factor which assumes Gaussian noise on a measurement
* predicted by a non-linear function h. * predicted by a non-linear function h.
*
* Templated on a configuration type. The configurations are typically more general
* than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds.
*/ */
class NonlinearFactor : public Factor template <class Config>
class NonlinearFactor : public Factor<Config>
{ {
protected: protected:
@ -49,15 +53,21 @@ namespace gtsam {
/** Default constructor, with easily identifiable bogus values */ /** Default constructor, with easily identifiable bogus values */
NonlinearFactor():z_(Vector_(2,888.0,999.0)),sigma_(0.1234567) {} NonlinearFactor():z_(Vector_(2,888.0,999.0)),sigma_(0.1234567) {}
/** Constructor */ /**
NonlinearFactor(const Vector& z, // the measurement * Constructor
const double sigma); // the variance * @param z the measurement
* @param sigma the standard deviation
*/
NonlinearFactor(const Vector& z, const double sigma) {
z_ = z;
sigma_ = sigma;
}
/** Vector of errors */ /** Vector of errors */
virtual Vector error_vector(const FGConfig& c) const = 0; virtual Vector error_vector(const Config& c) const = 0;
/** linearize to a LinearFactor */ /** linearize to a LinearFactor */
virtual boost::shared_ptr<LinearFactor> linearize(const FGConfig& c) const = 0; virtual boost::shared_ptr<LinearFactor> linearize(const Config& c) const = 0;
/** print to cout */ /** print to cout */
virtual void print(const std::string& s = "") const = 0; virtual void print(const std::string& s = "") const = 0;
@ -68,7 +78,7 @@ namespace gtsam {
std::list<std::string> keys() const {return keys_;} std::list<std::string> keys() const {return keys_;}
/** calculate the error of the factor */ /** calculate the error of the factor */
double error(const FGConfig& c) const { double error(const Config& c) const {
Vector e = error_vector(c) / sigma_; Vector e = error_vector(c) / sigma_;
return 0.5 * inner_prod(trans(e),e); return 0.5 * inner_prod(trans(e),e);
}; };
@ -90,7 +100,6 @@ namespace gtsam {
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) { void serialize(Archive & ar, const unsigned int version) {
// ar & boost::serialization::base_object<Factor>(*this); // TODO: needed ?
ar & BOOST_SERIALIZATION_NVP(z_); ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(sigma_); ar & BOOST_SERIALIZATION_NVP(sigma_);
ar & BOOST_SERIALIZATION_NVP(keys_); ar & BOOST_SERIALIZATION_NVP(keys_);
@ -104,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 { class NonlinearFactor1 : public NonlinearFactor<FGConfig> {
private: private:
std::string key1_; std::string key1_;
@ -124,7 +133,7 @@ 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 FGConfig& c) const {
return z_ - h_(c[key1_]); return z_ - h_(c[key1_]);
} }
@ -132,7 +141,7 @@ namespace gtsam {
boost::shared_ptr<LinearFactor> linearize(const FGConfig& c) const; boost::shared_ptr<LinearFactor> linearize(const FGConfig& c) const;
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const Factor& f, double tol=1e-9) const; bool equals(const NonlinearFactor<FGConfig>& f, double tol=1e-9) const;
std::string dump() const {return "";} std::string dump() const {return "";}
}; };
@ -142,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 { class NonlinearFactor2 : public NonlinearFactor<FGConfig> {
private: private:
@ -167,7 +176,7 @@ 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 FGConfig& c) const {
return z_ - h_(c[key1_], c[key2_]); return z_ - h_(c[key1_], c[key2_]);
} }
@ -175,7 +184,7 @@ namespace gtsam {
boost::shared_ptr<LinearFactor> linearize(const FGConfig& c) const; boost::shared_ptr<LinearFactor> linearize(const FGConfig& c) const;
/** Check if two factors are equal */ /** Check if two factors are equal */
bool equals(const Factor& f, double tol=1e-9) const; bool equals(const NonlinearFactor<FGConfig>& f, double tol=1e-9) const;
std::string dump() const{return "";}; std::string dump() const{return "";};
}; };

39
cpp/NonlinearFactorGraph-inl.h Normal file
View File

@ -0,0 +1,39 @@
/**
* @file NonlinearFactorGraph-inl.h
* @brief Factor Graph Consisting of non-linear factors
* @author Frank Dellaert
* @author Carlos Nieto
* @author Christian Potthast
*/
#pragma once
#include "LinearFactorGraph.h"
#include "NonlinearFactorGraph.h"
namespace gtsam {
/* ************************************************************************* */
template<class Config>
LinearFactorGraph NonlinearFactorGraph<Config>::linearize(const Config& config) const {
// TODO speed up the function either by returning a pointer or by
// returning the linearisation as a second argument and returning
// the reference
// create an empty linear FG
LinearFactorGraph linearFG;
typedef typename FactorGraph<NonlinearFactor<Config> ,Config>:: const_iterator const_iterator;
// linearize all factors
for (const_iterator factor = this->factors_.begin(); factor
< this->factors_.end(); factor++) {
boost::shared_ptr<LinearFactor> lf = (*factor)->linearize(config);
linearFG.push_back(lf);
}
return linearFG;
}
/* ************************************************************************* */
}

82
cpp/NonlinearFactorGraph.cpp
View File

@ -1,82 +0,0 @@
/**
* @file NonlinearFactorGraph.cpp
* @brief Factor Graph Constsiting of non-linear factors
* @brief nonlinearFactorGraph
* @author Frank Dellaert
* @author Carlos Nieto
* @author Christian Potthast
*/
#include <math.h>
#include <climits>
#include <stdexcept>
#include <boost/tuple/tuple.hpp>
#include "NonlinearFactorGraph.h"
using namespace std;
namespace gtsam {
/* ************************************************************************* */
LinearFactorGraph NonlinearFactorGraph::linearize(const FGConfig& config) const {
// TODO speed up the function either by returning a pointer or by
// returning the linearisation as a second argument and returning
// the reference
// create an empty linear FG
LinearFactorGraph linearFG;
// linearize all factors
for (const_iterator factor = factors_.begin(); factor < factors_.end(); factor++) {
LinearFactor::shared_ptr lf = (*factor)->linearize(config);
linearFG.push_back(lf);
}
return linearFG;
}
/* ************************************************************************* */
double calculate_error(const NonlinearFactorGraph& fg,
const FGConfig& config, int verbosity) {
double newError = fg.error(config);
if (verbosity >= 1)
cout << "error: " << newError << endl;
return newError;
}
/* ************************************************************************* */
bool check_convergence(double relativeErrorTreshold,
double absoluteErrorTreshold, double currentError, double newError,
int verbosity) {
// check if diverges
double absoluteDecrease = currentError - newError;
if (verbosity >= 2)
cout << "absoluteDecrease: " << absoluteDecrease << endl;
if (absoluteDecrease < 0)
throw overflow_error(
"NonlinearFactorGraph::optimize: error increased, diverges.");
// calculate relative error decrease and update currentError
double relativeDecrease = absoluteDecrease / currentError;
if (verbosity >= 2)
cout << "relativeDecrease: " << relativeDecrease << endl;
bool converged = (relativeDecrease < relativeErrorTreshold)
|| (absoluteDecrease < absoluteErrorTreshold);
if (verbosity >= 1 && converged)
cout << "converged" << endl;
return converged;
}
/* ************************************************************************* */
bool NonlinearFactorGraph::check_convergence(const FGConfig& config1,
const FGConfig& config2, double relativeErrorTreshold,
double absoluteErrorTreshold, int verbosity) const {
double currentError = calculate_error(*this, config1, verbosity);
double newError = calculate_error(*this, config2, verbosity);
return gtsam::check_convergence(relativeErrorTreshold,
absoluteErrorTreshold, currentError, newError, verbosity);
}
/* ************************************************************************* */
} // namespace gtsam

70
cpp/NonlinearFactorGraph.h
View File

@ -10,65 +10,29 @@
#pragma once #pragma once
#include <boost/serialization/base_object.hpp>
#include <colamd/colamd.h>
#include "FactorGraph.h"
#include "NonlinearFactor.h" #include "NonlinearFactor.h"
#include "LinearFactorGraph.h" #include "FactorGraph.h"
#include "ChordalBayesNet.h"
namespace gtsam { namespace gtsam {
typedef FactorGraph<NonlinearFactor> BaseFactorGraph;
/** Factor Graph consisting of non-linear factors */
class NonlinearFactorGraph : public BaseFactorGraph
{
public: // these you will probably want to use
/** /**
* linearize a non linear factor * A non-linear factor graph is templated on a configuration, but the factor type
*/ * is fixed as a NonLinearFactor. The configurations are typically (in SAM) more general
LinearFactorGraph linearize(const FGConfig& config) const; * 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
*/
template<class Config>
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<Config> ,Config> {
/** public:
* Given two configs, check whether the error of config2 is
* different enough from the error for config1, or whether config1
* is essentially optimal
*
* @param config1 Reference to first configuration
* @param config2 Reference to second configuration
* @param relativeErrorTreshold
* @param absoluteErrorTreshold
* @param verbosity Integer specifying how much output to provide
*/
bool check_convergence(const FGConfig& config1,
const FGConfig& config2,
double relativeErrorTreshold, double absoluteErrorTreshold,
int verbosity = 0) const;
private: /**
* linearize a nonlinear factor graph
*/
LinearFactorGraph linearize(const Config& config) const;
/** Serialization function */ };
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
// do not use BOOST_SERIALIZATION_NVP for this name-value-pair ! It will crash.
ar & boost::serialization::make_nvp("BaseFactorGraph",
boost::serialization::base_object<BaseFactorGraph>(*this));
}
}; } // namespace
/**
* Check convergence
*/
bool check_convergence (double relativeErrorTreshold,
double absoluteErrorTreshold,
double currentError, double newError,
int verbosity);
/**
* calculate error for current configuration
*/
double calculate_error (const NonlinearFactorGraph& fg, const FGConfig& config, int verbosity);
}

23
cpp/NonlinearOptimizer-inl.h
View File

@ -17,6 +17,29 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */
bool check_convergence(double relativeErrorTreshold,
double absoluteErrorTreshold, double currentError, double newError,
int verbosity) {
// check if diverges
double absoluteDecrease = currentError - newError;
if (verbosity >= 2)
cout << "absoluteDecrease: " << absoluteDecrease << endl;
if (absoluteDecrease < 0)
throw overflow_error(
"NonlinearFactorGraph::optimize: error increased, diverges.");
// calculate relative error decrease and update currentError
double relativeDecrease = absoluteDecrease / currentError;
if (verbosity >= 2)
cout << "relativeDecrease: " << relativeDecrease << endl;
bool converged = (relativeDecrease < relativeErrorTreshold)
|| (absoluteDecrease < absoluteErrorTreshold);
if (verbosity >= 1 && converged)
cout << "converged" << endl;
return converged;
}
/* ************************************************************************* */ /* ************************************************************************* */
// Constructors // Constructors
/* ************************************************************************* */ /* ************************************************************************* */

10
cpp/NonlinearOptimizer.h
View File

@ -142,6 +142,14 @@ namespace gtsam {
}; };
} /**
* Check convergence
*/
bool check_convergence (double relativeErrorTreshold,
double absoluteErrorTreshold,
double currentError, double newError,
int verbosity);
} // gtsam
#endif /* NONLINEAROPTIMIZER_H_ */ #endif /* NONLINEAROPTIMIZER_H_ */

6
cpp/VSLAMFactor.cpp
View File

@ -12,7 +12,7 @@ using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
VSLAMFactor::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Cal3_S2 &K) VSLAMFactor::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Cal3_S2 &K)
: NonlinearFactor(z, sigma) : NonlinearFactor<FGConfig>(z, sigma)
{ {
cameraFrameNumber_ = cn; cameraFrameNumber_ = cn;
landmarkNumber_ = ln; landmarkNumber_ = ln;
@ -67,7 +67,7 @@ LinearFactor::shared_ptr VSLAMFactor::linearize(const FGConfig& c) const
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool VSLAMFactor::equals(const Factor& f, double tol) const { bool VSLAMFactor::equals(const NonlinearFactor<FGConfig>& f, double tol) const {
const VSLAMFactor* p = dynamic_cast<const VSLAMFactor*>(&f); const VSLAMFactor* p = dynamic_cast<const VSLAMFactor*>(&f);
if (p == NULL) goto fail; if (p == NULL) goto fail;
if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail; if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail;
@ -88,7 +88,7 @@ string VSLAMFactor::dump() const
Vector z = measurement(); Vector z = measurement();
char buffer[200]; char buffer[200];
buffer[0] = 0; buffer[0] = 0;
sprintf(buffer, "1 %d %d %f %d", i, j , sigma(), z.size()); sprintf(buffer, "1 %d %d %f %d", i, j , sigma(), (int)z.size());
for(size_t i = 0; i < z.size(); i++) for(size_t i = 0; i < z.size(); i++)
sprintf(buffer, "%s %f", buffer, z(i)); sprintf(buffer, "%s %f", buffer, z(i));
sprintf(buffer, "%s %s", buffer, K_.dump().c_str()); sprintf(buffer, "%s %s", buffer, K_.dump().c_str());

16
cpp/VSLAMFactor.h
View File

@ -8,20 +8,23 @@
#include "NonlinearFactor.h" #include "NonlinearFactor.h"
#include "LinearFactor.h" #include "LinearFactor.h"
#include "FGConfig.h"
#include "Cal3_S2.h" #include "Cal3_S2.h"
#include "Pose3.h" #include "Pose3.h"
namespace gtsam {
/** /**
* Non-linear factor for a constraint derived from a 2D measurement, * Non-linear factor for a constraint derived from a 2D measurement,
* i.e. the main building block for visual SLAM. * i.e. the main building block for visual SLAM.
*/ */
class VSLAMFactor : public gtsam::NonlinearFactor class VSLAMFactor : public NonlinearFactor<FGConfig>
{ {
private: private:
int cameraFrameNumber_, landmarkNumber_; int cameraFrameNumber_, landmarkNumber_;
std::string cameraFrameName_, landmarkName_; std::string cameraFrameName_, landmarkName_;
gtsam::Cal3_S2 K_; // Calibration stored in each factor. FD: need to think about this. Cal3_S2 K_; // Calibration stored in each factor. FD: need to think about this.
public: public:
@ -35,7 +38,7 @@ class VSLAMFactor : public gtsam::NonlinearFactor
* @param landmarkNumber is the index of the landmark * @param landmarkNumber is the index of the landmark
* @param K the constant calibration * @param K the constant calibration
*/ */
VSLAMFactor(const Vector& z, double sigma, int cameraFrameNumber, int landmarkNumber, const gtsam::Cal3_S2& K); VSLAMFactor(const Vector& z, double sigma, int cameraFrameNumber, int landmarkNumber, const Cal3_S2& K);
/** /**
@ -47,17 +50,17 @@ class VSLAMFactor : public gtsam::NonlinearFactor
/** /**
* calculate the error of the factor * calculate the error of the factor
*/ */
Vector error_vector(const gtsam::FGConfig&) const; Vector error_vector(const FGConfig&) const;
/** /**
* linerarization * linerarization
*/ */
gtsam::LinearFactor::shared_ptr linearize(const gtsam::FGConfig&) const; LinearFactor::shared_ptr linearize(const FGConfig&) const;
/** /**
* equals * equals
*/ */
bool equals(const gtsam::Factor&, double tol=1e-9) const; bool equals(const NonlinearFactor<FGConfig>&, double tol=1e-9) const;
int getCameraFrameNumber() const { return cameraFrameNumber_; } int getCameraFrameNumber() const { return cameraFrameNumber_; }
int getLandmarkNumber() const { return landmarkNumber_; } int getLandmarkNumber() const { return landmarkNumber_; }
@ -68,3 +71,4 @@ class VSLAMFactor : public gtsam::NonlinearFactor
std::string dump() const; std::string dump() const;
}; };
}

41
cpp/smallExample.cpp
View File

@ -27,12 +27,12 @@ using namespace std;
namespace gtsam { namespace gtsam {
typedef boost::shared_ptr<NonlinearFactor> shared; typedef boost::shared_ptr<NonlinearFactor<FGConfig> > shared;
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<const NonlinearFactorGraph> sharedNonlinearFactorGraph() { boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedNonlinearFactorGraph() {
// Create // Create
boost::shared_ptr<NonlinearFactorGraph> nlfg(new NonlinearFactorGraph); boost::shared_ptr<ExampleNonlinearFactorGraph> nlfg(new ExampleNonlinearFactorGraph);
// prior on x1 // prior on x1
double sigma1=0.1; double sigma1=0.1;
@ -61,7 +61,7 @@ boost::shared_ptr<const NonlinearFactorGraph> sharedNonlinearFactorGraph() {
return nlfg; return nlfg;
} }
NonlinearFactorGraph createNonlinearFactorGraph() { ExampleNonlinearFactorGraph createNonlinearFactorGraph() {
return *sharedNonlinearFactorGraph(); return *sharedNonlinearFactorGraph();
} }
@ -92,24 +92,23 @@ ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph()
} }
/* ************************************************************************* */ /* ************************************************************************* */
ConstrainedNonlinearFactorGraph createConstrainedNonlinearFactorGraph() ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig> , FGConfig> createConstrainedNonlinearFactorGraph() {
{ ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig> , FGConfig> graph;
ConstrainedNonlinearFactorGraph graph; FGConfig c = createConstrainedConfig();
FGConfig c = createConstrainedConfig();
// equality constraint for initial pose // equality constraint for initial pose
EqualityFactor::shared_ptr f1(new EqualityFactor(c["x0"], "x0")); EqualityFactor::shared_ptr f1(new EqualityFactor(c["x0"], "x0"));
graph.push_back_eq(f1); graph.push_back_eq(f1);
// odometry between x0 and x1 // odometry between x0 and x1
double sigma=0.1; double sigma = 0.1;
shared f2(new Simulated2DOdometry(c["x1"]-c["x0"], sigma, "x0", "x1")); shared f2(new Simulated2DOdometry(c["x1"] - c["x0"], sigma, "x0", "x1"));
graph.push_back(f2); graph.push_back(f2); // TODO
return graph; return graph;
} }
/* ************************************************************************* */ /* ************************************************************************* */
FGConfig createConfig() FGConfig 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.;
@ -328,9 +327,9 @@ namespace optimize {
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<const NonlinearFactorGraph> sharedReallyNonlinearFactorGraph() boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedReallyNonlinearFactorGraph()
{ {
boost::shared_ptr<NonlinearFactorGraph> fg(new NonlinearFactorGraph); boost::shared_ptr<ExampleNonlinearFactorGraph> fg(new ExampleNonlinearFactorGraph);
Vector z = Vector_(2,1.0,0.0); Vector z = Vector_(2,1.0,0.0);
double sigma = 0.1; double sigma = 0.1;
boost::shared_ptr<NonlinearFactor1> boost::shared_ptr<NonlinearFactor1>
@ -339,7 +338,7 @@ boost::shared_ptr<const NonlinearFactorGraph> sharedReallyNonlinearFactorGraph()
return fg; return fg;
} }
NonlinearFactorGraph createReallyNonlinearFactorGraph() { ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph() {
return *sharedReallyNonlinearFactorGraph(); return *sharedReallyNonlinearFactorGraph();
} }

13
cpp/smallExample.h
View File

@ -21,11 +21,13 @@
namespace gtsam { namespace gtsam {
typedef NonlinearFactorGraph<FGConfig> ExampleNonlinearFactorGraph;
/** /**
* Create small example for non-linear factor graph * Create small example for non-linear factor graph
*/ */
boost::shared_ptr<const NonlinearFactorGraph> sharedNonlinearFactorGraph(); boost::shared_ptr<const ExampleNonlinearFactorGraph > sharedNonlinearFactorGraph();
NonlinearFactorGraph createNonlinearFactorGraph(); ExampleNonlinearFactorGraph createNonlinearFactorGraph();
/** /**
* Create small example constrained factor graph * Create small example constrained factor graph
@ -35,7 +37,8 @@ namespace gtsam {
/** /**
* Create small example constrained nonlinear factor graph * Create small example constrained nonlinear factor graph
*/ */
ConstrainedNonlinearFactorGraph createConstrainedNonlinearFactorGraph(); ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig>,FGConfig>
createConstrainedNonlinearFactorGraph();
/** /**
* Create configuration to go with it * Create configuration to go with it
@ -84,8 +87,8 @@ namespace gtsam {
/** /**
* Create really non-linear factor graph (cos/sin) * Create really non-linear factor graph (cos/sin)
*/ */
boost::shared_ptr<const NonlinearFactorGraph> sharedReallyNonlinearFactorGraph(); boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedReallyNonlinearFactorGraph();
NonlinearFactorGraph createReallyNonlinearFactorGraph(); ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph();
/** /**
* Create a noisy configuration for linearization * Create a noisy configuration for linearization

37
cpp/testConstrainedNonlinearFactorGraph.cpp
View File

@ -12,26 +12,27 @@
using namespace gtsam; using namespace gtsam;
typedef boost::shared_ptr<NonlinearFactor> shared; typedef boost::shared_ptr<NonlinearFactor<FGConfig> > shared;
typedef ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig>,FGConfig> TestGraph;
TEST( ConstrainedNonlinearFactorGraph, equals ) TEST( TestGraph, equals )
{ {
ConstrainedNonlinearFactorGraph fg = createConstrainedNonlinearFactorGraph(); TestGraph fg = createConstrainedNonlinearFactorGraph();
ConstrainedNonlinearFactorGraph fg2 = createConstrainedNonlinearFactorGraph(); TestGraph fg2 = createConstrainedNonlinearFactorGraph();
CHECK( fg.equals(fg2) ); CHECK( fg.equals(fg2) );
} }
TEST( ConstrainedNonlinearFactorGraph, copy ) TEST( TestGraph, copy )
{ {
NonlinearFactorGraph nfg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
ConstrainedNonlinearFactorGraph actual(nfg); TestGraph actual(nfg);
shared f1 = nfg[0]; shared f1 = nfg[0];
shared f2 = nfg[1]; shared f2 = nfg[1];
shared f3 = nfg[2]; shared f3 = nfg[2];
shared f4 = nfg[3]; shared f4 = nfg[3];
ConstrainedNonlinearFactorGraph expected; TestGraph expected;
expected.push_back(f1); expected.push_back(f1);
expected.push_back(f2); expected.push_back(f2);
expected.push_back(f3); expected.push_back(f3);
@ -40,11 +41,11 @@ TEST( ConstrainedNonlinearFactorGraph, copy )
CHECK(actual.equals(expected)); CHECK(actual.equals(expected));
} }
TEST( ConstrainedNonlinearFactorGraph, baseline ) TEST( TestGraph, baseline )
{ {
// use existing examples // use existing examples
NonlinearFactorGraph nfg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
ConstrainedNonlinearFactorGraph cfg(nfg); TestGraph cfg(nfg);
FGConfig initial = createNoisyConfig(); FGConfig initial = createNoisyConfig();
ConstrainedLinearFactorGraph linearized = cfg.linearize(initial); ConstrainedLinearFactorGraph linearized = cfg.linearize(initial);
@ -54,17 +55,19 @@ TEST( ConstrainedNonlinearFactorGraph, baseline )
CHECK(expected.equals(linearized)); CHECK(expected.equals(linearized));
} }
TEST( ConstrainedNonlinearFactorGraph, convert ) /*
TEST( TestGraph, convert )
{ {
NonlinearFactorGraph expected = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph expected = createNonlinearFactorGraph();
ConstrainedNonlinearFactorGraph cfg(expected); TestGraph cfg(expected);
NonlinearFactorGraph actual = cfg.convert(); ExampleNonlinearFactorGraph actual = cfg.convert();
CHECK(actual.equals(expected)); CHECK(actual.equals(expected));
} }
*/
TEST( ConstrainedNonlinearFactorGraph, linearize_and_solve ) TEST( TestGraph, linearize_and_solve )
{ {
ConstrainedNonlinearFactorGraph nfg = createConstrainedNonlinearFactorGraph(); TestGraph nfg = createConstrainedNonlinearFactorGraph();
FGConfig lin = createConstrainedLinConfig(); FGConfig lin = createConstrainedLinConfig();
ConstrainedLinearFactorGraph actual_lfg = nfg.linearize(lin); ConstrainedLinearFactorGraph actual_lfg = nfg.linearize(lin);
FGConfig actual = actual_lfg.optimize(actual_lfg.getOrdering()); FGConfig actual = actual_lfg.optimize(actual_lfg.getOrdering());

22
cpp/testNonlinearFactor.cpp
View File

@ -17,17 +17,19 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
typedef boost::shared_ptr<NonlinearFactor<FGConfig> > shared_nlf;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonLinearFactor, NonlinearFactor ) TEST( NonLinearFactor, NonlinearFactor )
{ {
// create a non linear factor graph // create a non linear factor graph
NonlinearFactorGraph 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(); FGConfig cfg = createNoisyConfig();
// get the factor "f1" from the factor graph // get the factor "f1" from the factor graph
boost::shared_ptr<NonlinearFactor> factor = fg[0]; shared_nlf factor = fg[0];
// calculate the error_vector from the factor "f1" // calculate the error_vector from the factor "f1"
Vector actual_e = factor->error_vector(cfg); Vector actual_e = factor->error_vector(cfg);
@ -48,7 +50,7 @@ TEST( NonLinearFactor, NonlinearFactor )
TEST( NonLinearFactor, linearize_f1 ) TEST( NonLinearFactor, linearize_f1 )
{ {
// Grab a non-linear factor // Grab a non-linear factor
NonlinearFactorGraph nfg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
boost::shared_ptr<NonlinearFactor1> nlf = boost::shared_ptr<NonlinearFactor1> nlf =
boost::static_pointer_cast<NonlinearFactor1>(nfg[0]); boost::static_pointer_cast<NonlinearFactor1>(nfg[0]);
@ -66,7 +68,7 @@ TEST( NonLinearFactor, linearize_f1 )
TEST( NonLinearFactor, linearize_f2 ) TEST( NonLinearFactor, linearize_f2 )
{ {
// Grab a non-linear factor // Grab a non-linear factor
NonlinearFactorGraph nfg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
boost::shared_ptr<NonlinearFactor1> nlf = boost::shared_ptr<NonlinearFactor1> nlf =
boost::static_pointer_cast<NonlinearFactor1>(nfg[1]); boost::static_pointer_cast<NonlinearFactor1>(nfg[1]);
@ -84,7 +86,7 @@ TEST( NonLinearFactor, linearize_f2 )
TEST( NonLinearFactor, linearize_f3 ) TEST( NonLinearFactor, linearize_f3 )
{ {
// Grab a non-linear factor // Grab a non-linear factor
NonlinearFactorGraph nfg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
boost::shared_ptr<NonlinearFactor1> nlf = boost::shared_ptr<NonlinearFactor1> nlf =
boost::static_pointer_cast<NonlinearFactor1>(nfg[2]); boost::static_pointer_cast<NonlinearFactor1>(nfg[2]);
@ -102,7 +104,7 @@ TEST( NonLinearFactor, linearize_f3 )
TEST( NonLinearFactor, linearize_f4 ) TEST( NonLinearFactor, linearize_f4 )
{ {
// Grab a non-linear factor // Grab a non-linear factor
NonlinearFactorGraph nfg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
boost::shared_ptr<NonlinearFactor1> nlf = boost::shared_ptr<NonlinearFactor1> nlf =
boost::static_pointer_cast<NonlinearFactor1>(nfg[3]); boost::static_pointer_cast<NonlinearFactor1>(nfg[3]);
@ -120,15 +122,15 @@ TEST( NonLinearFactor, linearize_f4 )
TEST( NonLinearFactor, size ) TEST( NonLinearFactor, size )
{ {
// create a non linear factor graph // create a non linear factor graph
NonlinearFactorGraph 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(); FGConfig cfg = createNoisyConfig();
// get some factors from the graph // get some factors from the graph
boost::shared_ptr<NonlinearFactor> factor1 = fg[0]; shared_nlf factor1 = fg[0];
boost::shared_ptr<NonlinearFactor> factor2 = fg[1]; shared_nlf factor2 = fg[1];
boost::shared_ptr<NonlinearFactor> factor3 = fg[2]; shared_nlf factor3 = fg[2];
CHECK(factor1->size() == 1); CHECK(factor1->size() == 1);
CHECK(factor2->size() == 2); CHECK(factor2->size() == 2);

20
cpp/testNonlinearFactorGraph.cpp
View File

@ -14,22 +14,24 @@ using namespace std;
#include "Matrix.h" #include "Matrix.h"
#include "smallExample.h" #include "smallExample.h"
#include "FactorGraph-inl.h"
#include "NonlinearFactorGraph-inl.h"
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearFactorGraph, equals ) TEST( ExampleNonlinearFactorGraph, equals )
{ {
NonlinearFactorGraph fg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
NonlinearFactorGraph fg2 = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph fg2 = createNonlinearFactorGraph();
CHECK( fg.equals(fg2) ); CHECK( fg.equals(fg2) );
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearFactorGraph, error ) TEST( ExampleNonlinearFactorGraph, error )
{ {
NonlinearFactorGraph fg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
FGConfig c1 = createConfig(); FGConfig c1 = createConfig();
double actual1 = fg.error(c1); double actual1 = fg.error(c1);
@ -41,9 +43,9 @@ TEST( NonlinearFactorGraph, error )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearFactorGraph, probPrime ) TEST( ExampleNonlinearFactorGraph, probPrime )
{ {
NonlinearFactorGraph fg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
FGConfig cfg = createConfig(); FGConfig cfg = createConfig();
// evaluate the probability of the factor graph // evaluate the probability of the factor graph
@ -53,9 +55,9 @@ TEST( NonlinearFactorGraph, probPrime )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearFactorGraph, linearize ) TEST( ExampleNonlinearFactorGraph, linearize )
{ {
NonlinearFactorGraph fg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
FGConfig initial = createNoisyConfig(); FGConfig initial = createNoisyConfig();
LinearFactorGraph linearized = fg.linearize(initial); LinearFactorGraph linearized = fg.linearize(initial);
LinearFactorGraph expected = createLinearFactorGraph(); LinearFactorGraph expected = createLinearFactorGraph();

17
cpp/testNonlinearOptimizer.cpp
View File

@ -12,16 +12,17 @@ using namespace std;
#include "Matrix.h" #include "Matrix.h"
#include "smallExample.h" #include "smallExample.h"
// template definitions // template definitions
#include "NonlinearFactorGraph-inl.h"
#include "NonlinearOptimizer-inl.h" #include "NonlinearOptimizer-inl.h"
using namespace gtsam; using namespace gtsam;
typedef NonlinearOptimizer<NonlinearFactorGraph,FGConfig> Optimizer; typedef NonlinearOptimizer<ExampleNonlinearFactorGraph,FGConfig> Optimizer;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearFactorGraph, delta ) TEST( NonlinearOptimizer, delta )
{ {
NonlinearFactorGraph fg = createNonlinearFactorGraph(); ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
Optimizer::shared_config initial = sharedNoisyConfig(); Optimizer::shared_config initial = sharedNoisyConfig();
// Expected configuration is the difference between the noisy config // Expected configuration is the difference between the noisy config
@ -69,10 +70,10 @@ TEST( NonlinearFactorGraph, delta )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearFactorGraph, iterateLM ) TEST( NonlinearOptimizer, iterateLM )
{ {
// really non-linear factor graph // really non-linear factor graph
NonlinearFactorGraph fg = createReallyNonlinearFactorGraph(); ExampleNonlinearFactorGraph fg = createReallyNonlinearFactorGraph();
// config far from minimum // config far from minimum
Vector x0 = Vector_(1, 3.0); Vector x0 = Vector_(1, 3.0);
@ -92,13 +93,13 @@ TEST( NonlinearFactorGraph, iterateLM )
// LM iterate with lambda 0 should be the same // LM iterate with lambda 0 should be the same
Optimizer iterated2 = optimizer.iterateLM(); Optimizer iterated2 = optimizer.iterateLM();
CHECK(assert_equal(*(iterated1.config()), *(iterated2.config()), 1e-9)); CHECK(assert_equal(*iterated1.config(), *iterated2.config(), 1e-9));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( NonlinearFactorGraph, optimize ) TEST( NonlinearOptimizer, optimize )
{ {
NonlinearFactorGraph fg = createReallyNonlinearFactorGraph(); ExampleNonlinearFactorGraph fg = createReallyNonlinearFactorGraph();
// test error at minimum // test error at minimum
Vector xstar = Vector_(1, 0.0); Vector xstar = Vector_(1, 0.0);