unit test, incl Jacobians

gtsam/sfm/TranslationFactor.h

@@ -35,7 +35,7 @@ namespace gtsam {
  * ambient world coordinate frame:
  *    normalized(Tb - Ta) - w_aZb.point3()
  *
- * @addtogroup SAM
+ * @addtogroup SFM
  */
 class TranslationFactor : public NoiseModelFactor2<Point3, Point3> {
  private:
@@ -67,8 +67,8 @@ class TranslationFactor : public NoiseModelFactor2<Point3, Point3> {
     Matrix33 H_predicted_dir;
     const Point3 predicted =
         dir.normalized(H1 || H2 ? &H_predicted_dir : nullptr);
-    if (H1) *H1 = H_predicted_dir;
+    if (H1) *H1 = -H_predicted_dir;
-    if (H2) *H2 = -H_predicted_dir;
+    if (H2) *H2 = H_predicted_dir;
     return predicted - measured_w_aZb_;
   }
 

gtsam/sfm/tests/testTranslationFactor.cpp

@@ -0,0 +1,90 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  testTranslationFactor.cpp
+ *  @brief Unit tests for TranslationFactor Class
+ *  @author Frank dellaert
+ *  @date March 2020
+ */
+
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/sfm/TranslationFactor.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+// Create a noise model for the chordal error
+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 Unit3 kMeasured(1, 0, 0);
+
+/* ************************************************************************* */
+TEST(TranslationFactor, Constructor) {
+  TranslationFactor factor(kKey1, kKey2, kMeasured, model);
+}
+
+/* ************************************************************************* */
+TEST(TranslationFactor, Error) {
+  // Create a factor
+  TranslationFactor factor(kKey1, kKey2, kMeasured, model);
+
+  // Set the linearization
+  Point3 T1(1, 0, 0), T2(2, 0, 0);
+
+  // Use the factor to calculate the error
+  Vector actualError(factor.evaluateError(T1, T2));
+
+  // Verify we get the expected error
+  Vector expected = (Vector3() << 0, 0, 0).finished();
+  EXPECT(assert_equal(expected, actualError, 1e-9));
+}
+
+/* ************************************************************************* */
+Vector factorError(const Point3& T1, const Point3& T2,
+                   const TranslationFactor& factor) {
+  return factor.evaluateError(T1, T2);
+}
+
+TEST(TranslationFactor, Jacobian) {
+  // Create a factor
+  TranslationFactor factor(kKey1, kKey2, kMeasured, model);
+
+  // Set the linearization
+  Point3 T1(1, 0, 0), T2(2, 0, 0);
+
+  // Use the factor to calculate the Jacobians
+  Matrix H1Actual, H2Actual;
+  factor.evaluateError(T1, T2, H1Actual, H2Actual);
+
+  // Use numerical derivatives to calculate the Jacobians
+  Matrix H1Expected, H2Expected;
+  H1Expected = numericalDerivative11<Vector, Point3>(
+      boost::bind(&factorError, _1, T2, factor), T1);
+  H2Expected = numericalDerivative11<Vector, Point3>(
+      boost::bind(&factorError, T1, _1, factor), T2);
+
+  // Verify the Jacobians are correct
+  EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
+  EXPECT(assert_equal(H2Expected, H2Actual, 1e-9));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */