diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 87ff1c7fb..4f5d2a1c6 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -91,11 +91,15 @@ public: type = Function; content.ptr = record; } - /// Return record pointer, highly unsafe, used only for testing - boost::optional*> record() { - return - (type == Function) ? boost::optional*>(content.ptr) : - boost::none; + /// Return record pointer, quite unsafe, used only for testing + template + boost::optional record() { + if (type != Function) + return boost::none; + else { + Record* p = dynamic_cast(content.ptr); + return p ? boost::optional(p) : boost::none; + } } // *** This is the main entry point for reverseAD, called from Expression::augmented *** // Called only once, either inserts identity into Jacobians (Leaf) or starts AD (Function) diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 3a9aa28a0..e1694f59a 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -122,6 +122,8 @@ struct TestBinaryExpression { /* ************************************************************************* */ // Binary(Leaf,Leaf) TEST(ExpressionFactor, binary) { + + typedef BinaryExpression Binary; TestBinaryExpression tester; // Create some values @@ -129,26 +131,31 @@ TEST(ExpressionFactor, binary) { values.insert(1, Cal3_S2()); values.insert(2, Point2(0, 0)); + // Expected Jacobians + Matrix25 expected25; + expected25 << 0, 0, 0, 1, 0, 0, 0, 0, 0, 1; + Matrix2 expected22; + expected22 << 1, 0, 0, 1; + // Do old trace ExecutionTrace trace; tester.binary_.traceExecution(values, trace); - // Extract record :-( - boost::optional*> r = trace.record(); - CHECK(r); - typedef BinaryExpression Binary; - Binary::Record* p = dynamic_cast(*r); - CHECK(p); - // Check matrices - Matrix25 expected25; - expected25 << 0, 0, 0, 1, 0, 0, 0, 0, 0, 1; - EXPECT( assert_equal(expected25, (Matrix)p->dTdA1, 1e-9)); - Matrix2 expected22; - expected22 << 1, 0, 0, 1; - EXPECT( assert_equal(expected22, (Matrix)p->dTdA2, 1e-9)); + boost::optional p = trace.record(); + CHECK(p); + EXPECT( assert_equal(expected25, (Matrix)(*p)->dTdA1, 1e-9)); + EXPECT( assert_equal(expected22, (Matrix)(*p)->dTdA2, 1e-9)); - // Check raw memory trace +// // Check raw memory trace +// double raw[10]; +// tester.binary_.traceRaw(values, 0); +// +// // Check matrices +// boost::optional p = trace.record(); +// CHECK(p); +// EXPECT( assert_equal(expected25, (Matrix)(*p)->dTdA1, 1e-9)); +// EXPECT( assert_equal(expected22, (Matrix)(*p)->dTdA2, 1e-9)); } /* ************************************************************************* */ // Unary(Binary(Leaf,Leaf))