12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged in fix/unrollingReverseADInterfaceRecursionForCodeSimplicity (pull request #56) 

unrolled the reverseAD recursion (issue #151)
release/4.3a0
Frank Dellaert 2014-12-01 20:43:45 +01:00
parent 133dbc4ded 6c62609737
commit a92f9b3335
5 changed files with 184 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
gtsam/nonlinear/Expression-inl.h
View File

@ -106,7 +106,7 @@ struct UseBlockIf<false, Derived> {
}; };
} }
/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians /// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians
template<typename Derived> template<typename Derived>
void handleLeafCase(const Eigen::MatrixBase<Derived>& dTdA, void handleLeafCase(const Eigen::MatrixBase<Derived>& dTdA,
JacobianMap& jacobians, Key key) { JacobianMap& jacobians, Key key) {
@ -186,28 +186,28 @@ public:
} }
} }
/** /**
* *** This is the main entry point for reverseAD, called from Expression *** * *** This is the main entry point for reverse AD, called from Expression ***
* Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function)
*/ */
typedef Eigen::Matrix<double, Dim, Dim> JacobianTT; typedef Eigen::Matrix<double, Dim, Dim> JacobianTT;
void startReverseAD(JacobianMap& jacobians) const { void startReverseAD1(JacobianMap& jacobians) const {
if (kind == Leaf) { if (kind == Leaf) {
// This branch will only be called on trivial Leaf expressions, i.e. Priors // This branch will only be called on trivial Leaf expressions, i.e. Priors
static const JacobianTT I = JacobianTT::Identity(); static const JacobianTT I = JacobianTT::Identity();
handleLeafCase(I, jacobians, content.key); handleLeafCase(I, jacobians, content.key);
} else if (kind == Function) } else if (kind == Function)
// This is the more typical entry point, starting the AD pipeline // This is the more typical entry point, starting the AD pipeline
// Inside the startReverseAD that the correctly dimensioned pipeline is chosen. // Inside startReverseAD2 the correctly dimensioned pipeline is chosen.
content.ptr->startReverseAD(jacobians); content.ptr->startReverseAD2(jacobians);
} }
// Either add to Jacobians (Leaf) or propagate (Function) // Either add to Jacobians (Leaf) or propagate (Function)
template<typename DerivedMatrix> template<typename DerivedMatrix>
void reverseAD(const Eigen::MatrixBase<DerivedMatrix> & dTdA, void reverseAD1(const Eigen::MatrixBase<DerivedMatrix> & dTdA,
JacobianMap& jacobians) const { JacobianMap& jacobians) const {
if (kind == Leaf) if (kind == Leaf)
handleLeafCase(dTdA, jacobians, content.key); handleLeafCase(dTdA, jacobians, content.key);
else if (kind == Function) else if (kind == Function)
content.ptr->reverseAD(dTdA, jacobians); content.ptr->reverseAD2(dTdA, jacobians);
} }
/// Define type so we can apply it as a meta-function /// Define type so we can apply it as a meta-function
@ -470,10 +470,10 @@ struct FunctionalBase: ExpressionNode<T> {
struct Record { struct Record {
void print(const std::string& indent) const { void print(const std::string& indent) const {
} }
void startReverseAD(JacobianMap& jacobians) const { void startReverseAD4(JacobianMap& jacobians) const {
} }
template<typename SomeMatrix> template<typename SomeMatrix>
void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const { void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
} }
}; };
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
@ -505,9 +505,9 @@ struct JacobianTrace {
typename Jacobian<T, A>::type dTdA; typename Jacobian<T, A>::type dTdA;
}; };
/** // Recursive Definition of Functional ExpressionNode
* Recursive Definition of Functional ExpressionNode // The reason we inherit from Argument<T, A, N> is because we can then
*/ // case to this unique signature to retrieve the expression at any level
template<class T, class A, class Base> template<class T, class A, class Base>
struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base { struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
@ -528,7 +528,9 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
This::expression->dims(map); This::expression->dims(map);
} }
/// Recursive Record Class for Functional Expressions // Recursive Record Class for Functional Expressions
// The reason we inherit from JacobianTrace<T, A, N> is because we can then
// case to this unique signature to retrieve the value/trace at any level
struct Record: JacobianTrace<T, A, N>, Base::Record { struct Record: JacobianTrace<T, A, N>, Base::Record {
typedef T return_type; typedef T return_type;
@ -543,17 +545,26 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
} }
/// Start the reverse AD process /// Start the reverse AD process
void startReverseAD(JacobianMap& jacobians) const { void startReverseAD4(JacobianMap& jacobians) const {
Base::Record::startReverseAD(jacobians); Base::Record::startReverseAD4(jacobians);
This::trace.reverseAD(This::dTdA, jacobians); // This is the crucial point where the size of the AD pipeline is selected.
// One pipeline is started for each argument, but the number of rows in each
// pipeline is the same, namely the dimension of the output argument T.
// For example, if the entire expression is rooted by a binary function
// yielding a 2D result, then the matrix dTdA will have 2 rows.
// ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2
// which calls the correctly sized CallRecord::reverseAD3, which in turn
// calls reverseAD4 below.
This::trace.reverseAD1(This::dTdA, jacobians);
} }
/// Given df/dT, multiply in dT/dA and continue reverse AD process /// Given df/dT, multiply in dT/dA and continue reverse AD process
// Cols is always known at compile time
template<int Rows, int Cols> template<int Rows, int Cols>
void reverseAD(const Eigen::Matrix<double, Rows, Cols> & dFdT, void reverseAD4(const Eigen::Matrix<double, Rows, Cols> & dFdT,
JacobianMap& jacobians) const { JacobianMap& jacobians) const {
Base::Record::reverseAD(dFdT, jacobians); Base::Record::reverseAD4(dFdT, jacobians);
This::trace.reverseAD(dFdT * This::dTdA, jacobians); This::trace.reverseAD1(dFdT * This::dTdA, jacobians);
} }
}; };
@ -614,8 +625,8 @@ struct FunctionalNode {
struct Record: public internal::CallRecordImplementor<Record, struct Record: public internal::CallRecordImplementor<Record,
traits::dimension<T>::value>, public Base::Record { traits::dimension<T>::value>, public Base::Record {
using Base::Record::print; using Base::Record::print;
using Base::Record::startReverseAD; using Base::Record::startReverseAD4;
using Base::Record::reverseAD; using Base::Record::reverseAD4;
virtual ~Record() { virtual ~Record() {
} }

2
gtsam/nonlinear/Expression.h
View File

@ -209,7 +209,7 @@ private:
ExecutionTraceStorage traceStorage[size]; ExecutionTraceStorage traceStorage[size];
ExecutionTrace<T> trace; ExecutionTrace<T> trace;
T value(traceExecution(values, trace, traceStorage)); T value(traceExecution(values, trace, traceStorage));
trace.startReverseAD(jacobians); trace.startReverseAD1(jacobians);
return value; return value;
} }

177
gtsam_unstable/nonlinear/CallRecord.h
View File

@ -32,12 +32,6 @@ class JacobianMap;
// forward declaration // forward declaration
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/**
* MaxVirtualStaticRows defines how many separate virtual reverseAD with specific
* static rows (1..MaxVirtualStaticRows) methods will be part of the CallRecord interface.
*/
const int MaxVirtualStaticRows = 4;
namespace internal { namespace internal {
/** /**
@ -57,7 +51,8 @@ struct ConvertToVirtualFunctionSupportedMatrixType {
template<> template<>
struct ConvertToVirtualFunctionSupportedMatrixType<false> { struct ConvertToVirtualFunctionSupportedMatrixType<false> {
template<typename Derived> template<typename Derived>
static const Eigen::Matrix<double, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> convert( static const Eigen::Matrix<double, Derived::RowsAtCompileTime,
Derived::ColsAtCompileTime> convert(
const Eigen::MatrixBase<Derived> & x) { const Eigen::MatrixBase<Derived> & x) {
return x; return x;
} }
@ -69,126 +64,132 @@ struct ConvertToVirtualFunctionSupportedMatrixType<false> {
} }
}; };
/**
* Recursive definition of an interface having several purely
* virtual _reverseAD(const Eigen::Matrix<double, Rows, Cols> &, JacobianMap&)
* with Rows in 1..MaxSupportedStaticRows
*/
template<int MaxSupportedStaticRows, int Cols>
struct ReverseADInterface: ReverseADInterface<MaxSupportedStaticRows - 1, Cols> {
using ReverseADInterface<MaxSupportedStaticRows - 1, Cols>::_reverseAD;
virtual void _reverseAD(
const Eigen::Matrix<double, MaxSupportedStaticRows, Cols> & dFdT,
JacobianMap& jacobians) const = 0;
};
template<int Cols>
struct ReverseADInterface<0, Cols> {
virtual void _reverseAD(
const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT,
JacobianMap& jacobians) const = 0;
virtual void _reverseAD(const Matrix & dFdT,
JacobianMap& jacobians) const = 0;
};
/**
* ReverseADImplementor is a utility class used by CallRecordImplementor to
* implementing the recursive ReverseADInterface interface.
*/
template<typename Derived, int MaxSupportedStaticRows, int Cols>
struct ReverseADImplementor: ReverseADImplementor<Derived,
MaxSupportedStaticRows - 1, Cols> {
private:
using ReverseADImplementor<Derived, MaxSupportedStaticRows - 1, Cols>::_reverseAD;
virtual void _reverseAD(
const Eigen::Matrix<double, MaxSupportedStaticRows, Cols> & dFdT,
JacobianMap& jacobians) const {
static_cast<const Derived *>(this)->reverseAD(dFdT, jacobians);
}
friend struct internal::ReverseADImplementor<Derived,
MaxSupportedStaticRows + 1, Cols>;
};
template<typename Derived, int Cols>
struct ReverseADImplementor<Derived, 0, Cols> : virtual internal::ReverseADInterface<
MaxVirtualStaticRows, Cols> {
private:
using internal::ReverseADInterface<MaxVirtualStaticRows, Cols>::_reverseAD;
const Derived & derived() const {
return static_cast<const Derived&>(*this);
}
virtual void _reverseAD(
const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT,
JacobianMap& jacobians) const {
derived().reverseAD(dFdT, jacobians);
}
virtual void _reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const {
derived().reverseAD(dFdT, jacobians);
}
friend struct internal::ReverseADImplementor<Derived, 1, Cols>;
};
} // namespace internal } // namespace internal
/** /**
* The CallRecord class stores the Jacobians of applying a function * The CallRecord is an abstract base class for the any class that stores
* with respect to each of its arguments. It also stores an execution trace * the Jacobians of applying a function with respect to each of its arguments,
* (defined below) for each of its arguments. * as well as an execution trace for each of its arguments.
*
* It is implemented in the function-style ExpressionNode's nested Record class below.
*/ */
template<int Cols> template<int Cols>
struct CallRecord: virtual private internal::ReverseADInterface< struct CallRecord {
MaxVirtualStaticRows, Cols> {
// Print entire record, recursively
inline void print(const std::string& indent) const { inline void print(const std::string& indent) const {
_print(indent); _print(indent);
} }
inline void startReverseAD(JacobianMap& jacobians) const { // Main entry point for the reverse AD process of a functional expression.
_startReverseAD(jacobians); // Called *once* by the main AD entry point, ExecutionTrace::startReverseAD1
// This function then calls ExecutionTrace::reverseAD for every argument
// which will in turn call the reverseAD method below.
// This non-virtual function _startReverseAD3, implemented in derived
inline void startReverseAD2(JacobianMap& jacobians) const {
_startReverseAD3(jacobians);
} }
// Dispatch the reverseAD2 calls issued by ExecutionTrace::reverseAD1
// Here we convert to dynamic if the
template<typename Derived> template<typename Derived>
inline void reverseAD(const Eigen::MatrixBase<Derived> & dFdT, inline void reverseAD2(const Eigen::MatrixBase<Derived> & dFdT,
JacobianMap& jacobians) const { JacobianMap& jacobians) const {
_reverseAD( _reverseAD3(
internal::ConvertToVirtualFunctionSupportedMatrixType<(Derived::RowsAtCompileTime > MaxVirtualStaticRows)>::convert( internal::ConvertToVirtualFunctionSupportedMatrixType<
dFdT), jacobians); (Derived::RowsAtCompileTime > 5)>::convert(dFdT),
jacobians);
} }
inline void reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const { // This overload supports matrices with both rows and columns dynamically sized.
_reverseAD(dFdT, jacobians); // The template version above would be slower by introducing an extra conversion
// to statically sized columns.
inline void reverseAD2(const Matrix & dFdT, JacobianMap& jacobians) const {
_reverseAD3(dFdT, jacobians);
} }
virtual ~CallRecord() { virtual ~CallRecord() {
} }
private: private:
virtual void _print(const std::string& indent) const = 0; virtual void _print(const std::string& indent) const = 0;
virtual void _startReverseAD(JacobianMap& jacobians) const = 0; virtual void _startReverseAD3(JacobianMap& jacobians) const = 0;
using internal::ReverseADInterface<MaxVirtualStaticRows, Cols>::_reverseAD;
virtual void _reverseAD3(const Matrix & dFdT,
JacobianMap& jacobians) const = 0;
virtual void _reverseAD3(
const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT,
JacobianMap& jacobians) const = 0;
virtual void _reverseAD3(const Eigen::Matrix<double, 1, Cols> & dFdT,
JacobianMap& jacobians) const = 0;
virtual void _reverseAD3(const Eigen::Matrix<double, 2, Cols> & dFdT,
JacobianMap& jacobians) const = 0;
virtual void _reverseAD3(const Eigen::Matrix<double, 3, Cols> & dFdT,
JacobianMap& jacobians) const = 0;
virtual void _reverseAD3(const Eigen::Matrix<double, 4, Cols> & dFdT,
JacobianMap& jacobians) const = 0;
virtual void _reverseAD3(const Eigen::Matrix<double, 5, Cols> & dFdT,
JacobianMap& jacobians) const = 0;
}; };
/**
* CallRecordMaxVirtualStaticRows tells which separate virtual reverseAD with specific
* static rows (1..CallRecordMaxVirtualStaticRows) methods are part of the CallRecord
* interface. It is used to keep the testCallRecord unit test in sync.
*/
const int CallRecordMaxVirtualStaticRows = 5;
namespace internal { namespace internal {
/** /**
* The CallRecordImplementor implements the CallRecord interface for a Derived class by * The CallRecordImplementor implements the CallRecord interface for a Derived class by
* delegating to its corresponding (templated) non-virtual methods. * delegating to its corresponding (templated) non-virtual methods.
*/ */
template<typename Derived, int Cols> template<typename Derived, int Cols>
struct CallRecordImplementor: public CallRecord<Cols>, struct CallRecordImplementor: public CallRecord<Cols> {
private ReverseADImplementor<Derived, MaxVirtualStaticRows, Cols> {
private: private:
const Derived & derived() const { const Derived & derived() const {
return static_cast<const Derived&>(*this); return static_cast<const Derived&>(*this);
} }
virtual void _print(const std::string& indent) const { virtual void _print(const std::string& indent) const {
derived().print(indent); derived().print(indent);
} }
virtual void _startReverseAD(JacobianMap& jacobians) const {
derived().startReverseAD(jacobians); virtual void _startReverseAD3(JacobianMap& jacobians) const {
derived().startReverseAD4(jacobians);
}
virtual void _reverseAD3(const Matrix & dFdT, JacobianMap& jacobians) const {
derived().reverseAD4(dFdT, jacobians);
}
virtual void _reverseAD3(
const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT,
JacobianMap& jacobians) const {
derived().reverseAD4(dFdT, jacobians);
}
virtual void _reverseAD3(const Eigen::Matrix<double, 1, Cols> & dFdT,
JacobianMap& jacobians) const {
derived().reverseAD4(dFdT, jacobians);
}
virtual void _reverseAD3(const Eigen::Matrix<double, 2, Cols> & dFdT,
JacobianMap& jacobians) const {
derived().reverseAD4(dFdT, jacobians);
}
virtual void _reverseAD3(const Eigen::Matrix<double, 3, Cols> & dFdT,
JacobianMap& jacobians) const {
derived().reverseAD4(dFdT, jacobians);
}
virtual void _reverseAD3(const Eigen::Matrix<double, 4, Cols> & dFdT,
JacobianMap& jacobians) const {
derived().reverseAD4(dFdT, jacobians);
}
virtual void _reverseAD3(const Eigen::Matrix<double, 5, Cols> & dFdT,
JacobianMap& jacobians) const {
derived().reverseAD4(dFdT, jacobians);
} }
template<typename D, int R, int C> friend struct ReverseADImplementor;
}; };
} // namespace internal } // namespace internal

54
gtsam_unstable/nonlinear/tests/testCallRecord.cpp
View File

@ -33,7 +33,7 @@ static const int Cols = 3;
int dynamicIfAboveMax(int i){ int dynamicIfAboveMax(int i){
if(i > MaxVirtualStaticRows){ if(i > CallRecordMaxVirtualStaticRows){
return Eigen::Dynamic; return Eigen::Dynamic;
} }
else return i; else return i;
@ -43,7 +43,6 @@ struct CallConfig {
int compTimeCols; int compTimeCols;
int runTimeRows; int runTimeRows;
int runTimeCols; int runTimeCols;
CallConfig() {}
CallConfig(int rows, int cols): CallConfig(int rows, int cols):
compTimeRows(dynamicIfAboveMax(rows)), compTimeRows(dynamicIfAboveMax(rows)),
compTimeCols(cols), compTimeCols(cols),
@ -72,25 +71,26 @@ struct CallConfig {
}; };
struct Record: public internal::CallRecordImplementor<Record, Cols> { struct Record: public internal::CallRecordImplementor<Record, Cols> {
Record() : cc(0, 0) {}
virtual ~Record() { virtual ~Record() {
} }
void print(const std::string& indent) const { void print(const std::string& indent) const {
} }
void startReverseAD(JacobianMap& jacobians) const { void startReverseAD4(JacobianMap& jacobians) const {
} }
mutable CallConfig cc; mutable CallConfig cc;
private: private:
template<typename SomeMatrix> template<typename SomeMatrix>
void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const { void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
cc.compTimeRows = SomeMatrix::RowsAtCompileTime; cc.compTimeRows = SomeMatrix::RowsAtCompileTime;
cc.compTimeCols = SomeMatrix::ColsAtCompileTime; cc.compTimeCols = SomeMatrix::ColsAtCompileTime;
cc.runTimeRows = dFdT.rows(); cc.runTimeRows = dFdT.rows();
cc.runTimeCols = dFdT.cols(); cc.runTimeCols = dFdT.cols();
} }
template<typename Derived, int Rows, int OtherCols> template<typename Derived, int Rows>
friend struct internal::ReverseADImplementor; friend struct internal::CallRecordImplementor;
}; };
JacobianMap & NJM= *static_cast<JacobianMap *>(NULL); JacobianMap & NJM= *static_cast<JacobianMap *>(NULL);
@ -102,56 +102,56 @@ TEST(CallRecord, virtualReverseAdDispatching) {
Record record; Record record;
{ {
const int Rows = 1; const int Rows = 1;
record.CallRecord::reverseAD(Eigen::Matrix<double, Rows, Cols>(), NJM); record.CallRecord::reverseAD2(Eigen::Matrix<double, Rows, Cols>(), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols))));
} }
{ {
const int Rows = 2; const int Rows = 2;
record.CallRecord::reverseAD(Eigen::Matrix<double, Rows, Cols>(), NJM); record.CallRecord::reverseAD2(Eigen::Matrix<double, Rows, Cols>(), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols))));
} }
{ {
const int Rows = 3; const int Rows = 3;
record.CallRecord::reverseAD(Eigen::Matrix<double, Rows, Cols>(), NJM); record.CallRecord::reverseAD2(Eigen::Matrix<double, Rows, Cols>(), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols))));
} }
{ {
const int Rows = MaxVirtualStaticRows; const int Rows = 4;
record.CallRecord::reverseAD(Eigen::Matrix<double, Rows, Cols>(), NJM); record.CallRecord::reverseAD2(Eigen::Matrix<double, Rows, Cols>(), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols))));
} }
{ {
const int Rows = MaxVirtualStaticRows + 1; const int Rows = 5;
record.CallRecord::reverseAD(Eigen::Matrix<double, Rows, Cols>(), NJM); record.CallRecord::reverseAD2(Eigen::Matrix<double, Rows, Cols>(), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols))));
} }
{ {
const int Rows = MaxVirtualStaticRows + 2; const int Rows = 6;
record.CallRecord::reverseAD(Eigen::Matrix<double, Rows, Cols>(), NJM); record.CallRecord::reverseAD2(Eigen::Matrix<double, Rows, Cols>(), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols))));
} }
} }

