diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 3f090c881..6fef90d38 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -488,28 +488,27 @@ struct GenerateRecord: JacobianTrace, Base { template struct Record: public boost::mpl::fold, GenerateRecord >::type { + + /// Access JacobianTrace + template + JacobianTrace& jacobianTrace() { + return static_cast&>(*this); + } + + /// Access Trace + template + ExecutionTrace& trace() { + return jacobianTrace().trace; + } + + /// Access Jacobian + template + Eigen::Matrix& jacobian() { + return jacobianTrace().dTdA; + } + }; -/// Access JacobianTrace -template -JacobianTrace& jacobianTrace( - Record& record) { - return static_cast&>(record); -} - -/// Access Trace -template -ExecutionTrace& getTrace(Record* record) { - return jacobianTrace(*record).trace; -} - -/// Access Jacobian -template -Eigen::Matrix& jacobian( - Record* record) { - return jacobianTrace(*record).dTdA; -} - //----------------------------------------------------------------------------- /** * Building block for Recursive FunctionalNode Class @@ -606,9 +605,10 @@ public: trace.setFunction(record); raw = (char*) (record + 1); - A1 a1 = expressionA1_->traceExecution(values, getTrace(record), raw); + A1 a1 = expressionA1_->traceExecution(values, + record->template trace(), raw); - return function_(a1, jacobian(record)); + return function_(a1, record->template jacobian()); } }; @@ -685,11 +685,14 @@ public: trace.setFunction(record); raw = (char*) (record + 1); - A1 a1 = expressionA1_->traceExecution(values, getTrace(record), raw); + A1 a1 = expressionA1_->traceExecution(values, + record->template trace(), raw); raw = raw + expressionA1_->traceSize(); - A2 a2 = expressionA2_->traceExecution(values, getTrace(record), raw); + A2 a2 = expressionA2_->traceExecution(values, + record->template trace(), raw); - return function_(a1, a2, jacobian(record), jacobian(record)); + return function_(a1, a2, record->template jacobian(), + record->template jacobian()); } }; @@ -775,14 +778,17 @@ public: trace.setFunction(record); raw = (char*) (record + 1); - A1 a1 = expressionA1_->traceExecution(values, getTrace(record), raw); + A1 a1 = expressionA1_->traceExecution(values, + record->template trace(), raw); raw = raw + expressionA1_->traceSize(); - A2 a2 = expressionA2_->traceExecution(values, getTrace(record), raw); + A2 a2 = expressionA2_->traceExecution(values, + record->template trace(), raw); raw = raw + expressionA2_->traceSize(); - A3 a3 = expressionA3_->traceExecution(values, getTrace(record), raw); + A3 a3 = expressionA3_->traceExecution(values, + record->template trace(), raw); - return function_(a1, a2, a3, jacobian(record), - jacobian(record), jacobian(record)); + return function_(a1, a2, a3, record->template jacobian(), + record->template jacobian(), record->template jacobian()); } }; diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index a7923b157..16eb2fd7b 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -175,8 +175,9 @@ TEST(ExpressionFactor, binary) { // Check matrices boost::optional r = trace.record(); CHECK(r); - EXPECT(assert_equal(expected25, (Matrix) jacobian(*r), 1e-9)); - EXPECT(assert_equal(expected22, (Matrix) jacobian(*r), 1e-9)); + EXPECT( + assert_equal(expected25, (Matrix) (*r)-> jacobian(), 1e-9)); + EXPECT( assert_equal(expected22, (Matrix) (*r)->jacobian(), 1e-9)); } /* ************************************************************************* */ // Unary(Binary(Leaf,Leaf)) @@ -222,7 +223,7 @@ TEST(ExpressionFactor, shallow) { // Check matrices boost::optional r = trace.record(); CHECK(r); - EXPECT(assert_equal(expected23, (Matrix)jacobian(*r), 1e-9)); + EXPECT(assert_equal(expected23, (Matrix)(*r)->jacobian(), 1e-9)); // Linearization ExpressionFactor f2(model, measured, expression);