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

Moved serialization support into ExpressionFactor

release/4.3a0
Frank Dellaert 2015-07-13 23:43:57 -07:00
parent b97afe338f
commit 6b51ef994c
5 changed files with 92 additions and 143 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
gtsam/nonlinear/ExpressionFactor.h
View File

@ -23,10 +23,10 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h>
#include <numeric>
namespace gtsam {
/**
* Factor that supports arbitrary expressions via AD
*/
template<typename T>
@ -132,10 +132,10 @@ public:
}
protected:
/// Default constructor, for serialization
ExpressionFactor() {}
/// Default constructor, for serialization
/// Constructor for use by SerializableExpressionFactor
/// Constructor for serializable derived classes
ExpressionFactor(const SharedNoiseModel& noiseModel, const T& measurement)
: NoiseModelFactor(noiseModel), measured_(measurement) {
// Not properly initialized yet, need to call initialize
@ -155,22 +155,91 @@ protected:
boost::tie(keys_, dims_) = expression_.keysAndDims();
}
private:
/// Serialization function
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor);
ar& boost::serialization::make_nvp("measured_", this->measured_);
/// Recreate expression from keys_ and measured_, used in load below.
/// Needed to deserialize a derived factor
virtual Expression<T> expression() const {
throw std::runtime_error("ExpressionFactor::expression not provided: cannot deserialize.");
}
private:
/// Save to an archive: just saves the base class
template <class Archive>
void save(Archive& ar, const unsigned int /*version*/) const {
ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor);
ar << boost::serialization::make_nvp("measured_", this->measured_);
}
/// Load from an archive, creating a valid expression using the overloaded
/// [expression] method
template <class Archive>
void load(Archive& ar, const unsigned int /*version*/) {
ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor);
ar >> boost::serialization::make_nvp("measured_", this->measured_);
this->initialize(expression());
}
// Indicate that we implement save/load separately, and be friendly to boost
BOOST_SERIALIZATION_SPLIT_MEMBER()
friend class boost::serialization::access;
};
// ExpressionFactor
/// traits
template <typename T>
struct traits<ExpressionFactor<T> > : public Testable<ExpressionFactor<T> > {};
/**
* Binary specialization of ExpressionFactor meant as a base class for binary factors
* Enforces expression method with two keys, and provides evaluateError
* Derived needs to call initialize.
*/
template <typename T, typename A1, typename A2>
class ExpressionFactor2 : public ExpressionFactor<T> {
public:
/// Destructor
virtual ~ExpressionFactor2() {}
/// Backwards compatible evaluateError, to make existing tests compile
Vector evaluateError(const A1& a1, const A2& a2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
Values values;
values.insert(this->keys_[0], a1);
values.insert(this->keys_[1], a2);
std::vector<Matrix> H(2);
Vector error = this->unwhitenedError(values, H);
if (H1) (*H1) = H[0];
if (H2) (*H2) = H[1];
return error;
}
/// Recreate expression from given keys_ and measured_, used in load
/// Needed to deserialize a derived factor
virtual Expression<T> expression(Key key1, Key key2) const {
throw std::runtime_error("ExpressionFactor2::expression not provided: cannot deserialize.");
}
protected:
/// Default constructor, for serialization
ExpressionFactor2() {}
/// Constructor takes care of keys, but still need to call initialize
ExpressionFactor2(Key key1, Key key2,
const SharedNoiseModel& noiseModel,
const T& measurement)
: ExpressionFactor<T>(noiseModel, measurement) {
this->keys_.push_back(key1);
this->keys_.push_back(key2);
}
private:
/// Return an expression that predicts the measurement given Values
virtual Expression<T> expression() const {
return expression(this->keys_[0], this->keys_[1]);
}
};
// ExpressionFactor2
}// \ namespace gtsam

120
gtsam/nonlinear/SerializableExpressionFactor.h
View File

@ -1,120 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SerializableExpressionFactor.h
* @date July 2015
* @author Frank Dellaert
* @brief ExpressionFactor variant that supports serialization
*/
#pragma once
#include <gtsam/nonlinear/ExpressionFactor.h>
namespace gtsam {
/**
* ExpressionFactor variant that supports serialization
* Simply overload the pure virtual method [expression] to construct an
* expression from keys_
*/
template <typename T>
class SerializableExpressionFactor : public ExpressionFactor<T> {
public:
/// Constructor takes only two arguments, still need to call initialize
SerializableExpressionFactor(const SharedNoiseModel& noiseModel,
const T& measurement)
: ExpressionFactor<T>(noiseModel, measurement) {}
/// Destructor
virtual ~SerializableExpressionFactor() {}
protected:
/// Return an expression that predicts the measurement given Values
virtual Expression<T> expression() const = 0;
/// Default constructor, for serialization
SerializableExpressionFactor() {}
/// Save to an archive: just saves the base class
template <class Archive>
void save(Archive& ar, const unsigned int /*version*/) const {
ar << boost::serialization::make_nvp(
"ExpressionFactor",
boost::serialization::base_object<ExpressionFactor<T> >(*this));
}
/// Load from an archive, creating a valid expression using the overloaded
/// [expression] method
template <class Archive>
void load(Archive& ar, const unsigned int /*version*/) {
ar >> boost::serialization::make_nvp(
"ExpressionFactor",
boost::serialization::base_object<ExpressionFactor<T> >(*this));
this->initialize(expression());
}
// Indicate that we implement save/load separately, and be friendly to boost
BOOST_SERIALIZATION_SPLIT_MEMBER()
friend class boost::serialization::access;
};
// SerializableExpressionFactor
/**
* Binary specialization of SerializableExpressionFactor
* Enforces expression method with two keys, and provides evaluateError
*/
template <typename T, typename A1, typename A2>
class SerializableExpressionFactor2 : public SerializableExpressionFactor<T> {
public:
/// Constructor takes care of keys, but still need to call initialize
SerializableExpressionFactor2(Key key1, Key key2,
const SharedNoiseModel& noiseModel,
const T& measurement)
: SerializableExpressionFactor<T>(noiseModel, measurement) {
this->keys_.push_back(key1);
this->keys_.push_back(key2);
}
/// Destructor
virtual ~SerializableExpressionFactor2() {}
/// Backwards compatible evaluateError, to make existing tests compile
Vector evaluateError(const A1& a1, const A2& a2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
Values values;
values.insert(this->keys_[0], a1);
values.insert(this->keys_[1], a2);
std::vector<Matrix> H(2);
Vector error = this->unwhitenedError(values, H);
if (H1) (*H1) = H[0];
if (H2) (*H2) = H[1];
return error;
}
/// Return an expression that predicts the measurement given Values
virtual Expression<T> expression(Key key1, Key key2) const = 0;
protected:
/// Default constructor, for serialization
SerializableExpressionFactor2() {}
private:
/// Return an expression that predicts the measurement given Values
virtual Expression<T> expression() const {
return expression(this->keys_[0], this->keys_[1]);
}
};
// SerializableExpressionFactor2
} // \ namespace gtsam

6
gtsam/sam/BearingFactor.h
View File

@ -18,7 +18,7 @@
#pragma once
#include <gtsam/nonlinear/SerializableExpressionFactor.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
namespace gtsam {
@ -34,8 +34,8 @@ struct Bearing;
*/
template <typename A1, typename A2,
typename T = typename Bearing<A1, A2>::result_type>
struct BearingFactor : public SerializableExpressionFactor2<T, A1, A2> {
typedef SerializableExpressionFactor2<T, A1, A2> Base;
struct BearingFactor : public ExpressionFactor2<T, A1, A2> {
typedef ExpressionFactor2<T, A1, A2> Base;
/// default constructor
BearingFactor() {}

9
gtsam/sam/BearingRangeFactor.h
View File

@ -19,7 +19,7 @@
#pragma once
#include <gtsam/nonlinear/SerializableExpressionFactor.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/geometry/BearingRange.h>
#include <boost/concept/assert.hpp>
@ -33,10 +33,10 @@ template <typename A1, typename A2,
typename B = typename Bearing<A1, A2>::result_type,
typename R = typename Range<A1, A2>::result_type>
class BearingRangeFactor
: public SerializableExpressionFactor2<BearingRange<A1, A2>, A1, A2> {
: public ExpressionFactor2<BearingRange<A1, A2>, A1, A2> {
private:
typedef BearingRange<A1, A2> T;
typedef SerializableExpressionFactor2<T, A1, A2> Base;
typedef ExpressionFactor2<T, A1, A2> Base;
typedef BearingRangeFactor<A1, A2> This;
public:
@ -62,7 +62,8 @@ class BearingRangeFactor
// Return measurement expression
virtual Expression<T> expression(Key key1, Key key2) const {
return Expression<T>(T::Measure, Expression<A1>(key1), Expression<A2>(key2));
return Expression<T>(T::Measure, Expression<A1>(key1),
Expression<A2>(key2));
}
/// print

11
gtsam/sam/RangeFactor.h
View File

@ -18,7 +18,7 @@
#pragma once
#include <gtsam/nonlinear/SerializableExpressionFactor.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
namespace gtsam {
@ -33,10 +33,10 @@ struct Range;
*/
template <typename A1, typename A2 = A1,
typename T = typename Range<A1, A2>::result_type>
class RangeFactor : public SerializableExpressionFactor2<T, A1, A2> {
class RangeFactor : public ExpressionFactor2<T, A1, A2> {
private:
typedef RangeFactor<A1, A2> This;
typedef SerializableExpressionFactor2<T, A1, A2> Base;
typedef ExpressionFactor2<T, A1, A2> Base;
public:
/// default constructor
@ -79,11 +79,10 @@ struct traits<RangeFactor<A1, A2, T> >
*/
template <typename A1, typename A2 = A1,
typename T = typename Range<A1, A2>::result_type>
class RangeFactorWithTransform
: public SerializableExpressionFactor2<T, A1, A2> {
class RangeFactorWithTransform : public ExpressionFactor2<T, A1, A2> {
private:
typedef RangeFactorWithTransform<A1, A2> This;
typedef SerializableExpressionFactor2<T, A1, A2> Base;
typedef ExpressionFactor2<T, A1, A2> Base;
A1 body_T_sensor_; ///< The pose of the sensor in the body frame