From 3d4ea47ab9fd1018f384bc3a693042788467a3d5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 12 Jul 2015 12:06:13 -0700 Subject: [PATCH] Added binary specialization --- .../nonlinear/SerializableExpressionFactor.h | 73 +++++++++++++++---- gtsam/sam/BearingFactor.h | 29 ++++---- gtsam/sam/tests/testBearingFactor.cpp | 10 ++- 3 files changed, 81 insertions(+), 31 deletions(-) diff --git a/gtsam/nonlinear/SerializableExpressionFactor.h b/gtsam/nonlinear/SerializableExpressionFactor.h index 09b4faa57..797bce4fd 100644 --- a/gtsam/nonlinear/SerializableExpressionFactor.h +++ b/gtsam/nonlinear/SerializableExpressionFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -24,21 +24,21 @@ namespace gtsam { /** * ExpressionFactor variant that supports serialization - * Simply overload the pure virtual method [expression] to construct an expression from keys_ + * 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) { - } + SerializableExpressionFactor(const SharedNoiseModel& noiseModel, + const T& measurement) + : ExpressionFactor(noiseModel, measurement) {} /// Destructor virtual ~SerializableExpressionFactor() {} protected: - /// Return an expression that predicts the measurement given Values virtual Expression expression() const = 0; @@ -49,14 +49,17 @@ class SerializableExpressionFactor : public ExpressionFactor { template void save(Archive& ar, const unsigned int /*version*/) const { ar << boost::serialization::make_nvp( - "ExpressionFactor", boost::serialization::base_object >(*this)); + "ExpressionFactor", + boost::serialization::base_object >(*this)); } - /// Load from an archive, creating a valid expression using the overloaded [expression] method + /// 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)); + "ExpressionFactor", + boost::serialization::base_object >(*this)); this->initialize(expression()); } @@ -66,10 +69,52 @@ class SerializableExpressionFactor : public ExpressionFactor { }; // SerializableExpressionFactor -/// traits -template -struct traits > - : public Testable > {}; +/** + * Binary specialization of SerializableExpressionFactor + * Enforces expression method with two keys, and provides evaluateError + */ +template +class SerializableExpressionFactor2 : public SerializableExpressionFactor { + public: + /// Constructor takes care of keys, but still need to call initialize + SerializableExpressionFactor2(Key key1, Key key2, + const SharedNoiseModel& noiseModel, + const T& measurement) + : SerializableExpressionFactor(noiseModel, measurement) { + this->keys_.push_back(key1); + this->keys_.push_back(key2); + } -}// \ namespace gtsam + /// Destructor + virtual ~SerializableExpressionFactor2() {} + /// Backwards compatible evaluateError, to make existing tests compile + Vector evaluateError(const A1& a1, const A2& a2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + Values values; + values.insert(this->keys_[0], a1); + values.insert(this->keys_[1], a2); + std::vector 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 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 expression() const { + return expression(this->keys_[0], this->keys_[1]); + } +}; +// SerializableExpressionFactor2 + +} // \ namespace gtsam diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h index 15fce6239..06543e13c 100644 --- a/gtsam/sam/BearingFactor.h +++ b/gtsam/sam/BearingFactor.h @@ -11,7 +11,8 @@ /** * @file BearingFactor.h - * @brief Serializable factor induced by a bearing measurement of a point from a given pose + * @brief Serializable factor induced by a bearing measurement of a point from + *a given pose * @date July 2015 * @author Frank Dellaert **/ @@ -28,34 +29,36 @@ namespace gtsam { * @addtogroup SAM */ template -struct BearingFactor : public SerializableExpressionFactor { +struct BearingFactor : public SerializableExpressionFactor2 { + typedef SerializableExpressionFactor2 Base; /// default constructor BearingFactor() {} /// primary constructor - BearingFactor(Key poseKey, Key pointKey, const T& measured, const SharedNoiseModel& model) - : SerializableExpressionFactor(model, measured) { - this->keys_.push_back(poseKey); - this->keys_.push_back(pointKey); - this->initialize(expression()); + BearingFactor(Key key1, Key key2, const T& measured, + const SharedNoiseModel& model) + : Base(key1, key2, model, measured) { + this->initialize(expression(key1, key2)); } // Return measurement expression - virtual Expression expression() const { - return Expression(Expression(this->keys_[0]), &POSE::bearing, - Expression(this->keys_[1])); + virtual Expression expression(Key key1, Key key2) const { + return Expression(Expression(key1), &POSE::bearing, + Expression(key2)); } /// print - void print(const std::string& s = "", const KeyFormatter& kf = DefaultKeyFormatter) const { + void print(const std::string& s = "", + const KeyFormatter& kf = DefaultKeyFormatter) const { std::cout << s << "BearingFactor" << std::endl; - SerializableExpressionFactor::print(s, kf); + Base::print(s, kf); } }; // BearingFactor /// traits template -struct traits > : public Testable > {}; +struct traits > + : public Testable > {}; } // namespace gtsam diff --git a/gtsam/sam/tests/testBearingFactor.cpp b/gtsam/sam/tests/testBearingFactor.cpp index 93ad9db7f..f351ecdbf 100644 --- a/gtsam/sam/tests/testBearingFactor.cpp +++ b/gtsam/sam/tests/testBearingFactor.cpp @@ -38,7 +38,7 @@ static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(1, 0.5)); BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model2D); typedef BearingFactor BearingFactor3D; -Unit3 measurement3D = Pose3().bearing(Point3(1,0,0)); // has to match values! +Unit3 measurement3D = Pose3().bearing(Point3(1, 0, 0)); // has to match values! static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5)); BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D); @@ -68,7 +68,8 @@ TEST(BearingFactor, 2D) { values.insert(poseKey, Pose2(1.0, 2.0, 0.57)); values.insert(pointKey, Point2(-4.0, 11.0)); - EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(), values, 1e-7,1e-5); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey), + values, 1e-7, 1e-5); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); } @@ -91,9 +92,10 @@ TEST(BearingFactor, 3D) { // Set the linearization point Values values; values.insert(poseKey, Pose3()); - values.insert(pointKey, Point3(1,0,0)); + values.insert(pointKey, Point3(1, 0, 0)); - EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(), values, 1e-7,1e-5); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey), + values, 1e-7, 1e-5); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); }