Major re-org in preparation of recursive Functional nodes
dellaert
parent
408be628d2
commit
ef21a4ba4a
|
|
@ -54,6 +54,114 @@ void move(JacobianMap& jacobians, std::vector<Matrix>& H) {
|
||||||
it->second.swap(H[j++]);
|
it->second.swap(H[j++]);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
//-----------------------------------------------------------------------------
|
||||||
|
/**
|
||||||
|
* Value and Jacobians
|
||||||
|
*/
|
||||||
|
template<class T>
|
||||||
|
class Augmented {
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
T value_;
|
||||||
|
JacobianMap jacobians_;
|
||||||
|
|
||||||
|
typedef std::pair<Key, Matrix> Pair;
|
||||||
|
|
||||||
|
/// Insert terms into jacobians_, adding if already exists
|
||||||
|
void add(const JacobianMap& terms) {
|
||||||
|
BOOST_FOREACH(const Pair& term, terms) {
|
||||||
|
JacobianMap::iterator it = jacobians_.find(term.first);
|
||||||
|
if (it != jacobians_.end())
|
||||||
|
it->second += term.second;
|
||||||
|
else
|
||||||
|
jacobians_[term.first] = term.second;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Insert terms into jacobians_, premultiplying by H, adding if already exists
|
||||||
|
void add(const Matrix& H, const JacobianMap& terms) {
|
||||||
|
BOOST_FOREACH(const Pair& term, terms) {
|
||||||
|
JacobianMap::iterator it = jacobians_.find(term.first);
|
||||||
|
if (it != jacobians_.end())
|
||||||
|
it->second += H * term.second;
|
||||||
|
else
|
||||||
|
jacobians_[term.first] = H * term.second;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
/// Construct value that does not depend on anything
|
||||||
|
Augmented(const T& t) :
|
||||||
|
value_(t) {
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Construct value dependent on a single key
|
||||||
|
Augmented(const T& t, Key key) :
|
||||||
|
value_(t) {
|
||||||
|
size_t n = t.dim();
|
||||||
|
jacobians_[key] = Eigen::MatrixXd::Identity(n, n);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Construct value, pre-multiply jacobians by dTdA
|
||||||
|
Augmented(const T& t, const Matrix& dTdA, const JacobianMap& jacobians) :
|
||||||
|
value_(t) {
|
||||||
|
add(dTdA, jacobians);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Construct value, pre-multiply jacobians
|
||||||
|
Augmented(const T& t, const Matrix& dTdA1, const JacobianMap& jacobians1,
|
||||||
|
const Matrix& dTdA2, const JacobianMap& jacobians2) :
|
||||||
|
value_(t) {
|
||||||
|
add(dTdA1, jacobians1);
|
||||||
|
add(dTdA2, jacobians2);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Construct value, pre-multiply jacobians
|
||||||
|
Augmented(const T& t, const Matrix& dTdA1, const JacobianMap& jacobians1,
|
||||||
|
const Matrix& dTdA2, const JacobianMap& jacobians2, const Matrix& dTdA3,
|
||||||
|
const JacobianMap& jacobians3) :
|
||||||
|
value_(t) {
|
||||||
|
add(dTdA1, jacobians1);
|
||||||
|
add(dTdA2, jacobians2);
|
||||||
|
add(dTdA3, jacobians3);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Return value
|
||||||
|
const T& value() const {
|
||||||
|
return value_;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Return jacobians
|
||||||
|
const JacobianMap& jacobians() const {
|
||||||
|
return jacobians_;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Return jacobians
|
||||||
|
JacobianMap& jacobians() {
|
||||||
|
return jacobians_;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Not dependent on any key
|
||||||
|
bool constant() const {
|
||||||
|
return jacobians_.empty();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// debugging
|
||||||
|
void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) {
|
||||||
|
BOOST_FOREACH(const Pair& term, jacobians_)
|
||||||
|
std::cout << "(" << keyFormatter(term.first) << ", " << term.second.rows()
|
||||||
|
<< "x" << term.second.cols() << ") ";
|
||||||
|
std::cout << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Move terms to array, destroys content
|
||||||
|
void move(std::vector<Matrix>& H) {
|
||||||
|
move(jacobians_, H);
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
//-----------------------------------------------------------------------------
|
//-----------------------------------------------------------------------------
|
||||||
/**
|
/**
|
||||||
|
|
@ -188,195 +296,6 @@ struct Select<2, A> {
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
//-----------------------------------------------------------------------------
|
|
||||||
/**
|
|
||||||
* Record the evaluation of a single argument in a functional expression
|
|
||||||
* Building block for Recursive Record Class
|
|
||||||
*/
|
|
||||||
template<class T, class A, size_t N>
|
|
||||||
struct Argument {
|
|
||||||
typedef Eigen::Matrix<double, T::dimension, A::dimension> JacobianTA;
|
|
||||||
ExecutionTrace<A> trace;
|
|
||||||
JacobianTA dTdA;
|
|
||||||
};
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Recursive Record Class for Functional Expressions
|
|
||||||
* Abrahams, David; Gurtovoy, Aleksey (2004-12-10).
|
|
||||||
* C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost
|
|
||||||
* and Beyond. Pearson Education.
|
|
||||||
*/
|
|
||||||
template<class T, class A, class More>
|
|
||||||
struct Record: Argument<T, A, More::N+1>, More {
|
|
||||||
|
|
||||||
typedef T return_type;
|
|
||||||
static size_t const N = More::N + 1;
|
|
||||||
typedef Argument<T, A, N> This;
|
|
||||||
|
|
||||||
/// Print to std::cout
|
|
||||||
virtual void print(const std::string& indent) const {
|
|
||||||
More::print(indent);
|
|
||||||
static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
|
|
||||||
std::cout << This::dTdA.format(matlab) << std::endl;
|
|
||||||
This::trace.print(indent);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Start the reverse AD process
|
|
||||||
virtual void startReverseAD(JacobianMap& jacobians) const {
|
|
||||||
More::startReverseAD(jacobians);
|
|
||||||
Select<T::dimension, A>::reverseAD(This::trace, This::dTdA, jacobians);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Given df/dT, multiply in dT/dA and continue reverse AD process
|
|
||||||
virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const {
|
|
||||||
More::reverseAD(dFdT, jacobians);
|
|
||||||
This::trace.reverseAD(dFdT * This::dTdA, jacobians);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Version specialized to 2-dimensional output
|
|
||||||
typedef Eigen::Matrix<double, 2, T::dimension> Jacobian2T;
|
|
||||||
virtual void reverseAD2(const Jacobian2T& dFdT,
|
|
||||||
JacobianMap& jacobians) const {
|
|
||||||
More::reverseAD2(dFdT, jacobians);
|
|
||||||
This::trace.reverseAD2(dFdT * This::dTdA, jacobians);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
/// Recursive Record class Generator
|
|
||||||
template<class T, class TYPES>
|
|
||||||
struct GenerateRecord {
|
|
||||||
typedef typename boost::mpl::fold<TYPES, CallRecord<T::dimension>,
|
|
||||||
Record<T, MPL::_2, MPL::_1> >::type type;
|
|
||||||
};
|
|
||||||
|
|
||||||
/// Access Argument
|
|
||||||
template<class A, size_t N, class Record>
|
|
||||||
Argument<typename Record::return_type, A, N>& argument(Record& record) {
|
|
||||||
return static_cast<Argument<typename Record::return_type, A, N>&>(record);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Access Trace
|
|
||||||
template<class A, size_t N, class Record>
|
|
||||||
ExecutionTrace<A>& getTrace(Record* record) {
|
|
||||||
return argument<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 argument<A, N>(*record).dTdA;
|
|
||||||
}
|
|
||||||
|
|
||||||
//-----------------------------------------------------------------------------
|
|
||||||
/**
|
|
||||||
* Value and Jacobians
|
|
||||||
*/
|
|
||||||
template<class T>
|
|
||||||
class Augmented {
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
T value_;
|
|
||||||
JacobianMap jacobians_;
|
|
||||||
|
|
||||||
typedef std::pair<Key, Matrix> Pair;
|
|
||||||
|
|
||||||
/// Insert terms into jacobians_, adding if already exists
|
|
||||||
void add(const JacobianMap& terms) {
|
|
||||||
BOOST_FOREACH(const Pair& term, terms) {
|
|
||||||
JacobianMap::iterator it = jacobians_.find(term.first);
|
|
||||||
if (it != jacobians_.end())
|
|
||||||
it->second += term.second;
|
|
||||||
else
|
|
||||||
jacobians_[term.first] = term.second;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Insert terms into jacobians_, premultiplying by H, adding if already exists
|
|
||||||
void add(const Matrix& H, const JacobianMap& terms) {
|
|
||||||
BOOST_FOREACH(const Pair& term, terms) {
|
|
||||||
JacobianMap::iterator it = jacobians_.find(term.first);
|
|
||||||
if (it != jacobians_.end())
|
|
||||||
it->second += H * term.second;
|
|
||||||
else
|
|
||||||
jacobians_[term.first] = H * term.second;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
/// Construct value that does not depend on anything
|
|
||||||
Augmented(const T& t) :
|
|
||||||
value_(t) {
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Construct value dependent on a single key
|
|
||||||
Augmented(const T& t, Key key) :
|
|
||||||
value_(t) {
|
|
||||||
size_t n = t.dim();
|
|
||||||
jacobians_[key] = Eigen::MatrixXd::Identity(n, n);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Construct value, pre-multiply jacobians by dTdA
|
|
||||||
Augmented(const T& t, const Matrix& dTdA, const JacobianMap& jacobians) :
|
|
||||||
value_(t) {
|
|
||||||
add(dTdA, jacobians);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Construct value, pre-multiply jacobians
|
|
||||||
Augmented(const T& t, const Matrix& dTdA1, const JacobianMap& jacobians1,
|
|
||||||
const Matrix& dTdA2, const JacobianMap& jacobians2) :
|
|
||||||
value_(t) {
|
|
||||||
add(dTdA1, jacobians1);
|
|
||||||
add(dTdA2, jacobians2);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Construct value, pre-multiply jacobians
|
|
||||||
Augmented(const T& t, const Matrix& dTdA1, const JacobianMap& jacobians1,
|
|
||||||
const Matrix& dTdA2, const JacobianMap& jacobians2, const Matrix& dTdA3,
|
|
||||||
const JacobianMap& jacobians3) :
|
|
||||||
value_(t) {
|
|
||||||
add(dTdA1, jacobians1);
|
|
||||||
add(dTdA2, jacobians2);
|
|
||||||
add(dTdA3, jacobians3);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Return value
|
|
||||||
const T& value() const {
|
|
||||||
return value_;
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Return jacobians
|
|
||||||
const JacobianMap& jacobians() const {
|
|
||||||
return jacobians_;
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Return jacobians
|
|
||||||
JacobianMap& jacobians() {
|
|
||||||
return jacobians_;
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Not dependent on any key
|
|
||||||
bool constant() const {
|
|
||||||
return jacobians_.empty();
|
|
||||||
}
|
|
||||||
|
|
||||||
/// debugging
|
|
||||||
void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) {
|
|
||||||
BOOST_FOREACH(const Pair& term, jacobians_)
|
|
||||||
std::cout << "(" << keyFormatter(term.first) << ", " << term.second.rows()
|
|
||||||
<< "x" << term.second.cols() << ") ";
|
|
||||||
std::cout << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Move terms to array, destroys content
|
|
||||||
void move(std::vector<Matrix>& H) {
|
|
||||||
move(jacobians_, H);
|
|
||||||
}
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
//-----------------------------------------------------------------------------
|
//-----------------------------------------------------------------------------
|
||||||
/**
|
/**
|
||||||
* Expression node. The superclass for objects that do the heavy lifting
|
* Expression node. The superclass for objects that do the heavy lifting
|
||||||
|
|
@ -388,6 +307,10 @@ public:
|
||||||
template<class T>
|
template<class T>
|
||||||
class ExpressionNode {
|
class ExpressionNode {
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
static size_t const N = 0; // number of arguments
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
size_t traceSize_;
|
size_t traceSize_;
|
||||||
|
|
@ -505,6 +428,123 @@ public:
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
//-----------------------------------------------------------------------------
|
||||||
|
/**
|
||||||
|
* Building block for Recursive Record Class
|
||||||
|
* Records the evaluation of a single argument in a functional expression
|
||||||
|
* The integer argument N is to guarantee a unique type signature,
|
||||||
|
* so we are guaranteed to be able to extract their values by static cast.
|
||||||
|
*/
|
||||||
|
template<class T, class A, size_t N>
|
||||||
|
struct JacobianTrace {
|
||||||
|
typedef Eigen::Matrix<double, T::dimension, A::dimension> JacobianTA;
|
||||||
|
ExecutionTrace<A> trace;
|
||||||
|
JacobianTA dTdA;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Recursive Record Class for Functional Expressions
|
||||||
|
* Abrahams, David; Gurtovoy, Aleksey (2004-12-10).
|
||||||
|
* C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost
|
||||||
|
* and Beyond. Pearson Education.
|
||||||
|
*/
|
||||||
|
template<class T, class A, class Base>
|
||||||
|
struct Record: JacobianTrace<T, A, Base::N + 1>, Base {
|
||||||
|
|
||||||
|
typedef T return_type;
|
||||||
|
static size_t const N = Base::N + 1;
|
||||||
|
typedef JacobianTrace<T, A, N> This;
|
||||||
|
|
||||||
|
/// Print to std::cout
|
||||||
|
virtual void print(const std::string& indent) const {
|
||||||
|
Base::print(indent);
|
||||||
|
static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
|
||||||
|
std::cout << This::dTdA.format(matlab) << std::endl;
|
||||||
|
This::trace.print(indent);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Start the reverse AD process
|
||||||
|
virtual void startReverseAD(JacobianMap& jacobians) const {
|
||||||
|
Base::startReverseAD(jacobians);
|
||||||
|
Select<T::dimension, A>::reverseAD(This::trace, This::dTdA, jacobians);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Given df/dT, multiply in dT/dA and continue reverse AD process
|
||||||
|
virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const {
|
||||||
|
Base::reverseAD(dFdT, jacobians);
|
||||||
|
This::trace.reverseAD(dFdT * This::dTdA, jacobians);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Version specialized to 2-dimensional output
|
||||||
|
typedef Eigen::Matrix<double, 2, T::dimension> Jacobian2T;
|
||||||
|
virtual void reverseAD2(const Jacobian2T& dFdT,
|
||||||
|
JacobianMap& jacobians) const {
|
||||||
|
Base::reverseAD2(dFdT, jacobians);
|
||||||
|
This::trace.reverseAD2(dFdT * This::dTdA, jacobians);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Recursive Record class Generator
|
||||||
|
template<class T, class TYPES>
|
||||||
|
struct GenerateRecord {
|
||||||
|
typedef typename boost::mpl::fold<TYPES, CallRecord<T::dimension>,
|
||||||
|
Record<T, MPL::_2, MPL::_1> >::type type;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// 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
|
||||||
|
*/
|
||||||
|
template<class T, class A, size_t N>
|
||||||
|
struct Argument {
|
||||||
|
boost::shared_ptr<ExpressionNode<A> > expressionA_;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Recursive Definition of Functional ExpressionNode
|
||||||
|
*/
|
||||||
|
template<class T, class A, class Base>
|
||||||
|
struct FunctionalNode: Argument<T, A, Base::N + 1>, Base {
|
||||||
|
|
||||||
|
typedef T return_type;
|
||||||
|
static size_t const N = Base::N + 1;
|
||||||
|
typedef Argument<T, A, N> This;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Recursive GenerateFunctionalNode class Generator
|
||||||
|
template<class T, class TYPES>
|
||||||
|
struct GenerateFunctionalNode {
|
||||||
|
typedef typename boost::mpl::fold<TYPES, ExpressionNode<T>,
|
||||||
|
Record<T, MPL::_2, MPL::_1> >::type type;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Access Argument
|
||||||
|
template<class A, size_t N, class Record>
|
||||||
|
Argument<typename Record::return_type, A, N>& argument(Record& record) {
|
||||||
|
return static_cast<Argument<typename Record::return_type, A, N>&>(record);
|
||||||
|
}
|
||||||
|
|
||||||
//-----------------------------------------------------------------------------
|
//-----------------------------------------------------------------------------
|
||||||
/// Unary Function Expression
|
/// Unary Function Expression
|
||||||
template<class T, class A1>
|
template<class T, class A1>
|
||||||
|
|
|
||||||
|
|
@ -175,10 +175,8 @@ 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(
|
EXPECT(assert_equal(expected25, (Matrix) jacobian<Cal3_S2, 1>(*r), 1e-9));
|
||||||
assert_equal(expected25, (Matrix) static_cast<Argument<Point2, Cal3_S2, 1>*> (*r)->dTdA, 1e-9));
|
EXPECT(assert_equal(expected22, (Matrix) jacobian<Point2, 2>(*r), 1e-9));
|
||||||
EXPECT(
|
|
||||||
assert_equal(expected22, (Matrix) static_cast<Argument<Point2, Point2, 2>*> (*r)->dTdA, 1e-9));
|
|
||||||
}
|
}
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// Unary(Binary(Leaf,Leaf))
|
// Unary(Binary(Leaf,Leaf))
|
||||||
|
|
@ -224,8 +222,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(
|
EXPECT(assert_equal(expected23, (Matrix)jacobian<Point3, 1>(*r), 1e-9));
|
||||||
assert_equal(expected23, (Matrix)static_cast<Argument<Point2, Point3, 1>*>(*r)->dTdA, 1e-9));
|
|
||||||
|
|
||||||
// Linearization
|
// Linearization
|
||||||
ExpressionFactor<Point2> f2(model, measured, expression);
|
ExpressionFactor<Point2> f2(model, measured, expression);
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue