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

Factor Graph serialization, and renaming of factors -> factors_

release/4.3a0
Frank Dellaert 2009-08-31 02:40:26 +00:00
parent 00fd3cebcf
commit c69d8d9b36
9 changed files with 150 additions and 114 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
.cproject
View File

@ -300,6 +300,7 @@
<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>
@ -307,7 +308,6 @@
</target>
<target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>testSimpleCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -315,7 +315,6 @@
</target>
<target name="testCal3_S2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>testCal3_S2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -323,6 +322,7 @@
</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,7 +330,6 @@
</target>
<target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -338,6 +337,7 @@
</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,14 +345,22 @@
</target>
<target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFGConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>testFGConfig.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>
@ -360,6 +368,7 @@
</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>
@ -367,6 +376,7 @@
</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>

8
cpp/ConstrainedLinearFactorGraph.cpp
View File

@ -91,14 +91,14 @@ DeltaFunction::shared_ptr ConstrainedLinearFactorGraph::eliminate_one_eq(const s
// remove all unary linear factors on this node
vector<LinearFactor::shared_ptr> newfactors;
BOOST_FOREACH(LinearFactor::shared_ptr f, factors)
BOOST_FOREACH(LinearFactor::shared_ptr f, factors_)
{
if (f->size() != 1 || !f->involves(key))
{
newfactors.push_back(f);
}
}
factors = newfactors;
factors_ = newfactors;
// combine the linear factors connected to equality node
boost::shared_ptr<MutableLinearFactor> joint_factor = combine_factors(key);
@ -146,7 +146,7 @@ FGConfig ConstrainedLinearFactorGraph::optimize(const Ordering& ordering){
void ConstrainedLinearFactorGraph::print(const std::string& s) const
{
cout << "ConstrainedFactorGraph: " << s << endl;
BOOST_FOREACH(LinearFactor::shared_ptr f, factors)
BOOST_FOREACH(LinearFactor::shared_ptr f, factors_)
{
f->print();
}
@ -208,7 +208,7 @@ Ordering ConstrainedLinearFactorGraph::getOrdering() const
LinearFactorGraph ConstrainedLinearFactorGraph::convert() const
{
LinearFactorGraph ret;
BOOST_FOREACH(LinearFactor::shared_ptr f, factors)
BOOST_FOREACH(LinearFactor::shared_ptr f, factors_)
{
ret.push_back(f);
}

9
cpp/ConstrainedLinearFactorGraph.h
View File

@ -56,15 +56,14 @@ public:
return eq_factors.end();
}
/** clear the factor graph - reimplemented to include equality factors */
/** clear the factor graph - re-implemented to include equality factors */
void clear(){
factors.clear();
node_to_factors_.clear();
factors_.clear();
eq_factors.clear();
}
/** size - reimplemented to include the equality factors */
inline size_t size() const { return factors.size() + eq_factors.size(); }
/** size - reimplemented to include the equality factors_ */
inline size_t size() const { return factors_.size() + eq_factors.size(); }
/** Check equality - checks equality constraints as well*/
bool equals(const LinearFactorGraph& fg, double tol=1e-9) const;

7
cpp/ConstrainedNonlinearFactorGraph.cpp
View File

@ -28,7 +28,7 @@ ConstrainedLinearFactorGraph ConstrainedNonlinearFactorGraph::linearize(const FG
ConstrainedLinearFactorGraph ret;
// linearize all nonlinear factors
for(const_iterator factor=factors.begin(); factor<factors.end(); factor++){
for(const_iterator factor=factors_.begin(); factor<factors_.end(); factor++){
LinearFactor::shared_ptr lf = (*factor)->linearize(config);
ret.push_back(lf);
}
@ -45,10 +45,9 @@ ConstrainedLinearFactorGraph ConstrainedNonlinearFactorGraph::linearize(const FG
NonlinearFactorGraph ConstrainedNonlinearFactorGraph::convert() const
{
NonlinearFactorGraph ret;
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> f, factors)
{
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> f, factors_)
ret.push_back(f);
}
return ret;
}

162
cpp/FactorGraph.h
View File

@ -9,104 +9,106 @@
#pragma once
#include <list>
#include <map>
#include <vector>
#include <boost/shared_ptr.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include "Factor.h"
#include "FGConfig.h"
namespace gtsam {
/**
* A factor graph is a bipartite graph with factor nodes connected to variable nodes.
* In this class, however, only factor nodes are kept around.
*/
template<class T> class FactorGraph
{
public:
typedef typename boost::shared_ptr<T> shared_factor;
typedef typename std::vector<shared_factor>::iterator iterator;
typedef typename std::vector<shared_factor>::const_iterator const_iterator;
protected:
/** Collection of factors */
std::vector<shared_factor> factors;
std::map<std::string, std::list<int> > node_to_factors_;
/**
* A factor graph is a bipartite graph with factor nodes connected to variable nodes.
* In this class, however, only factor nodes are kept around.
*/
template<class T> class FactorGraph {
public:
typedef typename boost::shared_ptr<T> shared_factor;
typedef typename std::vector<shared_factor>::iterator iterator;
typedef typename std::vector<shared_factor>::const_iterator const_iterator;
public:
protected:
/** Collection of factors */
std::vector<shared_factor> factors_;
/** get the factors to a specific node */
const std::list<int>& get_factors_to_node(const std::string& key) const {
return node_to_factors_[key];
}
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(factors_);
}
/** STL like, return the iterator pointing to the first factor */
const_iterator begin() const {
return factors.begin();
}
public:
/** STL like, return the iterator pointing to the last factor */
const_iterator end() const {
return factors.end();
}
/** STL like, return the iterator pointing to the first factor */
const_iterator begin() const {
return factors_.begin();
}
/** clear the factor graph */
void clear(){
factors.clear();
node_to_factors_.clear();
}
/** STL like, return the iterator pointing to the last factor */
const_iterator end() const {
return factors_.end();
}
/** Get a specific factor by index */
shared_factor operator[](size_t i) const {
return factors[i];
}
/** clear the factor graph */
void clear() {
factors_.clear();
}
/** return the numbers of the factors in the factor graph */
inline size_t size() const { return factors.size(); }
/** Get a specific factor by index */
shared_factor operator[](size_t i) const {
return factors_[i];
}
/** Add a factor */
void push_back(shared_factor ptr_f) {factors.push_back(ptr_f);}
/** unnormalized error */
double error(const FGConfig& 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++)
total_error += (*factor)->error(c);
/** return the numbers of the factors_ in the factor graph */
inline size_t size() const {
return factors_.size();
}
return total_error;
}
/** Add a factor */
void push_back(shared_factor ptr_f) {
factors_.push_back(ptr_f);
}
/** Unnormalized probability. O(n) */
double probPrime(const FGConfig& c) const {
return exp(-0.5*error(c));
}
/** unnormalized error */
double error(const FGConfig& 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++)
total_error += (*factor)->error(c);
/** print out graph */
void print(const std::string& s = "FactorGraph") const{
std::cout << s << std::endl;
printf("size: %d\n", (int)size());
for(const_iterator factor=factors.begin(); factor!=factors.end(); factor++)
(*factor)->print();
}
return total_error;
}
/** Check equality */
bool equals(const FactorGraph& fg, double tol=1e-9) const
{
/** check whether the two factor graphs have the same number of factors */
if( factors.size() != fg.size() ) goto fail;
/** check whether the factors are the same */
for(size_t i=0;i<factors.size();i++)
if ( ! factors[i]->equals(*fg.factors[i], tol) ) goto fail; //TODO: Doesn't this force order of factor insertion?
return true;
/** Unnormalized probability. O(n) */
double probPrime(const FGConfig& c) const {
return exp(-0.5 * error(c));
}
fail:
print();
fg.print();
return false;
}
};
/** print out graph */
void print(const std::string& s = "FactorGraph") const {
std::cout << s << std::endl;
printf("size: %d\n", (int) size());
for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++)
(*factor)->print();
}
/** Check equality */
bool equals(const FactorGraph& fg, double tol = 1e-9) const {
/** check whether the two factor graphs have the same number of factors_ */
if (factors_.size() != fg.size()) goto fail;
/** check whether the factors_ are the same */
for (size_t i = 0; i < factors_.size(); i++)
// TODO: Doesn't this force order of factor insertion?
if (!factors_[i]->equals(*fg.factors_[i], tol)) goto fail;
return true;
fail: print();
fg.print();
return false;
}
};
} // namespace gtsam

22
cpp/LinearFactorGraph.cpp
View File

@ -40,8 +40,8 @@ void LinearFactorGraph::setCBN(const ChordalBayesNet& CBN)
set<string> LinearFactorGraph::find_separator(const string& key) const
{
set<string> separator;
BOOST_FOREACH(shared_factor factor,factors)
factor->tally_separator(key,separator);
BOOST_FOREACH(shared_factor factor,factors_)
factor->tally_separator(key,separator);
return separator;
}
@ -54,10 +54,10 @@ LinearFactorGraph::find_factors_and_remove(const string& key)
{
LinearFactorSet found;
for(iterator factor=factors.begin(); factor!=factors.end(); )
for(iterator factor=factors_.begin(); factor!=factors_.end(); )
if ((*factor)->involves(key)) {
found.insert(*factor);
factor = factors.erase(factor);
factor = factors_.erase(factor);
} else {
factor++; // important, erase will have effect of ++
}
@ -130,7 +130,7 @@ LinearFactorGraph::eliminate(const Ordering& ordering)
// after eliminate, only one zero indegree factor should remain
// TODO: this check needs to exist - verify that unit tests work when this check is in place
/*
if (factors.size() != 1) {
if (factors_.size() != 1) {
print();
throw(invalid_argument("LinearFactorGraph::eliminate: graph not empty after eliminate, ordering incomplete?"));
}
@ -156,7 +156,7 @@ FGConfig LinearFactorGraph::optimize(const Ordering& ordering)
/** combine two factor graphs */
/* ************************************************************************* */
void LinearFactorGraph::combine(const LinearFactorGraph &lfg){
for(const_iterator factor=lfg.factors.begin(); factor!=lfg.factors.end(); factor++){
for(const_iterator factor=lfg.factors_.begin(); factor!=lfg.factors_.end(); factor++){
push_back(*factor);
}
}
@ -170,9 +170,9 @@ LinearFactorGraph LinearFactorGraph::combine2(const LinearFactorGraph& lfg1,
// create new linear factor graph equal to the first one
LinearFactorGraph fg = lfg1;
// add the second factors in the graph
for (const_iterator factor = lfg2.factors.begin(); factor
!= lfg2.factors.end(); factor++) {
// add the second factors_ in the graph
for (const_iterator factor = lfg2.factors_.begin(); factor
!= lfg2.factors_.end(); factor++) {
fg.push_back(*factor);
}
@ -184,7 +184,7 @@ LinearFactorGraph LinearFactorGraph::combine2(const LinearFactorGraph& lfg1,
// find all variables and their dimensions
VariableSet LinearFactorGraph::variables() const {
VariableSet result;
BOOST_FOREACH(shared_factor factor,factors) {
BOOST_FOREACH(shared_factor factor,factors_) {
VariableSet vs = factor->variables();
BOOST_FOREACH(Variable v,vs) result.insert(v);
}
@ -217,7 +217,7 @@ pair<Matrix,Vector> LinearFactorGraph::matrix(const Ordering& ordering) const {
// get all factors
LinearFactorSet found;
BOOST_FOREACH(shared_factor factor,factors)
BOOST_FOREACH(shared_factor factor,factors_)
found.insert(factor);
// combine them

20
cpp/NonlinearFactor.h
View File

@ -15,6 +15,7 @@
#include <list>
#include <boost/shared_ptr.hpp>
#include <boost/serialization/list.hpp>
#include "Factor.h"
#include "Matrix.h"
@ -37,6 +38,18 @@ namespace gtsam {
*/
class NonlinearFactor : public Factor
{
private:
/** Serialization function */
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_);
}
protected:
Vector z_; // measurement
@ -45,9 +58,12 @@ namespace gtsam {
public:
/** 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
NonlinearFactor(const Vector& z, // the measurement
const double sigma); // the variance
/** Vector of errors */
virtual Vector error_vector(const FGConfig& c) const = 0;

4
cpp/NonlinearFactorGraph.cpp
View File

@ -27,7 +27,7 @@ LinearFactorGraph NonlinearFactorGraph::linearize(const FGConfig& config) const
LinearFactorGraph linearFG;
// linearize all factors
for(const_iterator factor=factors.begin(); factor<factors.end(); factor++){
for(const_iterator factor=factors_.begin(); factor<factors_.end(); factor++){
LinearFactor::shared_ptr lf = (*factor)->linearize(config);
linearFG.push_back(lf);
}
@ -70,7 +70,7 @@ bool check_convergence (double relativeErrorTreshold,
FGConfig NonlinearFactorGraph::iterate
(const FGConfig& config, const Ordering& ordering) const
{
LinearFactorGraph linear = linearize(config); // linearize all factors
LinearFactorGraph linear = linearize(config); // linearize all factors_
return linear.optimize(ordering);
}

14
cpp/NonlinearFactorGraph.h
View File

@ -10,17 +10,27 @@
#pragma once
#include "LinearFactorGraph.h"
#include <boost/serialization/base_object.hpp>
#include <colamd/colamd.h>
#include "FactorGraph.h"
#include "NonlinearFactor.h"
#include "LinearFactorGraph.h"
#include "ChordalBayesNet.h"
#include <colamd/colamd.h>
namespace gtsam {
/** Factor Graph Constsiting of non-linear factors */
class NonlinearFactorGraph : public FactorGraph<NonlinearFactor>
{
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::base_object< FactorGraph<NonlinearFactor> >(*this);
}
public: // internal, exposed for testing only, doc in .cpp file
FGConfig iterate(const FGConfig& config, const Ordering& ordering) const;