diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 8fcf84a113..012a5ae9cd 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/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 points = createUnknowns(10, 'p', 1); - EXPECT(assert_equal(somePoint, points.back().value(values))); + std::vector pointExpressions = createUnknowns(10, 'p', 1); + 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 H) { return p; } -Point3_ p(1); +Point3_ pointExpression(1); set 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 e(f1, p); - EXPECT(expected == e.keys()); + Expression unaryExpression(f1, pointExpression); + EXPECT(expected == unaryExpression.keys()); } + +// Check that also works with a scalar return value. TEST(Expression, Unary2) { using namespace unary; - Double_ e(f2, p); - EXPECT(expected == e.keys()); + Double_ unaryExpression(f2, pointExpression); + EXPECT(expected == unaryExpression.keys()); } -/* ************************************************************************* */ // Unary(Leaf), dynamic TEST(Expression, Unary3) { using namespace unary; - // Expression e(f3, p); + // TODO(yetongumich): dynamic output arguments do not work yet! + // Expression 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 : public internal::VectorSpace {}; // Nullary Method TEST(Expression, NullaryMethod) { // Create expression - Expression p(67); - Expression norm_(p, &Class::norm); + const Key key(67); + Expression 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 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 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 expected = list_of(1)(2); EXPECT(expected == binary::p_cam.keys()); } /* ************************************************************************* */ -// dimensions +// Check dimensions by calling `dims` method. TEST(Expression, BinaryDimensions) { map actual, expected = map_list_of(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 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 actual_dims, expected_dims = map_list_of(key1, 3)(key2, 3); @@ -461,11 +478,11 @@ TEST(Expression, WeightedSum) { EXPECT(actual_dims == expected_dims); Values values; - values.insert(key1, Point3(1, 0, 0)); - values.insert(key2, Point3(0, 1, 0)); + values.insert(key1, point1); + values.insert(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 diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index e3e37e7c7d..657e777613 100644 --- a/tests/testExpressionFactor.cpp +++ b/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 old(2, Point2(0, 0), model); - // Concise version + // Create the equivalent factor with expression. ExpressionFactor 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 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 old(2, Point2(0, 0), model); - // Concise version + // Create the equivalent factor with expression. ExpressionFactor 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 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 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 gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(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 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 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 gf = f.linearize(values); - EXPECT( assert_equal(*expected, *gf, 1e-9)); + EXPECT(assert_equal(*expected, *gf, 1e-9)); // Concise version ExpressionFactor 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 gf2 = f2.linearize(values); - EXPECT( assert_equal(*expected, *gf2, 1e-9)); + EXPECT(assert_equal(*expected, *gf2, 1e-9)); // Try ternary version ExpressionFactor 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 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 H(2); Vector actual = f.unwhitenedError(values, H); - EXPECT( assert_equal(I_3x3, H[0],1e-9)); - EXPECT( assert_equal(I_3x3, H[1],1e-9)); + EXPECT(assert_equal(I_3x3, H[0],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 gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); + EXPECT(assert_equal(expected, *jf,1e-9)); } /* ************************************************************************* */ @@ -363,14 +368,14 @@ TEST(ExpressionFactor, compose2) { std::vector 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 gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); + EXPECT(assert_equal(expected, *jf,1e-9)); } /* ************************************************************************* */ @@ -392,14 +397,14 @@ TEST(ExpressionFactor, compose3) { std::vector 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 gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); - EXPECT( assert_equal(expected, *jf,1e-9)); + EXPECT(assert_equal(expected, *jf,1e-9)); } /* ************************************************************************* */ @@ -435,16 +440,16 @@ TEST(ExpressionFactor, composeTernary) { std::vector 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[1],1e-9)); - EXPECT( assert_equal(I_3x3, H[2],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[2],1e-9)); // Check linearization JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(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::Rot3, gtsam::Point3> { private: using This = TestNaryFactor; using Base =