Added binary specialization

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 <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)
+  SerializableExpressionFactor(const SharedNoiseModel& noiseModel,
-      : ExpressionFactor<T>(noiseModel, measurement) {
+                               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;
 
@@ -49,14 +49,17 @@ class SerializableExpressionFactor : public ExpressionFactor<T> {
   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));
+        "ExpressionFactor",
+        boost::serialization::base_object<ExpressionFactor<T> >(*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 <class Archive>
   void load(Archive& ar, const unsigned int /*version*/) {
     ar >> boost::serialization::make_nvp(
-              "ExpressionFactor", boost::serialization::base_object<ExpressionFactor<T> >(*this));
+              "ExpressionFactor",
+              boost::serialization::base_object<ExpressionFactor<T> >(*this));
     this->initialize(expression());
   }
 
@@ -66,10 +69,52 @@ class SerializableExpressionFactor : public ExpressionFactor<T> {
 };
 // SerializableExpressionFactor
 
-/// traits
+/**
-template <typename T>
+ * Binary specialization of SerializableExpressionFactor
-struct traits<SerializableExpressionFactor<T> >
+ * Enforces expression method with two keys, and provides evaluateError
-    : public Testable<SerializableExpressionFactor<T> > {};
+ */
+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);
+  }
 
-}// \ namespace gtsam
+  /// 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

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 <typename POSE, typename POINT, typename T>
-struct BearingFactor : public SerializableExpressionFactor<T> {
+struct BearingFactor : public SerializableExpressionFactor2<T, POSE, POINT> {
+  typedef SerializableExpressionFactor2<T, POSE, POINT> Base;
 
   /// default constructor
   BearingFactor() {}
 
   /// primary constructor
-  BearingFactor(Key poseKey, Key pointKey, const T& measured, const SharedNoiseModel& model)
+  BearingFactor(Key key1, Key key2, const T& measured,
-      : SerializableExpressionFactor<T>(model, measured) {
+                const SharedNoiseModel& model)
-    this->keys_.push_back(poseKey);
+      : Base(key1, key2, model, measured) {
-    this->keys_.push_back(pointKey);
+    this->initialize(expression(key1, key2));
-    this->initialize(expression());
   }
 
   // Return measurement expression
-  virtual Expression<T> expression() const {
+  virtual Expression<T> expression(Key key1, Key key2) const {
-    return Expression<T>(Expression<POSE>(this->keys_[0]), &POSE::bearing,
+    return Expression<T>(Expression<POSE>(key1), &POSE::bearing,
-                         Expression<POINT>(this->keys_[1]));
+                         Expression<POINT>(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<T>::print(s, kf);
+    Base::print(s, kf);
   }
 };  // BearingFactor
 
 /// traits
 template <class POSE, class POINT, class T>
-struct traits<BearingFactor<POSE, POINT, T> > : public Testable<BearingFactor<POSE, POINT, T> > {};
+struct traits<BearingFactor<POSE, POINT, T> >
+    : public Testable<BearingFactor<POSE, POINT, T> > {};
 
 }  // namespace gtsam

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<Pose3, Point3, Unit3> 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);
 }