refactor tests and add comments

gtsam/nonlinear/tests/testExpression.cpp

@@ -58,22 +58,23 @@ TEST(Expression, Constant) {
 /* ************************************************************************* */
 // Leaf
 TEST(Expression, Leaf) {
-  Rot3_ R(100);
+  const Key key = 100;
+  Rot3_ R(key);
   Values values;
-  values.insert(100, someR);
+  values.insert(key, someR);
 
   Rot3 actual2 = R.value(values);
   EXPECT(assert_equal(someR, actual2));
 }
 
 /* ************************************************************************* */
-// Many Leaves
+// Test the function `createUnknowns` to create many leaves at once.
 TEST(Expression, Leaves) {
   Values values;
-  Point3 somePoint(1, 2, 3);
+  const Point3 somePoint(1, 2, 3);
   values.insert(Symbol('p', 10), somePoint);
-  std::vector<Point3_> points = createUnknowns<Point3>(10, 'p', 1);
+  std::vector<Point3_> pointExpressions = createUnknowns<Point3>(10, 'p', 1);
-  EXPECT(assert_equal(somePoint, points.back().value(values)));
+  EXPECT(assert_equal(somePoint, pointExpressions.back().value(values)));
 }
 
 /* ************************************************************************* */
@@ -88,29 +89,34 @@ double f2(const Point3& p, OptionalJacobian<1, 3> H) {
 Vector f3(const Point3& p, OptionalJacobian<Eigen::Dynamic, 3> H) {
   return p;
 }
-Point3_ p(1);
+Point3_ pointExpression(1);
 set<Key> expected = list_of(1);
 }  // namespace unary
 
+// Create a unary expression that takes another expression as a single argument.
 TEST(Expression, Unary1) {
   using namespace unary;
-  Expression<Point2> e(f1, p);
+  Expression<Point2> unaryExpression(f1, pointExpression);
-  EXPECT(expected == e.keys());
+  EXPECT(expected == unaryExpression.keys());
-}
+}
-TEST(Expression, Unary2) {
+
-  using namespace unary;
+// Check that also works with a scalar return value.
-  Double_ e(f2, p);
+TEST(Expression, Unary2) {
-  EXPECT(expected == e.keys());
+  using namespace unary;
+  Double_ unaryExpression(f2, pointExpression);
+  EXPECT(expected == unaryExpression.keys());
 }
 
-/* ************************************************************************* */
 // Unary(Leaf), dynamic
 TEST(Expression, Unary3) {
   using namespace unary;
-  //  Expression<Vector> e(f3, p);
+  // TODO(yetongumich): dynamic output arguments do not work yet!
+  // Expression<Vector> unaryExpression(f3, pointExpression);
+  // EXPECT(expected == unaryExpression.keys());
 }
 
 /* ************************************************************************* */
+// Simple test class that implements the `VectorSpace` protocol.
 class Class : public Point3 {
  public:
   enum {dimension = 3};
@@ -133,16 +139,20 @@ template<> struct traits<Class> : public internal::VectorSpace<Class> {};
 // Nullary Method
 TEST(Expression, NullaryMethod) {
   // Create expression
-  Expression<Class> p(67);
+  const Key key(67);
-  Expression<double> norm_(p, &Class::norm);
+  Expression<Class> classExpression(key);
+
+  // Make expression from a class method, note how it differs from the function
+  // expressions by leading with the class expression in the constructor.
+  Expression<double> norm_(classExpression, &Class::norm);
 
   // Create Values
   Values values;
-  values.insert(67, Class(3, 4, 5));
+  values.insert(key, Class(3, 4, 5));
 
   // Check dims as map
   std::map<Key, int> map;
-  norm_.dims(map);
+  norm_.dims(map); // TODO(yetongumich): Change to google style pointer convention.
   LONGS_EQUAL(1, map.size());
 
   // Get value and Jacobians
@@ -150,9 +160,10 @@ TEST(Expression, NullaryMethod) {
   double actual = norm_.value(values, H);
 
   // Check all
-  EXPECT(actual == sqrt(50));
+  const double norm = sqrt(3*3 + 4*4 + 5*5);
+  EXPECT(actual == norm);
   Matrix expected(1, 3);
-  expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0);
+  expected << 3.0 / norm, 4.0 / norm, 5.0 / norm;
   EXPECT(assert_equal(expected, H[0]));
 }
 
@@ -170,21 +181,21 @@ Point3_ p_cam(x, &Pose3::transformTo, p);
 }
 
 /* ************************************************************************* */
-// Check that creating an expression to double compiles
+// Check that creating an expression to double compiles.
 TEST(Expression, BinaryToDouble) {
   using namespace binary;
   Double_ p_cam(doubleF, x, p);
 }
 
 /* ************************************************************************* */
-// keys
+// Check keys of an expression created from class method.
 TEST(Expression, BinaryKeys) {
   set<Key> expected = list_of(1)(2);
   EXPECT(expected == binary::p_cam.keys());
 }
 
 /* ************************************************************************* */
-// dimensions
+// Check dimensions by calling `dims` method.
 TEST(Expression, BinaryDimensions) {
   map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3);
   binary::p_cam.dims(actual);
@@ -192,7 +203,7 @@ TEST(Expression, BinaryDimensions) {
 }
 
 /* ************************************************************************* */
-// dimensions
+// Check dimensions by calling `traceSize` method.
 TEST(Expression, BinaryTraceSize) {
   typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
   size_t expectedTraceSize = sizeof(Binary::Record);
@@ -247,6 +258,7 @@ TEST(Expression, TreeTraceSize) {
 }
 
 /* ************************************************************************* */
+// Test compose operation with * operator.
 TEST(Expression, compose1) {
   // Create expression
   Rot3_ R1(1), R2(2);
@@ -258,7 +270,7 @@ TEST(Expression, compose1) {
 }
 
 /* ************************************************************************* */
-// Test compose with arguments referring to the same rotation
+// Test compose with arguments referring to the same rotation.
 TEST(Expression, compose2) {
   // Create expression
   Rot3_ R1(1), R2(1);
@@ -270,7 +282,7 @@ TEST(Expression, compose2) {
 }
 
 /* ************************************************************************* */
-// Test compose with one arguments referring to constant rotation
+// Test compose with one arguments referring to constant rotation.
 TEST(Expression, compose3) {
   // Create expression
   Rot3_ R1(Rot3::identity()), R2(3);
@@ -282,7 +294,7 @@ TEST(Expression, compose3) {
 }
 
 /* ************************************************************************* */
-// Test with ternary function
+// Test with ternary function.
 Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1,
                   OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) {
   // return dummy derivatives (not correct, but that's ok for testing here)
@@ -306,6 +318,7 @@ TEST(Expression, ternary) {
 }
 
 /* ************************************************************************* */
+// Test scalar multiplication with * operator.
 TEST(Expression, ScalarMultiply) {
   const Key key(67);
   const Point3_ expr = 23 * Point3_(key);
@@ -336,6 +349,7 @@ TEST(Expression, ScalarMultiply) {
 }
 
 /* ************************************************************************* */
+// Test sum with + operator.
 TEST(Expression, BinarySum) {
   const Key key(67);
   const Point3_ sum_ = Point3_(key) + Point3_(Point3(1, 1, 1));
@@ -366,6 +380,7 @@ TEST(Expression, BinarySum) {
 }
 
 /* ************************************************************************* */
+// Test sum of 3 variables with + operator.
 TEST(Expression, TripleSum) {
   const Key key(67);
   const Point3_ p1_(Point3(1, 1, 1)), p2_(key);
@@ -387,6 +402,7 @@ TEST(Expression, TripleSum) {
 }
 
 /* ************************************************************************* */
+// Test sum with += operator.
 TEST(Expression, PlusEqual) {
   const Key key(67);
   const Point3_ p1_(Point3(1, 1, 1)), p2_(key);
@@ -454,6 +470,7 @@ TEST(Expression, UnaryOfSum) {
 /* ************************************************************************* */
 TEST(Expression, WeightedSum) {
   const Key key1(42), key2(67);
+  const Point3 point1(1, 0, 0), point2(0, 1, 0);
   const Point3_ weighted_sum_ = 17 * Point3_(key1) + 23 * Point3_(key2);
 
   map<Key, int> actual_dims, expected_dims = map_list_of<Key, int>(key1, 3)(key2, 3);
@@ -461,11 +478,11 @@ TEST(Expression, WeightedSum) {
   EXPECT(actual_dims == expected_dims);
 
   Values values;
-  values.insert<Point3>(key1, Point3(1, 0, 0));
+  values.insert<Point3>(key1, point1);
-  values.insert<Point3>(key2, Point3(0, 1, 0));
+  values.insert<Point3>(key2, point2);
 
   // Check value
-  const Point3 expected = 17 * Point3(1, 0, 0) + 23 * Point3(0, 1, 0);
+  const Point3 expected = 17 * point1 + 23 * point2;
   EXPECT(assert_equal(expected, weighted_sum_.value(values)));
 
   // Check value + Jacobians

tests/testExpressionFactor.cpp

@@ -59,40 +59,42 @@ Point2_ p(2);
 TEST(ExpressionFactor, Leaf) {
   using namespace leaf;
 
-  // Create old-style factor to create expected value and derivatives
+  // Create old-style factor to create expected value and derivatives.
   PriorFactor<Point2> old(2, Point2(0, 0), model);
 
-  // Concise version
+  // Create the equivalent factor with expression.
   ExpressionFactor<Point2> f(model, Point2(0, 0), p);
+
+  // Check values and derivatives.
   EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f.dim());
   boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
-  EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
+  EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9));
 }
 
 /* ************************************************************************* */
-// non-zero noise model
+// Test leaf expression with noise model of differnt variance.
 TEST(ExpressionFactor, Model) {
   using namespace leaf;
 
   SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01));
 
-  // Create old-style factor to create expected value and derivatives
+  // Create old-style factor to create expected value and derivatives.
   PriorFactor<Point2> old(2, Point2(0, 0), model);
 
-  // Concise version
+  // Create the equivalent factor with expression.
   ExpressionFactor<Point2> f(model, Point2(0, 0), p);
 
-  // Check values and derivatives
+  // Check values and derivatives.
   EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f.dim());
   boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
-  EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
+  EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9));
   EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); // another way
 }
 
 /* ************************************************************************* */
-// Constrained noise model
+// Test leaf expression with constrained noise model.
 TEST(ExpressionFactor, Constrained) {
   using namespace leaf;
 
@@ -106,7 +108,7 @@ TEST(ExpressionFactor, Constrained) {
   EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f.dim());
   boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
-  EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
+  EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9));
 }
 
 /* ************************************************************************* */
@@ -130,7 +132,7 @@ TEST(ExpressionFactor, Unary) {
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
-  EXPECT( assert_equal(expected, *jf, 1e-9));
+  EXPECT(assert_equal(expected, *jf, 1e-9));
 }
 
 /* ************************************************************************* */
@@ -143,11 +145,13 @@ Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) {
   if (H) *H << I_3x3, I_3x3, I_3x3;
   return v;
 }
