Now use 3 template parameters, no more need for Pose::Rotation

gtsam/sam/BearingFactor.h

@@ -10,7 +10,9 @@
  * -------------------------------------------------------------------------- */
 
 /**
- *  @file  BearingFactor.H
+ *  @file  BearingFactor.h
+ *  @brief Serializable factor induced by a bearing measurement on a point from a pose
+ *  @date July 2015
  *  @author Frank Dellaert
  **/
 
@@ -26,33 +28,26 @@ namespace gtsam {
  * Binary factor for a bearing measurement
  * @addtogroup SAM
  */
-template <class Pose, class Point, class Measured = typename Pose::Rotation>
+template <typename Pose, typename Point, typename Measured>
-class BearingFactor : public SerializableExpressionFactor<Measured> {
+struct BearingFactor : public SerializableExpressionFactor<Measured> {
- private:
-  GTSAM_CONCEPT_TESTABLE_TYPE(Measured)
   GTSAM_CONCEPT_POSE_TYPE(Pose)
 
-  // Return measurement expression
-  virtual Expression<Measured> expression() const {
-    return Expression<Measured>(Expression<Pose>(this->keys_[0]), &Pose::bearing,
-                                Expression<Point>(this->keys_[1]));
-  }
-
- public:
   /// default constructor
   BearingFactor() {}
 
   /// primary constructor
-  BearingFactor(Key poseKey, Key pointKey, const Measured& measured,
+  BearingFactor(Key poseKey, Key pointKey, const Measured& measured, const SharedNoiseModel& model)
-                const SharedNoiseModel& model)
       : SerializableExpressionFactor<Measured>(model, measured) {
     this->keys_.push_back(poseKey);
     this->keys_.push_back(pointKey);
     this->initialize(expression());
   }
 
-  /// desructor
+  // Return measurement expression
-  virtual ~BearingFactor() {}
+  virtual Expression<Measured> expression() const {
+    return Expression<Measured>(Expression<Pose>(this->keys_[0]), &Pose::bearing,
+                                Expression<Point>(this->keys_[1]));
+  }
 
   /// print
   void print(const std::string& s = "", const KeyFormatter& kf = DefaultKeyFormatter) const {

gtsam/sam/tests/testBearingFactor.cpp

@@ -34,7 +34,7 @@ static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5));
 Key poseKey(1);
 Key pointKey(2);
 
-typedef BearingFactor<Pose2, Point2> BearingFactor2D;
+typedef BearingFactor<Pose2, Point2, Rot2> BearingFactor2D;
 double measurement2D(10.0);
 BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model);
 GTSAM_CONCEPT_TESTABLE_INST(BearingFactor2D)