diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 941d21dd8..31f5fb295 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -50,7 +50,8 @@ TEST(Expression, test) { // Create expression tree Expression p_cam(x, &Pose3::transform_to, p); - Expression projection(PinholeCamera::project_to_camera, p_cam); + Expression projection(PinholeCamera::project_to_camera, + p_cam); Expression uv_hat(uncalibrate, K, projection); // Check keys @@ -91,7 +92,7 @@ TEST(Expression, compose2) { } /* ************************************************************************* */ -// Test compose with one arguments referring to a constant same rotation +// Test compose with one arguments referring to constant rotation TEST(Expression, compose3) { // Create expression @@ -104,6 +105,35 @@ TEST(Expression, compose3) { EXPECT(expectedKeys == R3.keys()); } +/* ************************************************************************* */ +// Test with ternary function +Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, + boost::optional H1, boost::optional H2, + boost::optional H3) { + // return dummy derivatives (not correct, but that's ok for testing here) + if (H1) + *H1 = eye(3); + if (H2) + *H2 = eye(3); + if (H3) + *H3 = eye(3); + return R1 * (R2 * R3); +} + +//TEST(Expression, ternary) { +// +// // Create expression +// Expression A(1), B(2), C(3); +// Expression ABC(composeThree, A, B, C); +// +// // Check keys +// std::set expectedKeys; +// expectedKeys.insert(1); +// expectedKeys.insert(2); +// expectedKeys.insert(3); +// EXPECT(expectedKeys == ABC.keys()); +//} + /* ************************************************************************* */ int main() { TestResult tr;