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

Added serialization for most linear/inference structures, simulated2D - serialization, except for example domains, should be complete

release/4.3a0
Alex Cunningham 2011-06-04 16:52:54 +00:00
parent 265b057580
commit c4d9208eeb
9 changed files with 244 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/inference/BayesTree.h
View File

@ -21,8 +21,6 @@
#include <list>
#include <vector>
//#include <boost/serialization/map.hpp>
//#include <boost/serialization/list.hpp>
#include <stdexcept>
#include <deque>
@ -60,6 +58,7 @@ namespace gtsam {
void assertInvariants() const;
public:
typedef BayesNet<CONDITIONAL> Base;
typedef typename boost::shared_ptr<Clique> shared_ptr;
typedef typename boost::weak_ptr<Clique> weak_ptr;
weak_ptr parent_;
@ -123,6 +122,18 @@ namespace gtsam {
/** return the joint P(C1,C2), where C1==this. TODO: not a method? */
FactorGraph<FactorType> joint(shared_ptr C2, shared_ptr root, Eliminate function);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(parent_);
ar & BOOST_SERIALIZATION_NVP(children_);
ar & BOOST_SERIALIZATION_NVP(separator_);
ar & BOOST_SERIALIZATION_NVP(cachedFactor_);
}
}; // \struct Clique
// typedef for shared pointers to cliques
@ -313,6 +324,15 @@ namespace gtsam {
template<class CONTAINER>
void removeTop(const CONTAINER& keys, BayesNet<CONDITIONAL>& bn, Cliques& orphans);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(nodes_);
ar & BOOST_SERIALIZATION_NVP(root_);
}
}; // BayesTree
} /// namespace gtsam

1
gtsam/inference/Conditional.h
View File

@ -163,6 +163,7 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
}
};

1
gtsam/inference/Factor.h
View File

@ -67,7 +67,6 @@ public:
/** Const iterator over keys */
typedef typename std::vector<Key>::const_iterator const_iterator;
//private: // FIXME: factors that manually initialize keys_ break in this case
protected:
// The keys involved in this factor

8
gtsam/inference/IndexFactor.h
View File

@ -130,6 +130,14 @@ namespace gtsam {
virtual ~IndexFactor() {
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
}; // IndexFactor
}

8
gtsam/linear/GaussianFactor.h
View File

@ -94,6 +94,14 @@ namespace gtsam {
*/
virtual void permuteWithInverse(const Permutation& inversePermutation) = 0;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(IndexFactor);
}
}; // GaussianFactor
/** make keys from list, vector, or map of matrices */

9
gtsam/linear/GaussianFactorGraph.h
View File

@ -71,6 +71,7 @@ namespace gtsam {
public:
typedef boost::shared_ptr<GaussianFactorGraph> shared_ptr;
typedef FactorGraph<GaussianFactor> Base;
/**
* Default constructor
@ -165,6 +166,14 @@ namespace gtsam {
// // allocate a vectorvalues of b's structure
// VectorValues allocateVectorValuesb() const ;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
};
/**

11
gtsam/nonlinear/Ordering.h
View File

@ -131,11 +131,20 @@ public:
/** equals (from Testable) for testing and debugging */
bool equals(const Ordering& rhs, double tol = 0.0) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(order_);
ar & BOOST_SERIALIZATION_NVP(nVars_);
}
};
/**
* @class Unordered
* @brief a set of unordered indice
* @brief a set of unordered indices
*/
class Unordered: public std::set<Index>, public Testable<Unordered> {
public:

38
gtsam/slam/simulated2D.h
View File

@ -66,7 +66,9 @@ namespace gtsam {
* Unary factor encoding a soft prior on a vector
*/
template<class CFG = Values, class KEY = PoseKey>
struct GenericPrior: public NonlinearFactor1<CFG, KEY> {
class GenericPrior: public NonlinearFactor1<CFG, KEY> {
public:
typedef NonlinearFactor1<CFG, KEY> Base;
typedef boost::shared_ptr<GenericPrior<CFG, KEY> > shared_ptr;
typedef typename KEY::Value Pose;
Pose z_;
@ -80,13 +82,24 @@ namespace gtsam {
return (prior(x, H) - z_).vector();
}
private:
GenericPrior() {}
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(z_);
}
};
/**
* Binary factor simulating "odometry" between two Vectors
*/
template<class CFG = Values, class KEY = PoseKey>
struct GenericOdometry: public NonlinearFactor2<CFG, KEY, KEY> {
class GenericOdometry: public NonlinearFactor2<CFG, KEY, KEY> {
public:
typedef NonlinearFactor2<CFG, KEY, KEY> Base;
typedef boost::shared_ptr<GenericOdometry<CFG, KEY> > shared_ptr;
typedef typename KEY::Value Pose;
Pose z_;
@ -101,6 +114,15 @@ namespace gtsam {
return (odo(x1, x2, H1, H2) - z_).vector();
}
private:
GenericOdometry() {}
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(z_);
}
};
/**
@ -109,6 +131,7 @@ namespace gtsam {
template<class CFG = Values, class XKEY = PoseKey, class LKEY = PointKey>
class GenericMeasurement: public NonlinearFactor2<CFG, XKEY, LKEY> {
public:
typedef NonlinearFactor2<CFG, XKEY, LKEY> Base;
typedef boost::shared_ptr<GenericMeasurement<CFG, XKEY, LKEY> > shared_ptr;
typedef typename XKEY::Value Pose;
typedef typename LKEY::Value Point;
@ -117,7 +140,7 @@ namespace gtsam {
GenericMeasurement(const Point& z, const SharedGaussian& model,
const XKEY& i, const LKEY& j) :
NonlinearFactor2<CFG, XKEY, LKEY> (model, i, j), z_(z) {
NonlinearFactor2<CFG, XKEY, LKEY> (model, i, j), z_(z) {
}
Vector evaluateError(const Pose& x1, const Point& x2, boost::optional<
@ -125,6 +148,15 @@ namespace gtsam {
return (mea(x1, x2, H1, H2) - z_).vector();
}
private:
GenericMeasurement() {}
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(z_);
}
};
/** Typedefs for regular use */

175
tests/testSerialization.cpp
View File

@ -9,7 +9,7 @@
* -------------------------------------------------------------------------- */
/*
/**
* @brief Unit tests for serialization of library classes
* @author Frank Dellaert
* @author Alex Cunningham
@ -30,6 +30,8 @@
#include <boost/serialization/vector.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/list.hpp>
#include <boost/serialization/deque.hpp>
#include <boost/serialization/weak_ptr.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
@ -86,25 +88,30 @@ bool equalsDereferenced(const T& input) {
}
/* ************************************************************************* */
template<class T>
std::string serializeXML(const T& input) {
std::ostringstream out_archive_stream;
boost::archive::xml_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp("data", input);
return out_archive_stream.str();
}
template<class T>
void deserializeXML(const std::string& serialized, T& output) {
std::istringstream in_archive_stream(serialized);
boost::archive::xml_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp("data", output);
}
// Templated round-trip serialization using XML
template<class T>
void roundtripXML(const T& input, T& output) {
// Serialize
std::string serialized;
{
std::ostringstream out_archive_stream;
boost::archive::xml_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp("data", input);
// Convert to string
serialized = out_archive_stream.str();
}
std::string serialized = serializeXML<T>(input);
if (verbose) std::cout << serialized << std::endl << std::endl;
// De-serialize
std::istringstream in_archive_stream(serialized);
boost::archive::xml_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp("data", output);
deserializeXML(serialized, output);
}
// This version requires equality operator
@ -135,6 +142,8 @@ bool equalsDereferencedXML(const T& input = T()) {
// Actual Tests
/* ************************************************************************* */
#define GTSAM_MAGIC_KEY
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Pose2.h>
@ -303,9 +312,11 @@ TEST (Serialization, SharedDiagonal_noiseModels) {
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/slam/smallExample.h>
#include <gtsam/nonlinear/Ordering.h>
/* ************************************************************************* */
TEST (Serialization, linear) {
TEST (Serialization, linear_factors) {
vector<size_t> dims;
dims.push_back(1);
dims.push_back(2);
@ -326,15 +337,132 @@ TEST (Serialization, linear) {
HessianFactor hessianfactor(jacobianfactor);
EXPECT(equalsObj(hessianfactor));
EXPECT(equalsXML(hessianfactor));
{
Matrix A1 = Matrix_(2,2, 1., 2., 3., 4.);
Matrix A2 = Matrix_(2,2, 6., 0.2, 8., 0.4);
Matrix R = Matrix_(2,2, 0.1, 0.3, 0.0, 0.34);
Vector d(2); d << 0.2, 0.5;
GaussianConditional cg(0, d, R, 1, A1, 2, A2, ones(2));
// EXPECT(equalsObj(cg)); // FAILS: does not match
// EXPECT(equalsXML(cg)); // FAILS: does not match
}
}
/* ************************************************************************* */
TEST (Serialization, gaussian_conditional) {
Matrix A1 = Matrix_(2,2, 1., 2., 3., 4.);
Matrix A2 = Matrix_(2,2, 6., 0.2, 8., 0.4);
Matrix R = Matrix_(2,2, 0.1, 0.3, 0.0, 0.34);
Vector d(2); d << 0.2, 0.5;
GaussianConditional cg(0, d, R, 1, A1, 2, A2, ones(2));
EXPECT(equalsObj(cg));
EXPECT(equalsXML(cg));
}
/* Create GUIDs for factors */
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
/* ************************************************************************* */
TEST (Serialization, smallExample_linear) {
using namespace example;
Ordering ordering; ordering += "x1","x2","l1";
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
EXPECT(equalsObj(ordering));
EXPECT(equalsXML(ordering));
EXPECT(equalsObj(fg));
EXPECT(equalsXML(fg));
GaussianBayesNet cbn = createSmallGaussianBayesNet();
EXPECT(equalsObj(cbn));
EXPECT(equalsXML(cbn));
}
/* ************************************************************************* */
TEST (Serialization, symbolic_graph) {
Ordering o; o += "x1","l1","x2";
// construct expected symbolic graph
SymbolicFactorGraph sfg;
sfg.push_factor(o["x1"]);
sfg.push_factor(o["x1"],o["x2"]);
sfg.push_factor(o["x1"],o["l1"]);
sfg.push_factor(o["l1"],o["x2"]);
EXPECT(equalsObj(sfg));
EXPECT(equalsXML(sfg));
}
/* ************************************************************************* */
TEST (Serialization, symbolic_bn) {
Ordering o; o += "x2","l1","x1";
// create expected Chordal bayes Net
IndexConditional::shared_ptr x2(new IndexConditional(o["x2"], o["l1"], o["x1"]));
IndexConditional::shared_ptr l1(new IndexConditional(o["l1"], o["x1"]));
IndexConditional::shared_ptr x1(new IndexConditional(o["x1"]));
SymbolicBayesNet sbn;
sbn.push_back(x2);
sbn.push_back(l1);
sbn.push_back(x1);
EXPECT(equalsObj(sbn));
EXPECT(equalsXML(sbn));
}
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/inference/IndexFactor.h>
/* ************************************************************************* */
TEST (Serialization, symbolic_bayes_tree ) {
typedef BayesTree<IndexConditional> SymbolicBayesTree;
static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5;
IndexConditional::shared_ptr
B(new IndexConditional(_B_)),
L(new IndexConditional(_L_, _B_)),
E(new IndexConditional(_E_, _L_, _B_)),
S(new IndexConditional(_S_, _L_, _B_)),
T(new IndexConditional(_T_, _E_, _L_)),
X(new IndexConditional(_X_, _E_));
// Bayes Tree for Asia example
SymbolicBayesTree bayesTree;
bayesTree.insert(B);
bayesTree.insert(L);
bayesTree.insert(E);
bayesTree.insert(S);
bayesTree.insert(T);
bayesTree.insert(X);
EXPECT(equalsObj(bayesTree));
EXPECT(equalsXML(bayesTree));
}
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianISAM.h>
/* ************************************************************************* */
TEST (Serialization, gaussianISAM) {
using namespace example;
Ordering ordering;
GaussianFactorGraph smoother;
boost::tie(smoother, ordering) = createSmoother(7);
GaussianBayesNet chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate();
GaussianISAM bayesTree(chordalBayesNet);
EXPECT(equalsObj(bayesTree));
EXPECT(equalsXML(bayesTree));
}
/* ************************************************************************* */
/* Create GUIDs for factors in simulated2D */
BOOST_CLASS_EXPORT_GUID(gtsam::simulated2D::Prior, "gtsam::simulated2D::Prior" );
BOOST_CLASS_EXPORT_GUID(gtsam::simulated2D::Odometry, "gtsam::simulated2D::Odometry" );
BOOST_CLASS_EXPORT_GUID(gtsam::simulated2D::Measurement, "gtsam::simulated2D::Measurement");
/* ************************************************************************* */
TEST (Serialization, smallExample) {
using namespace example;
Graph nfg = createNonlinearFactorGraph();
Values c1 = createValues();
EXPECT(equalsObj(nfg));
EXPECT(equalsXML(nfg));
EXPECT(equalsObj(c1));
EXPECT(equalsXML(c1));
}
/* ************************************************************************* */
@ -345,7 +473,6 @@ BOOST_CLASS_EXPORT_GUID(gtsam::planarSLAM::Range, "gtsam::planarSLAM::Rang
BOOST_CLASS_EXPORT_GUID(gtsam::planarSLAM::BearingRange,"gtsam::planarSLAM::BearingRange");
BOOST_CLASS_EXPORT_GUID(gtsam::planarSLAM::Odometry, "gtsam::planarSLAM::Odometry");
BOOST_CLASS_EXPORT_GUID(gtsam::planarSLAM::Constraint, "gtsam::planarSLAM::Constraint");
/* ************************************************************************* */
TEST (Serialization, planar_system) {
using namespace planarSLAM;