So much better as methods

release/4.3a0
dellaert 2014-10-12 23:57:08 +02:00
parent 55cc4ba56c
commit 8100d89094
2 changed files with 40 additions and 33 deletions

View File

@ -488,28 +488,27 @@ struct GenerateRecord: JacobianTrace<T, A, Base::N + 1>, Base {
template<class T, class TYPES> template<class T, class TYPES>
struct Record: public boost::mpl::fold<TYPES, CallRecord<T::dimension>, struct Record: public boost::mpl::fold<TYPES, CallRecord<T::dimension>,
GenerateRecord<T, MPL::_2, MPL::_1> >::type { GenerateRecord<T, MPL::_2, MPL::_1> >::type {
/// Access JacobianTrace
template<class A, size_t N>
JacobianTrace<typename Record::return_type, A, N>& jacobianTrace() {
return static_cast<JacobianTrace<T, A, N>&>(*this);
}
/// Access Trace
template<class A, size_t N>
ExecutionTrace<A>& trace() {
return jacobianTrace<A, N>().trace;
}
/// Access Jacobian
template<class A, size_t N>
Eigen::Matrix<double, T::dimension, A::dimension>& jacobian() {
return jacobianTrace<A, N>().dTdA;
}
}; };
/// Access JacobianTrace
template<class A, size_t N, class Record>
JacobianTrace<typename Record::return_type, A, N>& jacobianTrace(
Record& record) {
return static_cast<JacobianTrace<typename Record::return_type, A, N>&>(record);
}
/// Access Trace
template<class A, size_t N, class Record>
ExecutionTrace<A>& getTrace(Record* record) {
return jacobianTrace<A, N>(*record).trace;
}
/// Access Jacobian
template<class A, size_t N, class Record>
Eigen::Matrix<double, Record::return_type::dimension, A::dimension>& jacobian(
Record* record) {
return jacobianTrace<A, N>(*record).dTdA;
}
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/** /**
* Building block for Recursive FunctionalNode Class * Building block for Recursive FunctionalNode Class
@ -606,9 +605,10 @@ public:
trace.setFunction(record); trace.setFunction(record);
raw = (char*) (record + 1); raw = (char*) (record + 1);
A1 a1 = expressionA1_->traceExecution(values, getTrace<A1, 1>(record), raw); A1 a1 = expressionA1_->traceExecution(values,
record->template trace<A1, 1>(), raw);
return function_(a1, jacobian<A1, 1>(record)); return function_(a1, record->template jacobian<A1, 1>());
} }
}; };
@ -685,11 +685,14 @@ public:
trace.setFunction(record); trace.setFunction(record);
raw = (char*) (record + 1); raw = (char*) (record + 1);
A1 a1 = expressionA1_->traceExecution(values, getTrace<A1, 1>(record), raw); A1 a1 = expressionA1_->traceExecution(values,
record->template trace<A1, 1>(), raw);
raw = raw + expressionA1_->traceSize(); raw = raw + expressionA1_->traceSize();
A2 a2 = expressionA2_->traceExecution(values, getTrace<A2, 2>(record), raw); A2 a2 = expressionA2_->traceExecution(values,
record->template trace<A2, 2>(), raw);
return function_(a1, a2, jacobian<A1, 1>(record), jacobian<A2, 2>(record)); return function_(a1, a2, record->template jacobian<A1, 1>(),
record->template jacobian<A2, 2>());
} }
}; };
@ -775,14 +778,17 @@ public:
trace.setFunction(record); trace.setFunction(record);
raw = (char*) (record + 1); raw = (char*) (record + 1);
A1 a1 = expressionA1_->traceExecution(values, getTrace<A1, 1>(record), raw); A1 a1 = expressionA1_->traceExecution(values,
record->template trace<A1, 1>(), raw);
raw = raw + expressionA1_->traceSize(); raw = raw + expressionA1_->traceSize();
A2 a2 = expressionA2_->traceExecution(values, getTrace<A2, 2>(record), raw); A2 a2 = expressionA2_->traceExecution(values,
record->template trace<A2, 2>(), raw);
raw = raw + expressionA2_->traceSize(); raw = raw + expressionA2_->traceSize();
A3 a3 = expressionA3_->traceExecution(values, getTrace<A3, 3>(record), raw); A3 a3 = expressionA3_->traceExecution(values,
record->template trace<A3, 3>(), raw);
return function_(a1, a2, a3, jacobian<A1, 1>(record), return function_(a1, a2, a3, record->template jacobian<A1, 1>(),
jacobian<A2, 2>(record), jacobian<A3, 3>(record)); record->template jacobian<A2, 2>(), record->template jacobian<A3, 3>());
} }
}; };

View File

@ -175,8 +175,9 @@ TEST(ExpressionFactor, binary) {
// Check matrices // Check matrices
boost::optional<Binary::Record*> r = trace.record<Binary::Record>(); boost::optional<Binary::Record*> r = trace.record<Binary::Record>();
CHECK(r); CHECK(r);
EXPECT(assert_equal(expected25, (Matrix) jacobian<Cal3_S2, 1>(*r), 1e-9)); EXPECT(
EXPECT(assert_equal(expected22, (Matrix) jacobian<Point2, 2>(*r), 1e-9)); assert_equal(expected25, (Matrix) (*r)-> jacobian<Cal3_S2, 1>(), 1e-9));
EXPECT( assert_equal(expected22, (Matrix) (*r)->jacobian<Point2, 2>(), 1e-9));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Unary(Binary(Leaf,Leaf)) // Unary(Binary(Leaf,Leaf))
@ -222,7 +223,7 @@ TEST(ExpressionFactor, shallow) {
// Check matrices // Check matrices
boost::optional<Unary::Record*> r = trace.record<Unary::Record>(); boost::optional<Unary::Record*> r = trace.record<Unary::Record>();
CHECK(r); CHECK(r);
EXPECT(assert_equal(expected23, (Matrix)jacobian<Point3, 1>(*r), 1e-9)); EXPECT(assert_equal(expected23, (Matrix)(*r)->jacobian<Point3, 1>(), 1e-9));
// Linearization // Linearization
ExpressionFactor<Point2> f2(model, measured, expression); ExpressionFactor<Point2> f2(model, measured, expression);