'''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
parent
34c1bb6f29
commit
989f290c99
46
.cproject
46
.cproject
|
|
@ -1,4 +1,4 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
|
||||
<?fileVersion 4.0.0?>
|
||||
|
||||
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
||||
|
|
@ -300,7 +300,6 @@
|
|||
<buildTargets>
|
||||
<target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -308,6 +307,7 @@
|
|||
</target>
|
||||
<target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimpleCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -315,14 +315,14 @@
|
|||
</target>
|
||||
<target name="testCal3_S2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-f local.mk</buildArguments>
|
||||
<buildTarget>testCal3_S2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testVSLAMFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -330,6 +330,7 @@
|
|||
</target>
|
||||
<target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testCalibratedCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -337,7 +338,6 @@
|
|||
</target>
|
||||
<target name="testConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testConditionalGaussian.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -345,6 +345,7 @@
|
|||
</target>
|
||||
<target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -352,14 +353,15 @@
|
|||
</target>
|
||||
<target name="testFGConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildArguments>-f local.mk</buildArguments>
|
||||
<buildTarget>testFGConfig.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testRot3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -367,7 +369,6 @@
|
|||
</target>
|
||||
<target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testNonlinearOptimizer.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -375,22 +376,45 @@
|
|||
</target>
|
||||
<target name="testLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testLinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildArguments/>
|
||||
<buildTarget>testLinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -398,7 +422,6 @@
|
|||
</target>
|
||||
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -406,7 +429,6 @@
|
|||
</target>
|
||||
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
|
|||
|
|
@ -7,48 +7,3 @@
|
|||
|
||||
#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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -8,39 +8,78 @@
|
|||
#ifndef CONSTRAINEDNONLINEARFACTORGRAPH_H_
|
||||
#define CONSTRAINEDNONLINEARFACTORGRAPH_H_
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include "NonlinearFactorGraph.h"
|
||||
#include "EqualityFactor.h"
|
||||
#include "ConstrainedLinearFactorGraph.h"
|
||||
|
||||
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:
|
||||
/** collection of equality factors */
|
||||
std::vector<EqualityFactor::shared_ptr> eq_factors;
|
||||
|
||||
public:
|
||||
// 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;
|
||||
|
||||
/**
|
||||
* Default constructor
|
||||
*/
|
||||
ConstrainedNonlinearFactorGraph();
|
||||
ConstrainedNonlinearFactorGraph() {
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
* 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
|
||||
|
|
@ -49,10 +88,6 @@ public:
|
|||
eq_factors.push_back(eq);
|
||||
}
|
||||
|
||||
/**
|
||||
* converts the graph to a regular nonlinear graph - removes equality constraints
|
||||
*/
|
||||
NonlinearFactorGraph convert() const;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
||||
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 ret = "[" + key_ + "] " + gtsam::dump(value_);
|
||||
|
|
|
|||
|
|
@ -9,11 +9,12 @@
|
|||
#define EQUALITYFACTOR_H_
|
||||
|
||||
#include "Factor.h"
|
||||
#include "FGConfig.h"
|
||||
#include "DeltaFunction.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class EqualityFactor: public Factor {
|
||||
class EqualityFactor: public Factor<FGConfig> {
|
||||
public:
|
||||
typedef boost::shared_ptr<EqualityFactor> shared_ptr;
|
||||
|
||||
|
|
@ -54,7 +55,6 @@ public:
|
|||
/**
|
||||
* equality up to tolerance
|
||||
*/
|
||||
bool equals(const Factor& f, double tol=1e-9) const;
|
||||
bool equals(const EqualityFactor& f, double tol=1e-9) const;
|
||||
|
||||
/**
|
||||
|
|
|
|||
14
cpp/Factor.h
14
cpp/Factor.h
|
|
@ -11,10 +11,11 @@
|
|||
|
||||
#include <set>
|
||||
#include <list>
|
||||
#include "FGConfig.h"
|
||||
#include <boost/utility.hpp> // for noncopyable
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** A combination of a key with a dimension - TODO FD: move, vector specific */
|
||||
struct Variable {
|
||||
private:
|
||||
std::string key_;
|
||||
|
|
@ -26,6 +27,7 @@ namespace gtsam {
|
|||
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> {
|
||||
};
|
||||
|
||||
|
|
@ -37,7 +39,13 @@ namespace gtsam {
|
|||
* immutable, i.e., practicing functional programming. However, this
|
||||
* 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
|
||||
* 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
|
||||
{
|
||||
public:
|
||||
|
|
@ -47,7 +55,7 @@ namespace gtsam {
|
|||
/**
|
||||
* negative log probability
|
||||
*/
|
||||
virtual double error(const FGConfig& c) const = 0;
|
||||
virtual double error(const Config& c) const = 0;
|
||||
|
||||
/**
|
||||
* print
|
||||
|
|
@ -58,8 +66,8 @@ namespace gtsam {
|
|||
/**
|
||||
* equality up to tolerance
|
||||
* tricky to implement, see NonLinearFactor1 for an example
|
||||
*/
|
||||
virtual bool equals(const Factor& f, double tol=1e-9) const = 0;
|
||||
*/
|
||||
|
||||
virtual std::string dump() const = 0;
|
||||
|
||||
|
|
|
|||
|
|
@ -10,6 +10,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <list>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#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>
|
||||
Ordering FactorGraph<Factor>::getOrdering() const {
|
||||
template<class Factor, class Config>
|
||||
Ordering FactorGraph<Factor,Config>::getOrdering() const {
|
||||
|
||||
// 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.
|
||||
|
|
|
|||
|
|
@ -13,18 +13,21 @@
|
|||
#include <boost/serialization/vector.hpp>
|
||||
#include <boost/serialization/shared_ptr.hpp>
|
||||
|
||||
#include "Factor.h"
|
||||
#include "FGConfig.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class Ordering;
|
||||
class FGConfig;
|
||||
class LinearFactor;
|
||||
class LinearFactorGraph;
|
||||
class Ordering;
|
||||
|
||||
/**
|
||||
* A factor graph is a bipartite graph with factor nodes connected to variable nodes.
|
||||
* 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:
|
||||
typedef typename boost::shared_ptr<Factor> shared_factor;
|
||||
typedef typename std::vector<shared_factor>::iterator iterator;
|
||||
|
|
@ -67,7 +70,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/** unnormalized error */
|
||||
double error(const FGConfig& c) const {
|
||||
double error(const Config& c) const {
|
||||
double total_error = 0.;
|
||||
/** iterate over all the factors_ to accumulate the log probabilities */
|
||||
for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++)
|
||||
|
|
@ -77,7 +80,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/** Unnormalized probability. O(n) */
|
||||
double probPrime(const FGConfig& c) const {
|
||||
double probPrime(const Config& c) const {
|
||||
return exp(-0.5 * error(c));
|
||||
}
|
||||
|
||||
|
|
@ -120,3 +123,4 @@ namespace gtsam {
|
|||
}
|
||||
}; // FactorGraph
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -44,15 +44,15 @@ void LinearFactor::print(const string& s) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// 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);
|
||||
if (lf == NULL) return false;
|
||||
//const LinearFactor* lf = dynamic_cast<const LinearFactor*>(&f);
|
||||
//if (lf == NULL) return false;
|
||||
|
||||
if (empty()) return (lf->empty());
|
||||
if (empty()) return (lf.empty());
|
||||
|
||||
const_iterator it1 = As.begin(), it2 = lf->As.begin();
|
||||
if(As.size() != lf->As.size()) goto fail;
|
||||
const_iterator it1 = As.begin(), it2 = lf.As.begin();
|
||||
if(As.size() != lf.As.size()) goto fail;
|
||||
|
||||
for(; it1 != As.end(); it1++, it2++){
|
||||
const string& j1 = it1->first, j2 = it2->first;
|
||||
|
|
@ -63,7 +63,7 @@ bool LinearFactor::equals(const Factor& f, double tol) const {
|
|||
goto fail;
|
||||
}
|
||||
}
|
||||
if( !(::equal_with_abs_tol(b, (lf->b),tol)) ) {
|
||||
if( !(::equal_with_abs_tol(b, (lf.b),tol)) ) {
|
||||
cout << "RHS disagree" << endl;
|
||||
goto fail;
|
||||
}
|
||||
|
|
@ -72,7 +72,7 @@ bool LinearFactor::equals(const Factor& f, double tol) const {
|
|||
fail:
|
||||
// they don't match, print out and fail
|
||||
print();
|
||||
lf->print();
|
||||
lf.print();
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
class LinearFactor: public Factor<FGConfig> {
|
||||
public:
|
||||
|
||||
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) */
|
||||
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::size_t size() const { return As.size();}
|
||||
|
||||
|
|
@ -234,8 +234,7 @@ public:
|
|||
* @param key the key of the node to be eliminated
|
||||
* @return a new factor and a conditional gaussian on the eliminated variable
|
||||
*/
|
||||
std::pair<ConditionalGaussian::shared_ptr, shared_ptr> eliminate(
|
||||
const std::string& key);
|
||||
std::pair<ConditionalGaussian::shared_ptr, shared_ptr> eliminate(const std::string& key);
|
||||
|
||||
/**
|
||||
* Take the factor f, and append to current matrices. Not very general.
|
||||
|
|
|
|||
|
|
@ -15,13 +15,21 @@
|
|||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#include "LinearFactor.h"
|
||||
//#include "Ordering.h"
|
||||
#include "FGConfig.h"
|
||||
#include "FactorGraph.h"
|
||||
#include "ChordalBayesNet.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Linear Factor Graph where all factors are Gaussians */
|
||||
class LinearFactorGraph : public FactorGraph<LinearFactor> {
|
||||
class ChordalBayesNet;
|
||||
|
||||
/**
|
||||
* 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:
|
||||
|
||||
/**
|
||||
|
|
@ -72,13 +80,13 @@ namespace gtsam {
|
|||
* eliminate factor graph in place(!) in the given order, yielding
|
||||
* 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
|
||||
* 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
|
||||
|
|
|
|||
|
|
@ -57,7 +57,8 @@ testVector_LDADD = libgtsam.la
|
|||
testMatrix_LDADD = libgtsam.la
|
||||
|
||||
# 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
|
||||
example = smallExample.cpp
|
||||
testFGConfig_SOURCES = testFGConfig.cpp
|
||||
|
|
@ -80,8 +81,8 @@ timeLinearFactor: CXXFLAGS += -I /opt/local/include
|
|||
timeLinearFactor: LDFLAGS += -L.libs -lgtsam
|
||||
|
||||
# graphs
|
||||
sources += ConstrainedChordalBayesNet.cpp ChordalBayesNet.cpp
|
||||
sources += LinearFactorGraph.cpp NonlinearFactorGraph.cpp
|
||||
sources += ConstrainedChordalBayesNet.cpp ChordalBayesNet.cpp
|
||||
sources += LinearFactorGraph.cpp
|
||||
sources += ConstrainedNonlinearFactorGraph.cpp ConstrainedLinearFactorGraph.cpp
|
||||
check_PROGRAMS += testChordalBayesNet testConstrainedChordalBayesNet testFactorgraph
|
||||
check_PROGRAMS += testLinearFactorGraph testNonlinearFactorGraph testNonlinearOptimizer
|
||||
|
|
@ -146,10 +147,11 @@ testVSLAMFactor_LDADD = libgtsam.la
|
|||
|
||||
# The header files will be installed in ~/include/gtsam
|
||||
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)
|
||||
# templates:
|
||||
headers += FactorGraph.h FactorGraph-inl.h
|
||||
headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
|
||||
headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h
|
||||
|
||||
# create both dynamic and static libraries
|
||||
|
|
|
|||
|
|
@ -7,105 +7,97 @@
|
|||
* @author Christian Potthast
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
|
||||
#include "NonlinearFactor.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
NonlinearFactor::NonlinearFactor(const Vector& z, const double sigma) :
|
||||
z_(z), sigma_(sigma) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
NonlinearFactor1::NonlinearFactor1(const Vector& z,
|
||||
const double sigma,
|
||||
Vector (*h)(const Vector&),
|
||||
const string& key1,
|
||||
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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void NonlinearFactor1::print(const string& s) const {
|
||||
cout << "NonLinearFactor1 " << s << endl;
|
||||
cout << "NonLinearFactor1 " << s << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearFactor::shared_ptr NonlinearFactor1::linearize(const FGConfig& c) const
|
||||
{
|
||||
// get argument 1 from config
|
||||
Vector x1 = c[key1_];
|
||||
LinearFactor::shared_ptr NonlinearFactor1::linearize(const FGConfig& c) const {
|
||||
// get argument 1 from config
|
||||
Vector x1 = c[key1_];
|
||||
|
||||
// Jacobian A = H(x1)/sigma
|
||||
Matrix A = H_(x1)/sigma_;
|
||||
// Jacobian A = H(x1)/sigma
|
||||
Matrix A = H_(x1) / sigma_;
|
||||
|
||||
// Right-hand-side b = error(c) = (z - h(x1))/sigma
|
||||
Vector b = (z_ - h_(x1))/sigma_;
|
||||
// Right-hand-side b = error(c) = (z - h(x1))/sigma
|
||||
Vector b = (z_ - h_(x1)) / sigma_;
|
||||
|
||||
LinearFactor::shared_ptr p(new LinearFactor(key1_,A,b));
|
||||
return p;
|
||||
LinearFactor::shared_ptr p(new LinearFactor(key1_, A, b));
|
||||
return p;
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
/** http://artis.imag.fr/~Xavier.Decoret/resources/C++/operator==.html */
|
||||
/* ************************************************************************* */
|
||||
bool NonlinearFactor1::equals(const Factor& f, double tol) const {
|
||||
const NonlinearFactor1* p = dynamic_cast<const NonlinearFactor1*>(&f);
|
||||
if (p == NULL) return false;
|
||||
return NonlinearFactor::equals(*p,tol)
|
||||
&& (h_ == p->h_)
|
||||
&& (key1_ == p->key1_)
|
||||
&& (H_ == p->H_);
|
||||
bool NonlinearFactor1::equals(const NonlinearFactor<FGConfig>& f, double tol) const {
|
||||
const NonlinearFactor1* p = dynamic_cast<const NonlinearFactor1*> (&f);
|
||||
if (p == NULL) return false;
|
||||
return NonlinearFactor<FGConfig>::equals(*p, tol)
|
||||
&& (h_ == p->h_)
|
||||
&& (key1_== p->key1_)
|
||||
&& (H_ == p->H_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
NonlinearFactor2::NonlinearFactor2(const Vector& z,
|
||||
const double sigma,
|
||||
Vector (*h)(const Vector&, const Vector&),
|
||||
const string& key1,
|
||||
Matrix (*H1)(const Vector&, const Vector&),
|
||||
const string& key2,
|
||||
Matrix (*H2)(const Vector&, const Vector&)
|
||||
)
|
||||
: NonlinearFactor(z, sigma), h_(h), key1_(key1), H1_(H1), key2_(key2), H2_(H2)
|
||||
{
|
||||
keys_.push_front(key1);
|
||||
keys_.push_front(key2);
|
||||
NonlinearFactor2::NonlinearFactor2(const Vector& z,
|
||||
const double sigma,
|
||||
Vector (*h)(const Vector&, const Vector&),
|
||||
const string& key1,
|
||||
Matrix (*H1)(const Vector&, const Vector&),
|
||||
const string& key2,
|
||||
Matrix (*H2)(const Vector&, const Vector&)
|
||||
)
|
||||
: NonlinearFactor<FGConfig>(z, sigma), h_(h), key1_(key1), H1_(H1), key2_(key2), H2_(H2)
|
||||
{
|
||||
keys_.push_front(key1);
|
||||
keys_.push_front(key2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void NonlinearFactor2::print(const string& s) const {
|
||||
cout << "NonLinearFactor2 " << s << endl;
|
||||
cout << "NonLinearFactor2 " << s << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearFactor::shared_ptr NonlinearFactor2::linearize(const FGConfig& c) const
|
||||
{
|
||||
// get arguments from config
|
||||
Vector x1 = c[key1_];
|
||||
Vector x2 = c[key2_];
|
||||
LinearFactor::shared_ptr NonlinearFactor2::linearize(const FGConfig& c) const {
|
||||
// get arguments from config
|
||||
Vector x1 = c[key1_];
|
||||
Vector x2 = c[key2_];
|
||||
|
||||
// Jacobian A = H(x)/sigma
|
||||
Matrix A1 = H1_(x1,x2)/sigma_;
|
||||
Matrix A2 = H2_(x1,x2)/sigma_;
|
||||
// Jacobian A = H(x)/sigma
|
||||
Matrix A1 = H1_(x1, x2) / sigma_;
|
||||
Matrix A2 = H2_(x1, x2) / sigma_;
|
||||
|
||||
// Right-hand-side b = (z - h(x))/sigma
|
||||
Vector b = (z_ - h_(x1,x2))/sigma_;
|
||||
// Right-hand-side b = (z - h(x))/sigma
|
||||
Vector b = (z_ - h_(x1, x2)) / sigma_;
|
||||
|
||||
LinearFactor::shared_ptr p(new LinearFactor(key1_,A1,key2_,A2,b));
|
||||
return p;
|
||||
LinearFactor::shared_ptr p(new LinearFactor(key1_, A1, key2_, A2, b));
|
||||
return p;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool NonlinearFactor2::equals(const Factor& f, double tol) const {
|
||||
const NonlinearFactor2* p = dynamic_cast<const NonlinearFactor2*>(&f);
|
||||
if (p == NULL) return false;
|
||||
return NonlinearFactor::equals(*p,tol)
|
||||
bool NonlinearFactor2::equals(const NonlinearFactor<FGConfig>& f, double tol) const {
|
||||
const NonlinearFactor2* p = dynamic_cast<const NonlinearFactor2*> (&f);
|
||||
if (p == NULL) return false;
|
||||
return NonlinearFactor<FGConfig>::equals(*p, tol)
|
||||
&& (h_ == p->h_)
|
||||
&& (key1_ == p->key1_)
|
||||
&& (H2_ == p->H1_)
|
||||
|
|
|
|||
|
|
@ -35,8 +35,12 @@ namespace gtsam {
|
|||
/**
|
||||
* Nonlinear factor which assumes Gaussian noise on a measurement
|
||||
* 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:
|
||||
|
||||
|
|
@ -49,15 +53,21 @@ namespace gtsam {
|
|||
/** Default constructor, with easily identifiable bogus values */
|
||||
NonlinearFactor():z_(Vector_(2,888.0,999.0)),sigma_(0.1234567) {}
|
||||
|
||||
/** Constructor */
|
||||
NonlinearFactor(const Vector& z, // the measurement
|
||||
const double sigma); // the variance
|
||||
/**
|
||||
* Constructor
|
||||
* @param z the measurement
|
||||
* @param sigma the standard deviation
|
||||
*/
|
||||
NonlinearFactor(const Vector& z, const double sigma) {
|
||||
z_ = z;
|
||||
sigma_ = sigma;
|
||||
}
|
||||
|
||||
/** 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 */
|
||||
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 */
|
||||
virtual void print(const std::string& s = "") const = 0;
|
||||
|
|
@ -68,7 +78,7 @@ namespace gtsam {
|
|||
std::list<std::string> keys() const {return keys_;}
|
||||
|
||||
/** calculate the error of the factor */
|
||||
double error(const FGConfig& c) const {
|
||||
double error(const Config& c) const {
|
||||
Vector e = error_vector(c) / sigma_;
|
||||
return 0.5 * inner_prod(trans(e),e);
|
||||
};
|
||||
|
|
@ -90,7 +100,6 @@ namespace gtsam {
|
|||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
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(sigma_);
|
||||
ar & BOOST_SERIALIZATION_NVP(keys_);
|
||||
|
|
@ -104,7 +113,7 @@ namespace gtsam {
|
|||
* Note: cannot be serialized as contains function pointers
|
||||
* Specialized derived classes could do this
|
||||
*/
|
||||
class NonlinearFactor1 : public NonlinearFactor {
|
||||
class NonlinearFactor1 : public NonlinearFactor<FGConfig> {
|
||||
private:
|
||||
|
||||
std::string key1_;
|
||||
|
|
@ -124,7 +133,7 @@ namespace gtsam {
|
|||
void print(const std::string& s = "") const;
|
||||
|
||||
/** error function */
|
||||
inline Vector error_vector(const FGConfig& c) const {
|
||||
inline Vector error_vector(const FGConfig& c) const {
|
||||
return z_ - h_(c[key1_]);
|
||||
}
|
||||
|
||||
|
|
@ -132,7 +141,7 @@ namespace gtsam {
|
|||
boost::shared_ptr<LinearFactor> linearize(const FGConfig& c) const;
|
||||
|
||||
/** 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 "";}
|
||||
};
|
||||
|
|
@ -142,7 +151,7 @@ namespace gtsam {
|
|||
* Note: cannot be serialized as contains function pointers
|
||||
* Specialized derived classes could do this
|
||||
*/
|
||||
class NonlinearFactor2 : public NonlinearFactor {
|
||||
class NonlinearFactor2 : public NonlinearFactor<FGConfig> {
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -167,7 +176,7 @@ namespace gtsam {
|
|||
void print(const std::string& s = "") const;
|
||||
|
||||
/** 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_]);
|
||||
}
|
||||
|
||||
|
|
@ -175,7 +184,7 @@ namespace gtsam {
|
|||
boost::shared_ptr<LinearFactor> linearize(const FGConfig& c) const;
|
||||
|
||||
/** 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 "";};
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -10,65 +10,29 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <boost/serialization/base_object.hpp>
|
||||
#include <colamd/colamd.h>
|
||||
#include "FactorGraph.h"
|
||||
#include "NonlinearFactor.h"
|
||||
#include "LinearFactorGraph.h"
|
||||
#include "ChordalBayesNet.h"
|
||||
#include "FactorGraph.h"
|
||||
|
||||
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
|
||||
*/
|
||||
LinearFactorGraph linearize(const FGConfig& config) const;
|
||||
* 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
|
||||
* 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> {
|
||||
|
||||
/**
|
||||
* 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;
|
||||
public:
|
||||
|
||||
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));
|
||||
}
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* 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);
|
||||
}
|
||||
} // namespace
|
||||
|
|
|
|||
|
|
@ -17,6 +17,29 @@ using namespace std;
|
|||
|
||||
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
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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_ */
|
||||
|
|
|
|||
|
|
@ -12,7 +12,7 @@ using namespace gtsam;
|
|||
|
||||
/* ************************************************************************* */
|
||||
VSLAMFactor::VSLAMFactor(const Vector& z, double sigma, int cn, int ln, const Cal3_S2 &K)
|
||||
: NonlinearFactor(z, sigma)
|
||||
: NonlinearFactor<FGConfig>(z, sigma)
|
||||
{
|
||||
cameraFrameNumber_ = cn;
|
||||
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);
|
||||
if (p == NULL) goto fail;
|
||||
if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail;
|
||||
|
|
@ -88,7 +88,7 @@ string VSLAMFactor::dump() const
|
|||
Vector z = measurement();
|
||||
char buffer[200];
|
||||
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++)
|
||||
sprintf(buffer, "%s %f", buffer, z(i));
|
||||
sprintf(buffer, "%s %s", buffer, K_.dump().c_str());
|
||||
|
|
|
|||
|
|
@ -8,20 +8,23 @@
|
|||
|
||||
#include "NonlinearFactor.h"
|
||||
#include "LinearFactor.h"
|
||||
#include "FGConfig.h"
|
||||
#include "Cal3_S2.h"
|
||||
#include "Pose3.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Non-linear factor for a constraint derived from a 2D measurement,
|
||||
* i.e. the main building block for visual SLAM.
|
||||
*/
|
||||
class VSLAMFactor : public gtsam::NonlinearFactor
|
||||
class VSLAMFactor : public NonlinearFactor<FGConfig>
|
||||
{
|
||||
|
||||
private:
|
||||
int cameraFrameNumber_, landmarkNumber_;
|
||||
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:
|
||||
|
||||
|
|
@ -35,7 +38,7 @@ class VSLAMFactor : public gtsam::NonlinearFactor
|
|||
* @param landmarkNumber is the index of the landmark
|
||||
* @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
|
||||
*/
|
||||
Vector error_vector(const gtsam::FGConfig&) const;
|
||||
Vector error_vector(const FGConfig&) const;
|
||||
|
||||
/**
|
||||
* linerarization
|
||||
*/
|
||||
gtsam::LinearFactor::shared_ptr linearize(const gtsam::FGConfig&) const;
|
||||
LinearFactor::shared_ptr linearize(const FGConfig&) const;
|
||||
|
||||
/**
|
||||
* 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 getLandmarkNumber() const { return landmarkNumber_; }
|
||||
|
|
@ -68,3 +71,4 @@ class VSLAMFactor : public gtsam::NonlinearFactor
|
|||
std::string dump() const;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -27,12 +27,12 @@ using namespace std;
|
|||
|
||||
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
|
||||
boost::shared_ptr<NonlinearFactorGraph> nlfg(new NonlinearFactorGraph);
|
||||
boost::shared_ptr<ExampleNonlinearFactorGraph> nlfg(new ExampleNonlinearFactorGraph);
|
||||
|
||||
// prior on x1
|
||||
double sigma1=0.1;
|
||||
|
|
@ -61,7 +61,7 @@ boost::shared_ptr<const NonlinearFactorGraph> sharedNonlinearFactorGraph() {
|
|||
return nlfg;
|
||||
}
|
||||
|
||||
NonlinearFactorGraph createNonlinearFactorGraph() {
|
||||
ExampleNonlinearFactorGraph createNonlinearFactorGraph() {
|
||||
return *sharedNonlinearFactorGraph();
|
||||
}
|
||||
|
||||
|
|
@ -92,24 +92,23 @@ ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph()
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ConstrainedNonlinearFactorGraph createConstrainedNonlinearFactorGraph()
|
||||
{
|
||||
ConstrainedNonlinearFactorGraph graph;
|
||||
FGConfig c = createConstrainedConfig();
|
||||
ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig> , FGConfig> createConstrainedNonlinearFactorGraph() {
|
||||
ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig> , FGConfig> graph;
|
||||
FGConfig c = createConstrainedConfig();
|
||||
|
||||
// equality constraint for initial pose
|
||||
EqualityFactor::shared_ptr f1(new EqualityFactor(c["x0"], "x0"));
|
||||
graph.push_back_eq(f1);
|
||||
// equality constraint for initial pose
|
||||
EqualityFactor::shared_ptr f1(new EqualityFactor(c["x0"], "x0"));
|
||||
graph.push_back_eq(f1);
|
||||
|
||||
// odometry between x0 and x1
|
||||
double sigma=0.1;
|
||||
shared f2(new Simulated2DOdometry(c["x1"]-c["x0"], sigma, "x0", "x1"));
|
||||
graph.push_back(f2);
|
||||
// odometry between x0 and x1
|
||||
double sigma = 0.1;
|
||||
shared f2(new Simulated2DOdometry(c["x1"] - c["x0"], sigma, "x0", "x1"));
|
||||
graph.push_back(f2); // TODO
|
||||
|
||||
return graph;
|
||||
}
|
||||
return graph;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* ************************************************************************* */
|
||||
FGConfig createConfig()
|
||||
{
|
||||
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);
|
||||
double sigma = 0.1;
|
||||
boost::shared_ptr<NonlinearFactor1>
|
||||
|
|
@ -339,7 +338,7 @@ boost::shared_ptr<const NonlinearFactorGraph> sharedReallyNonlinearFactorGraph()
|
|||
return fg;
|
||||
}
|
||||
|
||||
NonlinearFactorGraph createReallyNonlinearFactorGraph() {
|
||||
ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph() {
|
||||
return *sharedReallyNonlinearFactorGraph();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -21,11 +21,13 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
typedef NonlinearFactorGraph<FGConfig> ExampleNonlinearFactorGraph;
|
||||
|
||||
/**
|
||||
* Create small example for non-linear factor graph
|
||||
*/
|
||||
boost::shared_ptr<const NonlinearFactorGraph> sharedNonlinearFactorGraph();
|
||||
NonlinearFactorGraph createNonlinearFactorGraph();
|
||||
boost::shared_ptr<const ExampleNonlinearFactorGraph > sharedNonlinearFactorGraph();
|
||||
ExampleNonlinearFactorGraph createNonlinearFactorGraph();
|
||||
|
||||
/**
|
||||
* Create small example constrained factor graph
|
||||
|
|
@ -35,7 +37,8 @@ namespace gtsam {
|
|||
/**
|
||||
* Create small example constrained nonlinear factor graph
|
||||
*/
|
||||
ConstrainedNonlinearFactorGraph createConstrainedNonlinearFactorGraph();
|
||||
ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig>,FGConfig>
|
||||
createConstrainedNonlinearFactorGraph();
|
||||
|
||||
/**
|
||||
* Create configuration to go with it
|
||||
|
|
@ -84,8 +87,8 @@ namespace gtsam {
|
|||
/**
|
||||
* Create really non-linear factor graph (cos/sin)
|
||||
*/
|
||||
boost::shared_ptr<const NonlinearFactorGraph> sharedReallyNonlinearFactorGraph();
|
||||
NonlinearFactorGraph createReallyNonlinearFactorGraph();
|
||||
boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedReallyNonlinearFactorGraph();
|
||||
ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph();
|
||||
|
||||
/**
|
||||
* Create a noisy configuration for linearization
|
||||
|
|
|
|||
|
|
@ -12,26 +12,27 @@
|
|||
|
||||
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();
|
||||
ConstrainedNonlinearFactorGraph fg2 = createConstrainedNonlinearFactorGraph();
|
||||
TestGraph fg = createConstrainedNonlinearFactorGraph();
|
||||
TestGraph fg2 = createConstrainedNonlinearFactorGraph();
|
||||
CHECK( fg.equals(fg2) );
|
||||
}
|
||||
|
||||
TEST( ConstrainedNonlinearFactorGraph, copy )
|
||||
TEST( TestGraph, copy )
|
||||
{
|
||||
NonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
||||
ConstrainedNonlinearFactorGraph actual(nfg);
|
||||
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
||||
TestGraph actual(nfg);
|
||||
|
||||
shared f1 = nfg[0];
|
||||
shared f2 = nfg[1];
|
||||
shared f3 = nfg[2];
|
||||
shared f4 = nfg[3];
|
||||
|
||||
ConstrainedNonlinearFactorGraph expected;
|
||||
TestGraph expected;
|
||||
expected.push_back(f1);
|
||||
expected.push_back(f2);
|
||||
expected.push_back(f3);
|
||||
|
|
@ -40,11 +41,11 @@ TEST( ConstrainedNonlinearFactorGraph, copy )
|
|||
CHECK(actual.equals(expected));
|
||||
}
|
||||
|
||||
TEST( ConstrainedNonlinearFactorGraph, baseline )
|
||||
TEST( TestGraph, baseline )
|
||||
{
|
||||
// use existing examples
|
||||
NonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
||||
ConstrainedNonlinearFactorGraph cfg(nfg);
|
||||
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
||||
TestGraph cfg(nfg);
|
||||
|
||||
FGConfig initial = createNoisyConfig();
|
||||
ConstrainedLinearFactorGraph linearized = cfg.linearize(initial);
|
||||
|
|
@ -54,17 +55,19 @@ TEST( ConstrainedNonlinearFactorGraph, baseline )
|
|||
CHECK(expected.equals(linearized));
|
||||
}
|
||||
|
||||
TEST( ConstrainedNonlinearFactorGraph, convert )
|
||||
/*
|
||||
TEST( TestGraph, convert )
|
||||
{
|
||||
NonlinearFactorGraph expected = createNonlinearFactorGraph();
|
||||
ConstrainedNonlinearFactorGraph cfg(expected);
|
||||
NonlinearFactorGraph actual = cfg.convert();
|
||||
ExampleNonlinearFactorGraph expected = createNonlinearFactorGraph();
|
||||
TestGraph cfg(expected);
|
||||
ExampleNonlinearFactorGraph actual = cfg.convert();
|
||||
CHECK(actual.equals(expected));
|
||||
}
|
||||
*/
|
||||
|
||||
TEST( ConstrainedNonlinearFactorGraph, linearize_and_solve )
|
||||
TEST( TestGraph, linearize_and_solve )
|
||||
{
|
||||
ConstrainedNonlinearFactorGraph nfg = createConstrainedNonlinearFactorGraph();
|
||||
TestGraph nfg = createConstrainedNonlinearFactorGraph();
|
||||
FGConfig lin = createConstrainedLinConfig();
|
||||
ConstrainedLinearFactorGraph actual_lfg = nfg.linearize(lin);
|
||||
FGConfig actual = actual_lfg.optimize(actual_lfg.getOrdering());
|
||||
|
|
|
|||
|
|
@ -17,17 +17,19 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
typedef boost::shared_ptr<NonlinearFactor<FGConfig> > shared_nlf;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonLinearFactor, NonlinearFactor )
|
||||
{
|
||||
// create a non linear factor graph
|
||||
NonlinearFactorGraph fg = createNonlinearFactorGraph();
|
||||
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
|
||||
|
||||
// create a configuration for the non linear factor graph
|
||||
FGConfig cfg = createNoisyConfig();
|
||||
|
||||
// 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"
|
||||
Vector actual_e = factor->error_vector(cfg);
|
||||
|
|
@ -48,7 +50,7 @@ TEST( NonLinearFactor, NonlinearFactor )
|
|||
TEST( NonLinearFactor, linearize_f1 )
|
||||
{
|
||||
// Grab a non-linear factor
|
||||
NonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
||||
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
||||
boost::shared_ptr<NonlinearFactor1> nlf =
|
||||
boost::static_pointer_cast<NonlinearFactor1>(nfg[0]);
|
||||
|
||||
|
|
@ -66,7 +68,7 @@ TEST( NonLinearFactor, linearize_f1 )
|
|||
TEST( NonLinearFactor, linearize_f2 )
|
||||
{
|
||||
// Grab a non-linear factor
|
||||
NonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
||||
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
||||
boost::shared_ptr<NonlinearFactor1> nlf =
|
||||
boost::static_pointer_cast<NonlinearFactor1>(nfg[1]);
|
||||
|
||||
|
|
@ -84,7 +86,7 @@ TEST( NonLinearFactor, linearize_f2 )
|
|||
TEST( NonLinearFactor, linearize_f3 )
|
||||
{
|
||||
// Grab a non-linear factor
|
||||
NonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
||||
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
||||
boost::shared_ptr<NonlinearFactor1> nlf =
|
||||
boost::static_pointer_cast<NonlinearFactor1>(nfg[2]);
|
||||
|
||||
|
|
@ -102,7 +104,7 @@ TEST( NonLinearFactor, linearize_f3 )
|
|||
TEST( NonLinearFactor, linearize_f4 )
|
||||
{
|
||||
// Grab a non-linear factor
|
||||
NonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
||||
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
|
||||
boost::shared_ptr<NonlinearFactor1> nlf =
|
||||
boost::static_pointer_cast<NonlinearFactor1>(nfg[3]);
|
||||
|
||||
|
|
@ -120,15 +122,15 @@ TEST( NonLinearFactor, linearize_f4 )
|
|||
TEST( NonLinearFactor, size )
|
||||
{
|
||||
// create a non linear factor graph
|
||||
NonlinearFactorGraph fg = createNonlinearFactorGraph();
|
||||
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
|
||||
|
||||
// create a configuration for the non linear factor graph
|
||||
FGConfig cfg = createNoisyConfig();
|
||||
|
||||
// get some factors from the graph
|
||||
boost::shared_ptr<NonlinearFactor> factor1 = fg[0];
|
||||
boost::shared_ptr<NonlinearFactor> factor2 = fg[1];
|
||||
boost::shared_ptr<NonlinearFactor> factor3 = fg[2];
|
||||
shared_nlf factor1 = fg[0];
|
||||
shared_nlf factor2 = fg[1];
|
||||
shared_nlf factor3 = fg[2];
|
||||
|
||||
CHECK(factor1->size() == 1);
|
||||
CHECK(factor2->size() == 2);
|
||||
|
|
|
|||
|
|
@ -14,22 +14,24 @@ using namespace std;
|
|||
|
||||
#include "Matrix.h"
|
||||
#include "smallExample.h"
|
||||
#include "FactorGraph-inl.h"
|
||||
#include "NonlinearFactorGraph-inl.h"
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearFactorGraph, equals )
|
||||
TEST( ExampleNonlinearFactorGraph, equals )
|
||||
{
|
||||
|
||||
NonlinearFactorGraph fg = createNonlinearFactorGraph();
|
||||
NonlinearFactorGraph fg2 = createNonlinearFactorGraph();
|
||||
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
|
||||
ExampleNonlinearFactorGraph fg2 = createNonlinearFactorGraph();
|
||||
CHECK( fg.equals(fg2) );
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearFactorGraph, error )
|
||||
TEST( ExampleNonlinearFactorGraph, error )
|
||||
{
|
||||
NonlinearFactorGraph fg = createNonlinearFactorGraph();
|
||||
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
|
||||
|
||||
FGConfig c1 = createConfig();
|
||||
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();
|
||||
|
||||
// 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();
|
||||
LinearFactorGraph linearized = fg.linearize(initial);
|
||||
LinearFactorGraph expected = createLinearFactorGraph();
|
||||
|
|
|
|||
|
|
@ -12,16 +12,17 @@ using namespace std;
|
|||
#include "Matrix.h"
|
||||
#include "smallExample.h"
|
||||
// template definitions
|
||||
#include "NonlinearFactorGraph-inl.h"
|
||||
#include "NonlinearOptimizer-inl.h"
|
||||
|
||||
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();
|
||||
|
||||
// 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
|
||||
NonlinearFactorGraph fg = createReallyNonlinearFactorGraph();
|
||||
ExampleNonlinearFactorGraph fg = createReallyNonlinearFactorGraph();
|
||||
|
||||
// config far from minimum
|
||||
Vector x0 = Vector_(1, 3.0);
|
||||
|
|
@ -92,13 +93,13 @@ TEST( NonlinearFactorGraph, iterateLM )
|
|||
// LM iterate with lambda 0 should be the same
|
||||
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
|
||||
Vector xstar = Vector_(1, 0.0);
|
||||
|
|
|
|||
Loading…
Reference in New Issue