So much better as methods
parent
55cc4ba56c
commit
8100d89094
|
|
@ -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>());
|
||||||
}
|
}
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
|
||||||
|
|
@ -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);
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue