review1 changes

gtsam/sfm/TranslationFactor.h

@@ -51,7 +51,9 @@ 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

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);

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));
 }
 
 /* ************************************************************************* */