diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 04f456ef1..e14912a65 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -40,7 +40,7 @@ Point2 uncalibrate(const CAL& K, const Point2& p, static const Rot3 someR = Rot3::RzRyRx(1, 2, 3); /* ************************************************************************* */ - +// Constant TEST(Expression, constant) { Expression R(someR); Values values; @@ -51,7 +51,7 @@ TEST(Expression, constant) { } /* ************************************************************************* */ - +// Leaf TEST(Expression, leaf) { Expression R(100); Values values; @@ -77,31 +77,55 @@ TEST(Expression, leaf) { // EXPECT(assert_equal(expected.at(67),a.jacobians().at(67))); //} /* ************************************************************************* */ +// Binary(Leaf,Leaf) +namespace binary { +// Create leaves +Expression x(1); +Expression p(2); +Expression p_cam(x, &Pose3::transform_to, p); +} +/* ************************************************************************* */ +// keys +TEST(Expression, keys_binary) { + + // Check keys + set expectedKeys; + expectedKeys.insert(1); + expectedKeys.insert(2); + EXPECT(expectedKeys == binary::p_cam.keys()); +} +/* ************************************************************************* */ // Binary(Leaf,Unary(Binary(Leaf,Leaf))) -TEST(Expression, test) { +namespace tree { +using namespace binary; +// Create leaves +Expression K(3); - // Test Constant expression - Expression c(Rot3::identity()); - - // Create leaves - Expression x(1); - Expression p(2); - Expression K(3); - - // Create expression tree - Expression p_cam(x, &Pose3::transform_to, p); - Expression projection(PinholeCamera::project_to_camera, - p_cam); - Expression uv_hat(uncalibrate, K, projection); +// Create expression tree +Expression projection(PinholeCamera::project_to_camera, p_cam); +Expression uv_hat(uncalibrate, K, projection); +} +/* ************************************************************************* */ +// keys +TEST(Expression, keys_tree) { // Check keys set expectedKeys; expectedKeys.insert(1); expectedKeys.insert(2); expectedKeys.insert(3); - EXPECT(expectedKeys == uv_hat.keys()); + EXPECT(expectedKeys == tree::uv_hat.keys()); +} +/* ************************************************************************* */ +// keys +TEST(Expression, block_tree) { +// // Check VerticalBlockMatrix +// size_t dimensions[3] = { 6, 3, 5 }; +// Matrix matrix(2, 14); +// VerticalBlockMatrix expected(dimensions, matrix), actual = +// tree::uv_hat.verticalBlockMatrix(); +// EXPECT( assert_equal(expected, *jf, 1e-9)); } - /* ************************************************************************* */ TEST(Expression, compose1) {