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>
void handleLeafCase(const Eigen::MatrixBase<Derived>& dTdA,
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)
*/
typedef Eigen::Matrix<double, Dim, Dim> JacobianTT;
void startReverseAD(JacobianMap& jacobians) const {
void startReverseAD1(JacobianMap& jacobians) const {
if (kind == Leaf) {
// This branch will only be called on trivial Leaf expressions, i.e. Priors
static const JacobianTT I = JacobianTT::Identity();
handleLeafCase(I, jacobians, content.key);
} else if (kind == Function)
// This is the more typical entry point, starting the AD pipeline
// Inside the startReverseAD that the correctly dimensioned pipeline is chosen.
content.ptr->startReverseAD(jacobians);
// Inside startReverseAD2 the correctly dimensioned pipeline is chosen.
content.ptr->startReverseAD2(jacobians);
}
// Either add to Jacobians (Leaf) or propagate (Function)
template<typename DerivedMatrix>
void reverseAD(const Eigen::MatrixBase<DerivedMatrix> & dTdA,
void reverseAD1(const Eigen::MatrixBase<DerivedMatrix> & dTdA,
JacobianMap& jacobians) const {
if (kind == Leaf)
handleLeafCase(dTdA, jacobians, content.key);
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
@ -470,10 +470,10 @@ struct FunctionalBase: ExpressionNode<T> {
struct Record {
void print(const std::string& indent) const {
}
void startReverseAD(JacobianMap& jacobians) const {
void startReverseAD4(JacobianMap& jacobians) const {
}
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
@ -505,9 +505,9 @@ struct JacobianTrace {
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>
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);
}
/// 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 {
typedef T return_type;
@ -543,17 +545,26 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
}
/// Start the reverse AD process
void startReverseAD(JacobianMap& jacobians) const {
Base::Record::startReverseAD(jacobians);
This::trace.reverseAD(This::dTdA, jacobians);
void startReverseAD4(JacobianMap& jacobians) const {
Base::Record::startReverseAD4(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
// Cols is always known at compile time
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 {
Base::Record::reverseAD(dFdT, jacobians);
This::trace.reverseAD(dFdT * This::dTdA, jacobians);
Base::Record::reverseAD4(dFdT, jacobians);
This::trace.reverseAD1(dFdT * This::dTdA, jacobians);
}
};
@ -614,8 +625,8 @@ struct FunctionalNode {
struct Record: public internal::CallRecordImplementor<Record,
traits::dimension<T>::value>, public Base::Record {
using Base::Record::print;
using Base::Record::startReverseAD;
using Base::Record::reverseAD;
using Base::Record::startReverseAD4;
using Base::Record::reverseAD4;
virtual ~Record() {
}

2
gtsam/nonlinear/Expression.h
View File

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

177
gtsam_unstable/nonlinear/CallRecord.h
View File

@ -32,12 +32,6 @@ class JacobianMap;
// 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 {
/**
@ -57,7 +51,8 @@ struct ConvertToVirtualFunctionSupportedMatrixType {
template<>
struct ConvertToVirtualFunctionSupportedMatrixType<false> {
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) {
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
/**
* The CallRecord class stores the Jacobians of applying a function
* with respect to each of its arguments. It also stores an execution trace
* (defined below) for each of its arguments.
*
* It is implemented in the function-style ExpressionNode's nested Record class below.
* The CallRecord is an abstract base class for the any class that stores
* the Jacobians of applying a function with respect to each of its arguments,
* as well as an execution trace for each of its arguments.
*/
template<int Cols>
struct CallRecord: virtual private internal::ReverseADInterface<
MaxVirtualStaticRows, Cols> {
struct CallRecord {
// Print entire record, recursively
inline void print(const std::string& indent) const {
_print(indent);
}
inline void startReverseAD(JacobianMap& jacobians) const {
_startReverseAD(jacobians);
// Main entry point for the reverse AD process of a functional expression.
// 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>
inline void reverseAD(const Eigen::MatrixBase<Derived> & dFdT,
inline void reverseAD2(const Eigen::MatrixBase<Derived> & dFdT,
JacobianMap& jacobians) const {
_reverseAD(
internal::ConvertToVirtualFunctionSupportedMatrixType<(Derived::RowsAtCompileTime > MaxVirtualStaticRows)>::convert(
dFdT), jacobians);
_reverseAD3(
internal::ConvertToVirtualFunctionSupportedMatrixType<
(Derived::RowsAtCompileTime > 5)>::convert(dFdT),
jacobians);
}
inline void reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const {
_reverseAD(dFdT, jacobians);
// This overload supports matrices with both rows and columns dynamically sized.
// 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() {
}
private:
virtual void _print(const std::string& indent) const = 0;
virtual void _startReverseAD(JacobianMap& jacobians) const = 0;
using internal::ReverseADInterface<MaxVirtualStaticRows, Cols>::_reverseAD;
virtual void _startReverseAD3(JacobianMap& jacobians) const = 0;
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 {
/**
* The CallRecordImplementor implements the CallRecord interface for a Derived class by
* delegating to its corresponding (templated) non-virtual methods.
*/
template<typename Derived, int Cols>
struct CallRecordImplementor: public CallRecord<Cols>,
private ReverseADImplementor<Derived, MaxVirtualStaticRows, Cols> {
struct CallRecordImplementor: public CallRecord<Cols> {
private:
const Derived & derived() const {
return static_cast<const Derived&>(*this);
}
virtual void _print(const std::string& indent) const {
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

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

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

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

@ -76,10 +76,13 @@ TEST(ExpressionFactor, Model) {
// Concise version
ExpressionFactor<Point2> f(model, Point2(0, 0), p);
// Check values and derivatives
EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f.dim());
boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
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));
}
/* ************************************************************************* */
// 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,
boost::optional<Matrix25&> Dcal, boost::optional<Matrix2&> Dp) {