Moved TranslationFactor prototype into sfm directory

gtsam/sfm/TranslationFactor.h

@@ -0,0 +1,83 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
+#pragma once
+
+/**
+ * @file TranslationFactor.h
+ * @author Frank Dellaert
+ * @date March 2020
+ * @brief Binary factor for a relative translation direction measurement.
+ */
+
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+/**
+ * Binary factor for a relative translation direction measurement
+ * w_aZb. The measurement equation is
+ *    w_aZb = Unit3(Tb - Ta)
+ * i.e., w_aZb is the translation direction from frame A to B, in world
+ * coordinates. Although Unit3 instances live on a manifold, following
+ * Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the
+ * ambient world coordinate frame:
+ *    normalized(Tb - Ta) - w_aZb.point3()
+ *
+ * @addtogroup SAM
+ */
+class TranslationFactor : public NoiseModelFactor2<Point3, Point3> {
+ private:
+  typedef NoiseModelFactor2<Point3, Point3> Base;
+  Point3 measured_w_aZb_;
+
+ public:
+  /// default constructor
+  TranslationFactor() {}
+
+  TranslationFactor(Key a, Key b, const Unit3& w_aZb,
+                    const SharedNoiseModel& noiseModel)
+      : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {}
+
+  /**
+   * @brief Caclulate error norm(p1-p2) - measured
+   *
+   * @param Ta translation for key a
+   * @param Tb translation for key b
+   * @param H1 optional jacobian in Ta
+   * @param H2 optional jacobian in Tb
+   * @return * Vector
+   */
+  Vector evaluateError(
+      const Point3& Ta, const Point3& Tb,
+      boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none) const override {
+    const Point3 dir = Tb - Ta;
+    Matrix33 H_predicted_dir;
+    const Point3 predicted =
+        dir.normalized(H1 || H2 ? &H_predicted_dir : nullptr);
+    if (H1) *H1 = H_predicted_dir;
+    if (H2) *H2 = -H_predicted_dir;
+    return predicted - measured_w_aZb_;
+  }
+
+ private:
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& boost::serialization::make_nvp(
+        "Base", boost::serialization::base_object<Base>(*this));
+  }
+};  // \ TranslationFactor
+}  // namespace gtsam

tests/testTranslationRecovery.cpp

@@ -1,85 +1,3 @@
-/* ----------------------------------------------------------------------------
-
- * 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 TranslationFactor.h
- * @author Frank Dellaert
- * @date March 2020
- * @brief Binary factor for a relative translation direction measurement.
- */
-
-#include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Unit3.h>
-#include <gtsam/linear/NoiseModel.h>
-#include <gtsam/nonlinear/NonlinearFactor.h>
-
-namespace gtsam {
-
-/**
- * Binary factor for a relative translation direction measurement
- * w_aZb. The measurement equation is
- *    w_aZb = Unit3(Tb - Ta)
- * i.e., w_aZb is the translation direction from frame A to B, in world
- * coordinates. Although Unit3 instances live on a manifold, following
- * Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the
- * ambient world coordinate frame:
- *    normalized(Tb - Ta) - w_aZb.point3()
- *
- * @addtogroup SAM
- */
-class TranslationFactor : public NoiseModelFactor2<Point3, Point3> {
- private:
-  typedef NoiseModelFactor2<Point3, Point3> Base;
-  Point3 measured_w_aZb_;
-
- public:
-  /// default constructor
-  TranslationFactor() {}
-
-  TranslationFactor(Key a, Key b, const Unit3& w_aZb,
-                    const SharedNoiseModel& noiseModel)
-      : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {}
-
-  /**
-   * @brief Caclulate error norm(p1-p2) - measured
-   *
-   * @param Ta translation for key a
-   * @param Tb translation for key b
-   * @param H1 optional jacobian in Ta
-   * @param H2 optional jacobian in Tb
-   * @return * Vector
-   */
-  Vector evaluateError(
-      const Point3& Ta, const Point3& Tb,
-      boost::optional<Matrix&> H1 = boost::none,
-      boost::optional<Matrix&> H2 = boost::none) const override {
-    const Point3 dir = Tb - Ta;
-    Matrix33 H_predicted_dir;
-    const Point3 predicted =
-        dir.normalized(H1 || H2 ? &H_predicted_dir : nullptr);
-    if (H1) *H1 = H_predicted_dir;
-    if (H2) *H2 = -H_predicted_dir;
-    return predicted - measured_w_aZb_;
-  }
-
- private:
-  friend class boost::serialization::access;
-  template <class ARCHIVE>
-  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
-    ar& boost::serialization::make_nvp(
-        "Base", boost::serialization::base_object<Base>(*this));
-  }
-};  // \ TranslationFactor
-}  // namespace gtsam
-
 /* ----------------------------------------------------------------------------
 
  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
@@ -98,7 +16,7 @@ class TranslationFactor : public NoiseModelFactor2<Point3, Point3> {
  * @brief test recovering translations when rotations are given.
  */
 
-// #include <gtsam/sam/TranslationFactor.h>
+#include <gtsam/sfm/TranslationFactor.h>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Unit3.h>