35
gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
View File

@ -76,10 +76,13 @@ TEST(ExpressionFactor, Model) {
// Concise version // Concise version
ExpressionFactor<Point2> f(model, Point2(0, 0), p); ExpressionFactor<Point2> f(model, Point2(0, 0), p);
// Check values and derivatives
EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f.dim()); EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values); boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); // another way
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -124,6 +127,38 @@ TEST(ExpressionFactor, Unary) {
EXPECT( assert_equal(expected, *jf, 1e-9)); EXPECT( assert_equal(expected, *jf, 1e-9));
} }
/* ************************************************************************* */
// Unary(Leaf)) and Unary(Unary(Leaf)))
// wide version (not handled in fixed-size pipeline)
typedef Eigen::Matrix<double,9,3> Matrix93;
Vector9 wide(const Point3& p, boost::optional<Matrix93&> H) {
Vector9 v;
v << p.vector(), p.vector(), p.vector();
if (H) *H << eye(3), eye(3), eye(3);
return v;
}
typedef Eigen::Matrix<double,9,9> Matrix9;
Vector9 id9(const Vector9& v, boost::optional<Matrix9&> H) {
if (H) *H = Matrix9::Identity();
return v;
}
TEST(ExpressionFactor, Wide) {
// Create some values
Values values;
values.insert(2, Point3(0, 0, 1));
Point3_ point(2);
Vector9 measured;
Expression<Vector9> expression(wide,point);
SharedNoiseModel model = noiseModel::Unit::Create(9);
ExpressionFactor<Vector9> f1(model, measured, expression);
EXPECT_CORRECT_FACTOR_JACOBIANS(f1, values, 1e-5, 1e-9);
Expression<Vector9> expression2(id9,expression);
ExpressionFactor<Vector9> f2(model, measured, expression2);
EXPECT_CORRECT_FACTOR_JACOBIANS(f2, values, 1e-5, 1e-9);
}
/* ************************************************************************* */ /* ************************************************************************* */
static Point2 myUncal(const Cal3_S2& K, const Point2& p, static Point2 myUncal(const Cal3_S2& K, const Point2& p,
boost::optional<Matrix25&> Dcal, boost::optional<Matrix2&> Dp) { boost::optional<Matrix25&> Dcal, boost::optional<Matrix2&> Dp) {