From 9d9c30e5dc399fafc1726c949f24276b160244b4 Mon Sep 17 00:00:00 2001
From: akrishnan86 <akrishnan86@gatech.edu>
Date: Sun, 28 Jun 2020 11:03:38 -0700
Subject: [PATCH] review1 changes

---
 gtsam/sfm/TranslationFactor.h             |  6 ++++--
 gtsam/sfm/TranslationRecovery.cpp         |  2 +-
 gtsam/sfm/TranslationRecovery.h           |  5 +++++
 gtsam/sfm/tests/testTranslationFactor.cpp | 20 +++++++++++++++++++-
 4 files changed, 29 insertions(+), 4 deletions(-)

diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h
index 584b1fa69..9a4bd68a7 100644
--- a/gtsam/sfm/TranslationFactor.h
+++ b/gtsam/sfm/TranslationFactor.h
@@ -51,8 +51,10 @@ class TranslationFactor : public NoiseModelFactor2<Point3, Point3> {
       : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {}
 
   /**
-   * @brief Caclulate error norm(p1-p2) - measured
-   *
+   * @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
diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp
index a0f3eb6b6..aeeae688f 100644
--- a/gtsam/sfm/TranslationRecovery.cpp
+++ b/gtsam/sfm/TranslationRecovery.cpp
@@ -60,7 +60,7 @@ void TranslationRecovery::addPrior(const double scale,
 }
 
 Values TranslationRecovery::initalizeRandomly() const {
-  // Create a lambda expression that  checks whether value exists and randomly
+  // Create a lambda expression that checks whether value exists and randomly
   // initializes if not.
   Values initial;
   auto insert = [&initial](Key j) {
diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h
index 634825d29..bb3c3cdb1 100644
--- a/gtsam/sfm/TranslationRecovery.h
+++ b/gtsam/sfm/TranslationRecovery.h
@@ -60,6 +60,9 @@ class TranslationRecovery {
    *
    * @param relativeTranslations the relative translations, in world coordinate
    * frames, indexed in a map by a pair of Pose keys.
+   * @param lmParams (optional) gtsam::LavenbergMarquardtParams that can be
+   * used to modify the parameters for the LM optimizer. By default, uses the
+   * default LM parameters. 
    */
   TranslationRecovery(const TranslationEdges& relativeTranslations,
                       const LevenbergMarquardtParams& lmParams = LevenbergMarquardtParams())
@@ -102,6 +105,8 @@ class TranslationRecovery {
    *
    * @param poses SE(3) ground truth poses stored as Values
    * @param edges pairs (a,b) for which a measurement w_aZb will be generated.
+   * @return TranslationEdges map from a KeyPair to the simulated Unit3
+   * translation direction measurement between the cameras in KeyPair.
    */
   static TranslationEdges SimulateMeasurements(
       const Values& poses, const std::vector<KeyPair>& edges);
diff --git a/gtsam/sfm/tests/testTranslationFactor.cpp b/gtsam/sfm/tests/testTranslationFactor.cpp
index da284bfd6..37e8b6c0f 100644
--- a/gtsam/sfm/tests/testTranslationFactor.cpp
+++ b/gtsam/sfm/tests/testTranslationFactor.cpp
@@ -38,7 +38,7 @@ TEST(TranslationFactor, Constructor) {
 }
 
 /* ************************************************************************* */
-TEST(TranslationFactor, Error) {
+TEST(TranslationFactor, ZeroError) {
   // Create a factor
   TranslationFactor factor(kKey1, kKey2, kMeasured, model);
 
@@ -51,6 +51,24 @@ TEST(TranslationFactor, Error) {
   // Verify we get the expected error
   Vector expected = (Vector3() << 0, 0, 0).finished();
   EXPECT(assert_equal(expected, actualError, 1e-9));
+
+
+}
+
+/* ************************************************************************* */
+TEST(TranslationFactor, NonZeroError) {
+  // create a factor
+  TranslationFactor factor(kKey1, kKey2, kMeasured, model);
+
+  // set the linearization
+  Point3 T1(0, 1, 1), T2(0, 2, 2);
+
+  // use the factor to calculate the error
+  Vector actualError(factor.evaluateError(T1, T2));
+
+  // verify we get the expected error
+  Vector expected = (Vector3() << -1, 1/sqrt(2), 1/sqrt(2)).finished();
+  EXPECT(assert_equal(expected, actualError, 1e-9));
 }
 
 /* ************************************************************************* */