From ed0d66cf62250f0c89003f23bceb18fb339ca9cc Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 11 Jul 2015 15:55:01 -0700 Subject: [PATCH] Fully serializable expression factors --- gtsam/nonlinear/ExpressionFactor.h | 97 ++++++++++++++++++++++----- gtsam/sam/BearingFactor.h | 47 ++++--------- gtsam/sam/tests/testBearingFactor.cpp | 41 +++++++---- 3 files changed, 121 insertions(+), 64 deletions(-) diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index f1ff36c1e..53c183c91 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -46,19 +46,10 @@ public: typedef boost::shared_ptr > shared_ptr; /// Constructor - ExpressionFactor(const SharedNoiseModel& noiseModel, // - const T& measurement, const Expression& expression) : - measurement_(measurement), expression_(expression) { - if (!noiseModel) - throw std::invalid_argument("ExpressionFactor: no NoiseModel."); - if (noiseModel->dim() != Dim) - throw std::invalid_argument( - "ExpressionFactor was created with a NoiseModel of incorrect dimension."); - noiseModel_ = noiseModel; - - // Get keys and dimensions for Jacobian matrices - // An Expression is assumed unmutable, so we do this now - boost::tie(keys_, dims_) = expression_.keysAndDims(); + ExpressionFactor(const SharedNoiseModel& noiseModel, // + const T& measurement, const Expression& expression) + : NoiseModelFactor(noiseModel), measurement_(measurement) { + initialize(expression); } /// print relies on Testable traits being defined for T @@ -71,7 +62,8 @@ public: bool equals(const NonlinearFactor& f, double tol) const { const ExpressionFactor* p = dynamic_cast(&f); return p && NoiseModelFactor::equals(f, tol) && - traits::Equals(measurement_, p->measurement_, tol); + traits::Equals(measurement_, p->measurement_, tol) && + dims_ == p->dims_; } /** @@ -132,13 +124,84 @@ public: gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - protected: - /// Default constructor, for serialization - ExpressionFactor() {} +protected: + /// Default constructor, for serialization + ExpressionFactor() {} + /// Constructor for use by SerializableExpressionFactor + ExpressionFactor(const SharedNoiseModel& noiseModel, const T& measurement) + : NoiseModelFactor(noiseModel), measurement_(measurement) { + // Not properly initialized yet, need to call initialize + } + + /// Initialize with constructor arguments + void initialize(const Expression& expression) { + if (!noiseModel_) + throw std::invalid_argument("ExpressionFactor: no NoiseModel."); + if (noiseModel_->dim() != Dim) + throw std::invalid_argument( + "ExpressionFactor was created with a NoiseModel of incorrect dimension."); + expression_ = expression; + + // Get keys and dimensions for Jacobian matrices + // An Expression is assumed unmutable, so we do this now + boost::tie(keys_, dims_) = expression_.keysAndDims(); + } + +private: + /// Serialization function + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor); + ar& boost::serialization::make_nvp("measurement_", this->measurement_); + } + + friend class boost::serialization::access; }; // ExpressionFactor + +/** + * ExpressionFactor variant that supports serialization + * Simply overload the pure virtual method [expression] to construct an expression from keys_ + */ +template +class SerializableExpressionFactor : public ExpressionFactor { + public: + /// Constructor takes only two arguments, still need to call initialize + SerializableExpressionFactor(const SharedNoiseModel& noiseModel, const T& measurement) + : ExpressionFactor(noiseModel, measurement) { + } + + protected: + + /// Return an expression that predicts the measurement given Values + virtual Expression expression() const = 0; + + /// Default constructor, for serialization + SerializableExpressionFactor() {} + + /// Save to an archive: just saves the base class + template + void save(Archive& ar, const unsigned int /*version*/) const { + ar << boost::serialization::make_nvp( + "ExpressionFactor", boost::serialization::base_object >(*this)); + } + + /// Load from an archive, creating a valid expression using the overloaded [expression] method + template + void load(Archive& ar, const unsigned int /*version*/) { + ar >> boost::serialization::make_nvp( + "ExpressionFactor", boost::serialization::base_object >(*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 + /// traits template struct traits > : public Testable > {}; diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h index 9d5105604..87effa981 100644 --- a/gtsam/sam/BearingFactor.h +++ b/gtsam/sam/BearingFactor.h @@ -27,16 +27,6 @@ template void serialize(Archive& ar, gtsam::NonlinearFactor& factor, const unsigned int version) {} -// template -// void serialize(Archive& ar, gtsam::ExpressionFactor& factor, -// const unsigned int version) { -// ar& BOOST_SERIALIZATION_NVP(factor.measurement_); -//} - -// template -// void serialize(Archive& ar, Factor& factor, const unsigned int version) { -//} - } // namespace serialization } // namespace boost @@ -47,10 +37,10 @@ namespace gtsam { * @addtogroup SAM */ template -class BearingFactor : public ExpressionFactor { +class BearingFactor : public SerializableExpressionFactor { private: typedef BearingFactor This; - typedef ExpressionFactor Base; + typedef SerializableExpressionFactor Base; /** concept check by type */ GTSAM_CONCEPT_TESTABLE_TYPE(Measured) @@ -61,37 +51,28 @@ class BearingFactor : public ExpressionFactor { BearingFactor() {} /// primary constructor - BearingFactor(Key poseKey, Key pointKey, const Measured& measured, - const SharedNoiseModel& model) - : Base(model, measured, - Expression(Expression(poseKey), &Pose::bearing, - Expression(pointKey))) {} + BearingFactor(Key poseKey, Key pointKey, const Measured& measured, const SharedNoiseModel& model) + : Base(model, measured) { + this->keys_.push_back(poseKey); + this->keys_.push_back(pointKey); + this->initialize(expression()); + } virtual ~BearingFactor() {} /** print contents */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "BearingFactor, bearing = "; - this->measurement_.print(); - Base::print("", keyFormatter); + std::cout << s << "BearingFactor" << std::endl; + Base::print(s, keyFormatter); } private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp( - "NoiseModelFactor", - boost::serialization::base_object(*this)); - ar& boost::serialization::make_nvp("measurement_", this->measurement_); + // Return an expression + virtual Expression expression() const { + return Expression(Expression(this->keys_[0]), &Pose::bearing, + Expression(this->keys_[1])); } - - template - friend void boost::serialization::serialize(Archive& ar, Base& factor, - const unsigned int version); - }; // BearingFactor /// traits diff --git a/gtsam/sam/tests/testBearingFactor.cpp b/gtsam/sam/tests/testBearingFactor.cpp index c3cb55b0d..b1cbfca8f 100644 --- a/gtsam/sam/tests/testBearingFactor.cpp +++ b/gtsam/sam/tests/testBearingFactor.cpp @@ -29,30 +29,41 @@ using namespace std; using namespace gtsam; // Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Unit::Create(1)); - -typedef BearingFactor BearingFactor2D; -typedef BearingFactor BearingFactor3D; +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5)); Key poseKey(1); Key pointKey(2); +typedef BearingFactor BearingFactor2D; +double measurement2D(10.0); +BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model); GTSAM_CONCEPT_TESTABLE_INST(BearingFactor2D) +typedef BearingFactor BearingFactor3D; + /* ************************************************************************* */ // Export Noisemodels // See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic); +//BOOST_CLASS_EXPORT(BearingFactor2D); +//BOOST_CLASS_EXPORT(ExpressionFactor); +//BOOST_CLASS_EXPORT_GUID(gtsam::ExpressionFactor, "ExpressionFactorRot2") + +/* ************************************************************************* */ +TEST(BearingFactor, Serialization2D) { + EXPECT(serializationTestHelpers::equalsObj(factor2D)); + EXPECT(serializationTestHelpers::equalsXML(factor2D)); + EXPECT(serializationTestHelpers::equalsBinary(factor2D)); +} /* ************************************************************************* */ TEST(BearingFactor, 2D) { - // Create a factor - double measurement(10.0); - BearingFactor2D factor(poseKey, pointKey, measurement, model); - std::string serialized = serializeXML(factor); - cout << serialized << endl; + // Serialize the factor + std::string serialized = serializeXML(factor2D); - EXPECT(serializationTestHelpers::equalsObj(factor)); + // And de-serialize it + BearingFactor2D factor; + deserializeXML(serialized, factor); // Set the linearization point Values values; @@ -63,15 +74,17 @@ TEST(BearingFactor, 2D) { } /* ************************************************************************* */ -// TODO(frank): this is broken: (non-existing) Pose3::bearing should yield a Unit3 -//TEST(BearingFactor, 3D) { +// TODO(frank): this is broken: (non-existing) Pose3::bearing should yield a +// Unit3 +// TEST(BearingFactor, 3D) { // // Create a factor // Rot3 measurement; // BearingFactor factor(poseKey, pointKey, measurement, model); // // // Set the linearization point // Values values; -// values.insert(poseKey, Pose3(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0))); +// values.insert(poseKey, Pose3(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, +// -3.0))); // values.insert(pointKey, Point3(-2.0, 11.0, 1.0)); // // EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);