+
 typedef Eigen::Matrix<double,9,9> Matrix9;
 Vector9 id9(const Vector9& v, OptionalJacobian<9,9> H) {
   if (H) *H = Matrix9::Identity();
   return v;
 }
+
 TEST(ExpressionFactor, Wide) {
   // Create some values
   Values values;
@@ -208,6 +212,7 @@ TEST(ExpressionFactor, Binary) {
   EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9));
   EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9));
 }
+
 /* ************************************************************************* */
 // Unary(Binary(Leaf,Leaf))
 TEST(ExpressionFactor, Shallow) {
@@ -264,7 +269,7 @@ TEST(ExpressionFactor, Shallow) {
   EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f2.dim());
   boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
-  EXPECT( assert_equal(*expected, *gf2, 1e-9));
+  EXPECT(assert_equal(*expected, *gf2, 1e-9));
 }
 
 /* ************************************************************************* */
@@ -297,7 +302,7 @@ TEST(ExpressionFactor, tree) {
   EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f.dim());
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
-  EXPECT( assert_equal(*expected, *gf, 1e-9));
+  EXPECT(assert_equal(*expected, *gf, 1e-9));
 
   // Concise version
   ExpressionFactor<Point2> f2(model, measured,
@@ -305,14 +310,14 @@ TEST(ExpressionFactor, tree) {
   EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f2.dim());
   boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
-  EXPECT( assert_equal(*expected, *gf2, 1e-9));
+  EXPECT(assert_equal(*expected, *gf2, 1e-9));
 
   // Try ternary version
   ExpressionFactor<Point2> f3(model, measured, project3(x, p, K));
   EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f3.dim());
   boost::shared_ptr<GaussianFactor> gf3 = f3.linearize(values);
-  EXPECT( assert_equal(*expected, *gf3, 1e-9));
+  EXPECT(assert_equal(*expected, *gf3, 1e-9));
 }
 
 /* ************************************************************************* */
@@ -333,15 +338,15 @@ TEST(ExpressionFactor, Compose1) {
   // Check unwhitenedError
   std::vector<Matrix> H(2);
   Vector actual = f.unwhitenedError(values, H);
-  EXPECT( assert_equal(I_3x3, H[0],1e-9));
+  EXPECT(assert_equal(I_3x3, H[0],1e-9));
-  EXPECT( assert_equal(I_3x3, H[1],1e-9));
+  EXPECT(assert_equal(I_3x3, H[1],1e-9));
 
   // Check linearization
   JacobianFactor expected(1, I_3x3, 2, I_3x3, Z_3x1);
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
-  EXPECT( assert_equal(expected, *jf,1e-9));
+  EXPECT(assert_equal(expected, *jf,1e-9));
 }
 
 /* ************************************************************************* */
@@ -363,14 +368,14 @@ TEST(ExpressionFactor, compose2) {
   std::vector<Matrix> H(1);
   Vector actual = f.unwhitenedError(values, H);
   EXPECT_LONGS_EQUAL(1, H.size());
-  EXPECT( assert_equal(2*I_3x3, H[0],1e-9));
+  EXPECT(assert_equal(2*I_3x3, H[0],1e-9));
 
   // Check linearization
   JacobianFactor expected(1, 2 * I_3x3, Z_3x1);
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
-  EXPECT( assert_equal(expected, *jf,1e-9));
+  EXPECT(assert_equal(expected, *jf,1e-9));
 }
 
 /* ************************************************************************* */
@@ -392,14 +397,14 @@ TEST(ExpressionFactor, compose3) {
   std::vector<Matrix> H(1);
   Vector actual = f.unwhitenedError(values, H);
   EXPECT_LONGS_EQUAL(1, H.size());
-  EXPECT( assert_equal(I_3x3, H[0],1e-9));
+  EXPECT(assert_equal(I_3x3, H[0],1e-9));
 
   // Check linearization
   JacobianFactor expected(3, I_3x3, Z_3x1);
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
-  EXPECT( assert_equal(expected, *jf,1e-9));
+  EXPECT(assert_equal(expected, *jf,1e-9));
 }
 
 /* ************************************************************************* */
@@ -435,16 +440,16 @@ TEST(ExpressionFactor, composeTernary) {
   std::vector<Matrix> H(3);
   Vector actual = f.unwhitenedError(values, H);
   EXPECT_LONGS_EQUAL(3, H.size());
-  EXPECT( assert_equal(I_3x3, H[0],1e-9));
+  EXPECT(assert_equal(I_3x3, H[0],1e-9));
-  EXPECT( assert_equal(I_3x3, H[1],1e-9));
+  EXPECT(assert_equal(I_3x3, H[1],1e-9));
-  EXPECT( assert_equal(I_3x3, H[2],1e-9));
+  EXPECT(assert_equal(I_3x3, H[2],1e-9));
 
   // Check linearization
   JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1);
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   boost::shared_ptr<JacobianFactor> jf = //
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
-  EXPECT( assert_equal(expected, *jf,1e-9));
+  EXPECT(assert_equal(expected, *jf,1e-9));
 }
 
 TEST(ExpressionFactor, tree_finite_differences) {
@@ -636,7 +641,7 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) {
 class TestNaryFactor
     : public gtsam::ExpressionFactorN<gtsam::Point3 /*return type*/,
                                       gtsam::Rot3, gtsam::Point3, 
-                                      gtsam::Rot3,gtsam::Point3> {
+                                      gtsam::Rot3, gtsam::Point3> {
 private:
   using This = TestNaryFactor;
   using Base =