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

JacobianTrace no longer templated

release/4.3a0
dellaert 2014-10-07 16:11:55 +02:00
parent 83d77271d9
commit e4392c0a3b
4 changed files with 61 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
gtsam_unstable/nonlinear/Expression-inl.h
View File

@ -22,6 +22,7 @@
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
namespace gtsam { namespace gtsam {
@ -134,16 +135,23 @@ public:
}; };
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
template<class T>
struct JacobianTrace { struct JacobianTrace {
T t;
T value() const {
return t;
}
virtual void reverseAD(JacobianMap& jacobians) const = 0; virtual void reverseAD(JacobianMap& jacobians) const = 0;
virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0; virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0;
// template<class JacobianFT>
// void reverseAD(const JacobianFT& dFdT, JacobianMap& jacobians) const {
}; };
typedef boost::shared_ptr<JacobianTrace> TracePtr;
//template <class Derived>
//struct TypedTrace {
// virtual void reverseAD(JacobianMap& jacobians) const = 0;
// virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0;
//// template<class JacobianFT>
//// void reverseAD(const JacobianFT& dFdT, JacobianMap& jacobians) const {
//};
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/** /**
* Expression node. The superclass for objects that do the heavy lifting * Expression node. The superclass for objects that do the heavy lifting
@ -175,8 +183,7 @@ public:
virtual Augmented<T> forward(const Values& values) const = 0; virtual Augmented<T> forward(const Values& values) const = 0;
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual boost::shared_ptr<JacobianTrace<T> > traceExecution( virtual std::pair<T, TracePtr> traceExecution(const Values& values) const = 0;
const Values& values) const = 0;
}; };
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
@ -217,7 +224,7 @@ public:
} }
/// Trace structure for reverse AD /// Trace structure for reverse AD
struct Trace: public JacobianTrace<T> { struct Trace: public JacobianTrace {
/// If the expression is just a constant, we do nothing /// If the expression is just a constant, we do nothing
virtual void reverseAD(JacobianMap& jacobians) const { virtual void reverseAD(JacobianMap& jacobians) const {
} }
@ -227,11 +234,9 @@ public:
}; };
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual boost::shared_ptr<JacobianTrace<T> > traceExecution( virtual std::pair<T, TracePtr> traceExecution(const Values& values) const {
const Values& values) const {
boost::shared_ptr<Trace> trace = boost::make_shared<Trace>(); boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
trace->t = constant_; return std::make_pair(constant_, trace);
return trace;
} }
}; };
@ -270,12 +275,11 @@ public:
/// Return value and derivatives /// Return value and derivatives
virtual Augmented<T> forward(const Values& values) const { virtual Augmented<T> forward(const Values& values) const {
T t = value(values); return Augmented<T>(values.at<T>(key_), key_);
return Augmented<T>(t, key_);
} }
/// Trace structure for reverse AD /// Trace structure for reverse AD
struct Trace: public JacobianTrace<T> { struct Trace: public JacobianTrace {
Key key; Key key;
/// If the expression is just a leaf, we just insert an identity matrix /// If the expression is just a leaf, we just insert an identity matrix
virtual void reverseAD(JacobianMap& jacobians) const { virtual void reverseAD(JacobianMap& jacobians) const {
@ -293,12 +297,10 @@ public:
}; };
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual boost::shared_ptr<JacobianTrace<T> > traceExecution( virtual std::pair<T, TracePtr> traceExecution(const Values& values) const {
const Values& values) const {
boost::shared_ptr<Trace> trace = boost::make_shared<Trace>(); boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
trace->t = value(values);
trace->key = key_; trace->key = key_;
return trace; return std::make_pair(values.at<T>(key_), trace);
} }
}; };
@ -352,26 +354,25 @@ public:
} }
/// Trace structure for reverse AD /// Trace structure for reverse AD
struct Trace: public JacobianTrace<T> { struct Trace: public JacobianTrace {
boost::shared_ptr<JacobianTrace<A> > trace1; TracePtr trace;
JacobianTA dTdA; JacobianTA dTdA;
/// Start the reverse AD process /// Start the reverse AD process
virtual void reverseAD(JacobianMap& jacobians) const { virtual void reverseAD(JacobianMap& jacobians) const {
trace1->reverseAD(dTdA, jacobians); trace->reverseAD(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
virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const {
trace1->reverseAD(dFdT * dTdA, jacobians); trace->reverseAD(dFdT * dTdA, jacobians);
} }
}; };
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual boost::shared_ptr<JacobianTrace<T> > traceExecution( virtual std::pair<T, TracePtr> traceExecution(const Values& values) const {
const Values& values) const { A a;
boost::shared_ptr<Trace> trace = boost::make_shared<Trace>(); boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
trace->trace1 = this->expressionA_->traceExecution(values); boost::tie(a, trace->trace) = this->expressionA_->traceExecution(values);
trace->t = function_(trace->trace1->value(), trace->dTdA); return std::make_pair(function_(a, trace->dTdA),trace);
return trace;
} }
}; };
@ -438,9 +439,8 @@ public:
} }
/// Trace structure for reverse AD /// Trace structure for reverse AD
struct Trace: public JacobianTrace<T> { struct Trace: public JacobianTrace {
boost::shared_ptr<JacobianTrace<A1> > trace1; TracePtr trace1, trace2;
boost::shared_ptr<JacobianTrace<A2> > trace2;
JacobianTA1 dTdA1; JacobianTA1 dTdA1;
JacobianTA2 dTdA2; JacobianTA2 dTdA2;
/// Start the reverse AD process /// Start the reverse AD process
@ -456,14 +456,13 @@ public:
}; };
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual boost::shared_ptr<JacobianTrace<T> > traceExecution( virtual std::pair<T, TracePtr> traceExecution(const Values& values) const {
const Values& values) const { A1 a1;
A2 a2;
boost::shared_ptr<Trace> trace = boost::make_shared<Trace>(); boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
trace->trace1 = this->expressionA1_->traceExecution(values); boost::tie(a1, trace->trace1) = this->expressionA1_->traceExecution(values);
trace->trace2 = this->expressionA2_->traceExecution(values); boost::tie(a2, trace->trace2) = this->expressionA2_->traceExecution(values);
trace->t = function_(trace->trace1->value(), trace->trace2->value(), return std::make_pair(function_(a1, a2, trace->dTdA1, trace->dTdA2), trace);
trace->dTdA1, trace->dTdA2);
return trace;
} }
}; };
@ -543,10 +542,10 @@ public:
} }
/// Trace structure for reverse AD /// Trace structure for reverse AD
struct Trace: public JacobianTrace<T> { struct Trace: public JacobianTrace {
boost::shared_ptr<JacobianTrace<A1> > trace1; TracePtr trace1;
boost::shared_ptr<JacobianTrace<A2> > trace2; TracePtr trace2;
boost::shared_ptr<JacobianTrace<A3> > trace3; TracePtr trace3;
JacobianTA1 dTdA1; JacobianTA1 dTdA1;
JacobianTA2 dTdA2; JacobianTA2 dTdA2;
JacobianTA3 dTdA3; JacobianTA3 dTdA3;
@ -565,15 +564,16 @@ public:
}; };
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual boost::shared_ptr<JacobianTrace<T> > traceExecution( virtual std::pair<T, TracePtr> traceExecution(const Values& values) const {
const Values& values) const { A1 a1;
A2 a2;
A3 a3;
boost::shared_ptr<Trace> trace = boost::make_shared<Trace>(); boost::shared_ptr<Trace> trace = boost::make_shared<Trace>();
trace->trace1 = this->expressionA1_->traceExecution(values); boost::tie(a1, trace->trace1) = this->expressionA1_->traceExecution(values);
trace->trace2 = this->expressionA2_->traceExecution(values); boost::tie(a2, trace->trace2) = this->expressionA2_->traceExecution(values);
trace->trace3 = this->expressionA3_->traceExecution(values); boost::tie(a3, trace->trace3) = this->expressionA3_->traceExecution(values);
trace->t = function_(trace->trace1->value(), trace->trace2->value(), return std::make_pair(
trace->trace3->value(), trace->dTdA1, trace->dTdA2, trace->dTdA3); function_(a1, a2, a3, trace->dTdA1, trace->dTdA2, trace->dTdA3), trace);
return trace;
} }
}; };

6
gtsam_unstable/nonlinear/Expression.h
View File

@ -117,8 +117,10 @@ public:
Augmented<T> augmented(const Values& values) const { Augmented<T> augmented(const Values& values) const {
#define REVERSE_AD #define REVERSE_AD
#ifdef REVERSE_AD #ifdef REVERSE_AD
boost::shared_ptr<JacobianTrace<T> > trace(root_->traceExecution(values)); T value;
Augmented<T> augmented(trace->value()); TracePtr trace;
boost::tie(value,trace) = root_->traceExecution(values);
Augmented<T> augmented(value);
trace->reverseAD(augmented.jacobians()); trace->reverseAD(augmented.jacobians());
return augmented; return augmented;
#else #else

9
gtsam_unstable/nonlinear/tests/testExpression.cpp
View File

@ -32,8 +32,8 @@ using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
template<class CAL> template<class CAL>
Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional<Matrix25&> Dcal, Point2 uncalibrate(const CAL& K, const Point2& p,
boost::optional<Matrix2&> Dp) { boost::optional<Matrix25&> Dcal, boost::optional<Matrix2&> Dp) {
return K.uncalibrate(p, Dcal, Dp); return K.uncalibrate(p, Dcal, Dp);
} }
@ -76,7 +76,6 @@ TEST(Expression, leaf) {
// expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50)); // expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50));
// EXPECT(assert_equal(expected.at(67),a.jacobians().at(67))); // EXPECT(assert_equal(expected.at(67),a.jacobians().at(67)));
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Expression, test) { TEST(Expression, test) {
@ -149,8 +148,8 @@ TEST(Expression, compose3) {
/* ************************************************************************* */ /* ************************************************************************* */
// Test with ternary function // Test with ternary function
Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2, boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2,
boost::optional<Matrix&> H3) { boost::optional<Matrix3&> H3) {
// return dummy derivatives (not correct, but that's ok for testing here) // return dummy derivatives (not correct, but that's ok for testing here)
if (H1) if (H1)
*H1 = eye(3); *H1 = eye(3);