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

Serialization for (some) nonlinear factors now works, added virtual destructors to factor classes that needed them.

release/4.3a0
Alex Cunningham 2011-03-03 17:16:13 +00:00
parent 64591e45e4
commit a87a52035d
15 changed files with 130 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/linear/NoiseModel.cpp
View File

@ -300,6 +300,10 @@ Diagonal::Diagonal(const Vector& sigmas) :
Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)) {
}
Diagonal::Diagonal(const Vector& sigmas, const Vector& invsigmas):
Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(invsigmas) {
}
/* ************************************************************************* */
Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
if (smart) {
@ -412,7 +416,8 @@ SharedDiagonal Constrained::QR(Matrix& Ab, boost::optional<std::vector<int>&> fi
list<Triple> Rd;
Vector pseudo(m); // allocate storage for pseudo-inverse
Vector weights = emul(invsigmas_,invsigmas_); // calculate weights once
Vector invsigmas = reciprocal(sigmas_);
Vector weights = emul(invsigmas,invsigmas); // calculate weights once
// We loop over all columns, because the columns that can be eliminated
// are not necessarily contiguous. For each one, estimate the corresponding

21
gtsam/linear/NoiseModel.h
View File

@ -126,6 +126,8 @@ namespace gtsam {
typedef boost::shared_ptr<Gaussian> shared_ptr;
virtual ~Gaussian() {}
/**
* A Gaussian noise model created by specifying a square root information matrix.
* @param smart, check if can be simplified to derived class
@ -222,6 +224,9 @@ namespace gtsam {
/** sigmas and reciprocal */
Vector sigmas_, invsigmas_;
/** protected constructor for constructing constraints */
Diagonal(const Vector& sigmas, const Vector& invsigmas);
/** protected constructor takes sigmas */
Diagonal(const Vector& sigmas = ones(1));
@ -229,6 +234,8 @@ namespace gtsam {
typedef boost::shared_ptr<Diagonal> shared_ptr;
virtual ~Diagonal() {}
/**
* A diagonal noise model created by specifying a Vector of sigmas, i.e.
* standard devations, the diagonal of the square root covariance matrix.
@ -307,12 +314,16 @@ namespace gtsam {
// Instead (possibly zero) sigmas are stored in Diagonal Base class
/** protected constructor takes sigmas */
Constrained(const Vector& sigmas = zero(1)) :Diagonal(sigmas) {}
// Keeps only sigmas and calculates invsigmas when necessary
Constrained(const Vector& sigmas = zero(1)) :
Diagonal(sigmas, sigmas) {}
public:
typedef boost::shared_ptr<Constrained> shared_ptr;
virtual ~Constrained() {}
/**
* A diagonal noise model created by specifying a Vector of
* standard devations, some of which might be zero
@ -393,11 +404,13 @@ namespace gtsam {
Isotropic(size_t dim, double sigma) :
Diagonal(repeat(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
public:
/* dummy constructor to allow for serialization */
Isotropic() : Diagonal(repeat(1, 1.0)),sigma_(1.0),invsigma_(1.0) {}
public:
virtual ~Isotropic() {}
typedef boost::shared_ptr<Isotropic> shared_ptr;
/**
@ -462,6 +475,8 @@ namespace gtsam {
typedef boost::shared_ptr<Unit> shared_ptr;
virtual ~Unit() {}
/**
* Create a unit covariance noise model
*/

6
gtsam/linear/SharedGaussian.h
View File

@ -70,12 +70,6 @@ namespace gtsam { // note, deliberately not in noiseModel namespace
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
// apparently requires that subclasses be registered?
ar.template register_type<noiseModel::Gaussian>();
ar.template register_type<noiseModel::Diagonal>();
ar.template register_type<noiseModel::Constrained>();
ar.template register_type<noiseModel::Isotropic>();
ar.template register_type<noiseModel::Unit>();
ar & boost::serialization::make_nvp("SharedGaussian",
boost::serialization::base_object<noiseModel::Gaussian::shared_ptr >(*this));
}

4
gtsam/nonlinear/NonlinearEquality.h
View File

@ -70,12 +70,14 @@ namespace gtsam {
/** default constructor - only for serialization */
NonlinearEquality() {}
virtual ~NonlinearEquality() {}
/**
* Constructor - forces exact evaluation
*/
NonlinearEquality(const KEY& j, const T& feasible, bool (*compare)(const T&, const T&) = compare<T>) :
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
allow_error_(false), error_gain_(std::numeric_limits<double>::infinity()),
allow_error_(false), error_gain_(0.0),
compare_(compare) {
}

8
gtsam/nonlinear/NonlinearFactor.h
View File

@ -71,6 +71,8 @@ namespace gtsam {
NonlinearFactor() {
}
// virtual ~NonlinearFactor() {}
/**
* Constructor
* @param noiseModel shared pointer to a noise model
@ -185,6 +187,8 @@ namespace gtsam {
NonlinearFactor1() {
}
virtual ~NonlinearFactor1() {}
inline const KEY& key() const {
return key_;
}
@ -310,6 +314,8 @@ namespace gtsam {
this->keys_.push_back(key2_);
}
virtual ~NonlinearFactor2() {}
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor2\n";
@ -449,6 +455,8 @@ namespace gtsam {
this->keys_.push_back(key3_);
}
virtual ~NonlinearFactor3() {}
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor3\n";

8
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -96,7 +96,15 @@ namespace gtsam {
boost::shared_ptr<FactorGraph<JacobianFactor> >
linearize(const VALUES& config, const Ordering& ordering) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactorGraph",
boost::serialization::base_object<Base>(*this));
}
};
} // namespace

2
gtsam/slam/BearingFactor.h
View File

@ -46,6 +46,8 @@ namespace gtsam {
Base(model, i, j), z_(z) {
}
virtual ~BearingFactor() {}
/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
Vector evaluateError(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {

2
gtsam/slam/BearingRangeFactor.h
View File

@ -46,6 +46,8 @@ namespace gtsam {
Base(model, i, j), bearing_(bearing), range_(range) {
}
virtual ~BearingRangeFactor() {}
/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
Vector evaluateError(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {

2
gtsam/slam/BetweenFactor.h
View File

@ -57,6 +57,8 @@ namespace gtsam {
Base(model, key1, key2), measured_(measured) {
}
virtual ~BetweenFactor() {}
/** implement functions needed for Testable */
/** print */

2
gtsam/slam/GeneralSFMFactor.h
View File

@ -46,6 +46,8 @@ namespace gtsam {
GeneralSFMFactor(double x, double y):z_(x,y) {}
GeneralSFMFactor(const Point2& z, const SharedGaussian& model, const CamK& i, const LmK& j) : Base(model, i, j), z_(z) {}
virtual ~GeneralSFMFactor() {}
/**
* print
* @param s optional string naming the factor

2
gtsam/slam/PriorFactor.h
View File

@ -51,6 +51,8 @@ namespace gtsam {
/** default constructor - only use for serialization */
PriorFactor() {}
virtual ~PriorFactor() {}
/** Constructor */
PriorFactor(const KEY& key, const T& prior,
const SharedGaussian& model) :

2
gtsam/slam/RangeFactor.h
View File

@ -43,6 +43,8 @@ namespace gtsam {
Base(model, i, j), z_(z) {
}
virtual ~RangeFactor() {}
/** h(x)-z */
Vector evaluateError(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {

2
gtsam/slam/StereoFactor.h
View File

@ -62,6 +62,8 @@ public:
Base(model, j_pose, j_landmark), z_(z), K_(K), baseline_(baseline) {
}
virtual ~GenericStereoFactor() {}
/**
* print
* @param s optional string naming the factor

3
gtsam/slam/planarSLAM.h
View File

@ -17,6 +17,9 @@
#pragma once
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/export.hpp>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h>

103
tests/testSerialization.cpp
View File

@ -29,6 +29,7 @@
#include <boost/serialization/vector.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/list.hpp>
#include <boost/serialization/export.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
@ -153,6 +154,7 @@ bool equalsDereferencedXML(const T& input = T()) {
using namespace std;
using namespace gtsam;
using namespace planarSLAM;
/* ************************************************************************* */
TEST (Serialization, text_geometry) {
@ -195,29 +197,23 @@ TEST (Serialization, xml_linear) {
}
/* ************************************************************************* */
TEST (Serialization, Shared_noiseModels) {
SharedDiagonal diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
SharedGaussian iso3 = noiseModel::Isotropic::Sigma(3, 0.3);
SharedGaussian gaussian3 = noiseModel::Gaussian::SqrtInformation(eye(3,3));
// example noise models
noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1));
noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
EXPECT(equalsDereferenced<SharedDiagonal>(diag3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(diag3));
EXPECT(equalsDereferenced<SharedGaussian>(iso3));
EXPECT(equalsDereferencedXML<SharedGaussian>(iso3));
EXPECT(equalsDereferenced<SharedGaussian>(gaussian3));
EXPECT(equalsDereferencedXML<SharedGaussian>(gaussian3));
}
// export GUIDs for noisemodels
BOOST_CLASS_EXPORT_GUID(noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
/* ************************************************************************* */
TEST (Serialization, noiseModels) {
noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::All(3);
noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
// tests using pointers to the derived class
EXPECT( equalsDereferenced<noiseModel::Diagonal::shared_ptr>(diag3));
EXPECT(equalsDereferencedXML<noiseModel::Diagonal::shared_ptr>(diag3));
@ -227,17 +223,57 @@ TEST (Serialization, noiseModels) {
EXPECT( equalsDereferenced<noiseModel::Isotropic::shared_ptr>(iso3));
EXPECT(equalsDereferencedXML<noiseModel::Isotropic::shared_ptr>(iso3));
// FAIL: stream error
// EXPECT( equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3));
// EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3));
EXPECT( equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3));
EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3));
EXPECT( equalsDereferenced<noiseModel::Unit::shared_ptr>(unit3));
EXPECT(equalsDereferencedXML<noiseModel::Unit::shared_ptr>(unit3));
}
/* ************************************************************************* */
TEST (Serialization, SharedGaussian_noiseModels) {
EXPECT(equalsDereferenced<SharedGaussian>(diag3));
EXPECT(equalsDereferencedXML<SharedGaussian>(diag3));
EXPECT(equalsDereferenced<SharedGaussian>(iso3));
EXPECT(equalsDereferencedXML<SharedGaussian>(iso3));
EXPECT(equalsDereferenced<SharedGaussian>(gaussian3));
EXPECT(equalsDereferencedXML<SharedGaussian>(gaussian3));
EXPECT(equalsDereferenced<SharedGaussian>(unit3));
EXPECT(equalsDereferencedXML<SharedGaussian>(unit3));
EXPECT(equalsDereferenced<SharedGaussian>(constrained3));
EXPECT(equalsDereferencedXML<SharedGaussian>(constrained3));
}
/* ************************************************************************* */
TEST (Serialization, SharedDiagonal_noiseModels) {
EXPECT(equalsDereferenced<SharedDiagonal>(diag3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(diag3));
EXPECT(equalsDereferenced<SharedDiagonal>(iso3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(iso3));
EXPECT(equalsDereferenced<SharedDiagonal>(unit3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(unit3));
EXPECT(equalsDereferenced<SharedDiagonal>(constrained3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(constrained3));
}
/* ************************************************************************* */
// exporting factor classes
BOOST_CLASS_EXPORT_GUID(Prior, "gtsam_planarSLAM_Prior");
BOOST_CLASS_EXPORT_GUID(Bearing, "gtsam_planarSLAM_Bearing");
BOOST_CLASS_EXPORT_GUID(Range, "gtsam_planarSLAM_Range");
BOOST_CLASS_EXPORT_GUID(BearingRange, "gtsam_planarSLAM_BearingRange");
BOOST_CLASS_EXPORT_GUID(Odometry, "gtsam_planarSLAM_Odometry");
BOOST_CLASS_EXPORT_GUID(Constraint, "gtsam_planarSLAM_Constraint");
/* ************************************************************************* */
TEST (Serialization, planar_system) {
using namespace planarSLAM;
Values values;
values.insert(PointKey(3), Point2(1.0, 2.0));
@ -247,18 +283,19 @@ TEST (Serialization, planar_system) {
SharedGaussian model2 = noiseModel::Isotropic::Sigma(2, 0.3);
SharedGaussian model3 = noiseModel::Isotropic::Sigma(3, 0.3);
Graph graph;
Prior prior(PoseKey(3), Pose2(0.1,-0.3, 0.2), model1);
graph.add(prior);
Bearing bearing(PoseKey(3), PointKey(5), Rot2::fromDegrees(0.5), model1);
graph.add(bearing);
Range range(PoseKey(2), PointKey(9), 7.0, model1);
graph.add(range);
BearingRange bearingRange(PoseKey(2), PointKey(3), Rot2::fromDegrees(0.6), 2.0, model2);
graph.add(bearingRange);
Odometry odometry(PoseKey(2), PoseKey(3), Pose2(1.0, 2.0, 0.3), model3);
graph.add(odometry);
Constraint constraint(PoseKey(9), Pose2(2.0,-1.0, 0.2));
Graph graph;
graph.add(prior);
graph.add(bearing);
graph.add(range);
graph.add(bearingRange);
graph.add(odometry);
graph.add(constraint);
// text
@ -270,8 +307,8 @@ TEST (Serialization, planar_system) {
EXPECT(equalsObj<BearingRange>(bearingRange));
EXPECT(equalsObj<Range>(range));
EXPECT(equalsObj<Odometry>(odometry));
// EXPECT(equalsObj<Constraint>(constraint)); // FAIL: stream error
// EXPECT(equalsObj<Graph>(graph)); // FAIL: segfaults if there are factors
EXPECT(equalsObj<Constraint>(constraint));
EXPECT(equalsObj<Graph>(graph));
// xml
EXPECT(equalsXML<PoseKey>(PoseKey(2)));
@ -282,8 +319,8 @@ TEST (Serialization, planar_system) {
EXPECT(equalsXML<BearingRange>(bearingRange));
EXPECT(equalsXML<Range>(range));
EXPECT(equalsXML<Odometry>(odometry));
// EXPECT(equalsXML<Constraint>(constraint)); // FAIL: stream error
// EXPECT(equalsXML<Graph>(graph));
EXPECT(equalsXML<Constraint>(constraint));
EXPECT(equalsXML<Graph>(graph));
}
/* ************************************************************************* */