Merge pull request #1828 from borglab/ta-bata2

Add the BATA translation averaging factor with unit tests

gtsam/sfm/TranslationFactor.h

@@ -18,6 +18,7 @@
  * @brief Binary factor for a relative translation direction measurement.
  */
 
+#include <gtsam/base/Vector.h>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/linear/NoiseModel.h>
@@ -36,16 +37,13 @@ namespace gtsam {
  *    normalized(Tb - Ta) - w_aZb.point3()
  *
  * @ingroup sfm
- * 
- * 
  */
 class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
  private:
   typedef NoiseModelFactorN<Point3, Point3> Base;
   Point3 measured_w_aZb_;
 
  public:
-
   // Provide access to the Matrix& version of evaluateError:
   using NoiseModelFactor2<Point3, Point3>::evaluateError;
 
@@ -60,20 +58,20 @@ class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
    * @brief Caclulate error: (norm(Tb - Ta) - measurement)
    * where Tb and Ta are Point3 translations and measurement is
    * the Unit3 translation direction from a to b.
-   * 
+   *
    * @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,
-      OptionalMatrixType H1,
-      OptionalMatrixType H2) const override {
+  Vector evaluateError(const Point3& Ta, const Point3& Tb,
+                       OptionalMatrixType H1,
+                       OptionalMatrixType H2) const override {
     const Point3 dir = Tb - Ta;
     Matrix33 H_predicted_dir;
-    const Point3 predicted = normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr);
+    const Point3 predicted =
+        normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr);
     if (H1) *H1 = -H_predicted_dir;
     if (H2) *H2 = H_predicted_dir;
     return predicted - measured_w_aZb_;
@@ -89,4 +87,73 @@ class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
   }
 #endif
 };  // \ TranslationFactor
+
+/**
+ * Binary factor for a relative translation direction measurement
+ * w_aZb. The measurement equation is
+ *    w_aZb = scale * (Tb - Ta)
+ * i.e., w_aZb is the translation direction from frame A to B, in world
+ * coordinates.
+ *
+ * Instead of normalizing the Tb - Ta vector, we multiply it by a scalar
+ * which is also jointly optimized. This is inspired by the work on
+ * BATA (Zhuang et al, CVPR 2018).
+ *
+ * @ingroup sfm
+ */
+class BilinearAngleTranslationFactor
+    : public NoiseModelFactorN<Point3, Point3, Vector1> {
+ private:
+  typedef NoiseModelFactorN<Point3, Point3, Vector1> Base;
+  Point3 measured_w_aZb_;
+
+ public:
+  /// default constructor
+  BilinearAngleTranslationFactor() {}
+
+  BilinearAngleTranslationFactor(Key a, Key b, Key scale_key,
+                                 const Unit3& w_aZb,
+                                 const SharedNoiseModel& noiseModel)
+      : Base(noiseModel, a, b, scale_key), measured_w_aZb_(w_aZb.point3()) {}
+
+  // Provide access to the Matrix& version of evaluateError:
+  using NoiseModelFactor2<Point3, Point3, Vector1>::evaluateError;
+
+  /**
+   * @brief Caclulate error: (scale * (Tb - Ta) - measurement)
+   * where Tb and Ta are Point3 translations and measurement is
+   * the Unit3 translation direction from a to b.
+   *
+   * @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, const Vector1& scale,
+                       OptionalMatrixType H1, OptionalMatrixType H2,
+                       OptionalMatrixType H3) const override {
+    // Ideally we should use a positive real valued scalar datatype for scale.
+    const double abs_scale = abs(scale[0]);
+    const Point3 predicted = (Tb - Ta) * abs_scale;
+    if (H1) {
+      *H1 = -Matrix3::Identity() * abs_scale;
+    }
+    if (H2) {
+      *H2 = Matrix3::Identity() * abs_scale;
+    }
+    if (H3) {
+      *H3 = scale[0] >= 0 ? (Tb - Ta) : (Ta - Tb);
+    }
+    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));
+  }
+};  // \ BilinearAngleTranslationFactor
 }  // namespace gtsam

gtsam/sfm/TranslationRecovery.cpp

@@ -101,9 +101,17 @@ NonlinearFactorGraph TranslationRecovery::buildGraph(
   NonlinearFactorGraph graph;
 
   // Add translation factors for input translation directions.
+  uint64_t i = 0;
   for (auto edge : relativeTranslations) {
-    graph.emplace_shared<TranslationFactor>(edge.key1(), edge.key2(),
-                                            edge.measured(), edge.noiseModel());
+    if (use_bilinear_translation_factor_) {
+      graph.emplace_shared<BilinearAngleTranslationFactor>(
+          edge.key1(), edge.key2(), Symbol('S', i), edge.measured(),
+          edge.noiseModel());
+    } else {
+      graph.emplace_shared<TranslationFactor>(
+          edge.key1(), edge.key2(), edge.measured(), edge.noiseModel());
+    }
+    i++;
   }
   return graph;
 }
@@ -163,6 +171,12 @@ Values TranslationRecovery::initializeRandomly(
     insert(edge.key1());
     insert(edge.key2());
   }
+
+  if (use_bilinear_translation_factor_) {
+    for (uint64_t i = 0; i < relativeTranslations.size(); i++) {
+      initial.insert<Vector1>(Symbol('S', i), Vector1(1.0));
+    }
+  }
   return initial;
 }
 

gtsam/sfm/TranslationRecovery.h

@@ -60,14 +60,18 @@ class GTSAM_EXPORT TranslationRecovery {
   // Parameters.
   LevenbergMarquardtParams lmParams_;
 
+  const bool use_bilinear_translation_factor_ = false;
+
  public:
   /**
    * @brief Construct a new Translation Recovery object
    *
    * @param lmParams parameters for optimization.
    */
-  TranslationRecovery(const LevenbergMarquardtParams &lmParams)
-      : lmParams_(lmParams) {}
+  TranslationRecovery(const LevenbergMarquardtParams &lmParams,
+                      bool use_bilinear_translation_factor = false)
+      : lmParams_(lmParams),
+        use_bilinear_translation_factor_(use_bilinear_translation_factor) {}
 
   /**
    * @brief Default constructor.

gtsam/sfm/sfm.i

@@ -23,7 +23,7 @@ virtual class SfmTrack : gtsam::SfmTrack2d {
   SfmTrack();
   SfmTrack(const gtsam::Point3& pt);
   const Point3& point3() const;
-  
+
   Point3 p;
 
   double r;
@@ -37,8 +37,8 @@ virtual class SfmTrack : gtsam::SfmTrack2d {
   bool equals(const gtsam::SfmTrack& expected, double tol) const;
 };
 
-#include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
 #include <gtsam/sfm/SfmData.h>
 class SfmData {
   SfmData();
@@ -268,6 +268,8 @@ class MFAS {
 #include <gtsam/sfm/TranslationRecovery.h>
 
 class TranslationRecovery {
+  TranslationRecovery(const gtsam::LevenbergMarquardtParams& lmParams,
+                      const bool use_bilinear_translation_factor);
   TranslationRecovery(const gtsam::LevenbergMarquardtParams& lmParams);
   TranslationRecovery();  // default params.
   void addPrior(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,

gtsam/sfm/tests/testTranslationFactor.cpp

@@ -30,7 +30,7 @@ using namespace gtsam;
 static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.05));
 
 // Keys are deliberately *not* in sorted order to test that case.
-static const Key kKey1(2), kKey2(1);
+static const Key kKey1(2), kKey2(1), kEdgeKey(3);
 static const Unit3 kMeasured(1, 0, 0);
 
 /* ************************************************************************* */
@@ -99,6 +99,79 @@ TEST(TranslationFactor, Jacobian) {
   EXPECT(assert_equal(H2Expected, H2Actual, 1e-9));
 }
 
+
+/* ************************************************************************* */
+TEST(BilinearAngleTranslationFactor, Constructor) {
+  BilinearAngleTranslationFactor factor(kKey1, kKey2, kEdgeKey, kMeasured, model);
+}
+
+/* ************************************************************************* */
+TEST(BilinearAngleTranslationFactor, ZeroError) {
+  // Create a factor
+  BilinearAngleTranslationFactor factor(kKey1, kKey2, kEdgeKey, kMeasured, model);
+
+  // Set the linearization
+  Point3 T1(1, 0, 0), T2(2, 0, 0);
+  Vector1 scale(1.0);
+
+  // Use the factor to calculate the error
+  Vector actualError(factor.evaluateError(T1, T2, scale));
+
+  // Verify we get the expected error
+  Vector expected = (Vector3() << 0, 0, 0).finished();
+  EXPECT(assert_equal(expected, actualError, 1e-9));
+}
+
+/* ************************************************************************* */
+TEST(BilinearAngleTranslationFactor, NonZeroError) {
+  // create a factor
+  BilinearAngleTranslationFactor factor(kKey1, kKey2, kEdgeKey, kMeasured, model);
+
+  // set the linearization
+  Point3 T1(0, 1, 1), T2(0, 2, 2);
+  Vector1 scale(1.0 / sqrt(2));
+
+  // use the factor to calculate the error
+  Vector actualError(factor.evaluateError(T1, T2, scale));
+
+  // verify we get the expected error
+  Vector expected = (Vector3() << -1, 1 / sqrt(2), 1 / sqrt(2)).finished();
+  EXPECT(assert_equal(expected, actualError, 1e-9));
+}
+
+/* ************************************************************************* */
+Vector bilinearAngleFactorError(const Point3 &T1, const Point3 &T2, const Vector1 &scale,
+                   const BilinearAngleTranslationFactor &factor) {
+  return factor.evaluateError(T1, T2, scale);
+}
+
+TEST(BilinearAngleTranslationFactor, Jacobian) {
+  // Create a factor
+  BilinearAngleTranslationFactor factor(kKey1, kKey2, kEdgeKey, kMeasured, model);
+
+  // Set the linearization
+  Point3 T1(1, 0, 0), T2(2, 0, 0);
+  Vector1 scale(1.0);
+
+  // Use the factor to calculate the Jacobians
+  Matrix H1Actual, H2Actual, H3Actual;
+  factor.evaluateError(T1, T2, scale, H1Actual, H2Actual, H3Actual);
+
+  // Use numerical derivatives to calculate the Jacobians
+  Matrix H1Expected, H2Expected, H3Expected;
+  H1Expected = numericalDerivative11<Vector, Point3>(
+      std::bind(&bilinearAngleFactorError, std::placeholders::_1, T2, scale, factor), T1);
+  H2Expected = numericalDerivative11<Vector, Point3>(
+      std::bind(&bilinearAngleFactorError, T1, std::placeholders::_1, scale, factor), T2);
+  H3Expected = numericalDerivative11<Vector, Vector1>(
+      std::bind(&bilinearAngleFactorError, T1, T2, std::placeholders::_1, factor), scale);
+
+  // Verify the Jacobians are correct
+  EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
+  EXPECT(assert_equal(H2Expected, H2Actual, 1e-9));
+  EXPECT(assert_equal(H3Expected, H3Actual, 1e-9));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;