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

reverted extra spaces which were added in last commit

release/4.3a0
Renaud Dube 2014-11-07 12:11:08 +01:00
parent bd3f9db7df
commit 90ec6b1452
1 changed files with 52 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -58,9 +58,9 @@ class Expression;
class JacobianMap { class JacobianMap {
const FastVector<Key>& keys_; const FastVector<Key>& keys_;
VerticalBlockMatrix& Ab_; VerticalBlockMatrix& Ab_;
public: public:
JacobianMap(const FastVector<Key>& keys, VerticalBlockMatrix& Ab) : JacobianMap(const FastVector<Key>& keys, VerticalBlockMatrix& Ab) :
keys_(keys), Ab_(Ab) { keys_(keys), Ab_(Ab) {
} }
/// Access via key /// Access via key
VerticalBlockMatrix::Block operator()(Key key) { VerticalBlockMatrix::Block operator()(Key key) {
@ -89,7 +89,7 @@ struct CallRecord {
} }
typedef Eigen::Matrix<double, 2, COLS> Jacobian2T; typedef Eigen::Matrix<double, 2, COLS> Jacobian2T;
virtual void reverseAD2(const Jacobian2T& dFdT, virtual void reverseAD2(const Jacobian2T& dFdT,
JacobianMap& jacobians) const { JacobianMap& jacobians) const {
} }
}; };
@ -97,13 +97,8 @@ struct CallRecord {
/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians /// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians
template<int ROWS, int COLS> template<int ROWS, int COLS>
void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA, void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA,
JacobianMap& jacobians, Key key) { JacobianMap& jacobians, Key key) {
// if (ROWS == -1 && COLS == -1 ) { jacobians(key).block < ROWS, COLS > (0, 0) += dTdA; // block makes HUGE difference
// jacobians(key) += dTdA;
// } else {
jacobians(key).block < ROWS, COLS > (0, 0) += dTdA; // block makes HUGE difference
// }
} }
/// Handle Leaf Case for Dynamic Matrix type (slower) /// Handle Leaf Case for Dynamic Matrix type (slower)
template<> template<>
@ -145,10 +140,10 @@ class ExecutionTrace {
Key key; Key key;
CallRecord<Dim>* ptr; CallRecord<Dim>* ptr;
} content; } content;
public: public:
/// Pointer always starts out as a Constant /// Pointer always starts out as a Constant
ExecutionTrace() : ExecutionTrace() :
kind(Constant) { kind(Constant) {
} }
/// Change pointer to a Leaf Record /// Change pointer to a Leaf Record
void setLeaf(Key key) { void setLeaf(Key key) {
@ -221,7 +216,7 @@ template<size_t ROWS, class A>
struct Select { struct Select {
typedef Eigen::Matrix<double, ROWS, traits::dimension<A>::value> Jacobian; typedef Eigen::Matrix<double, ROWS, traits::dimension<A>::value> Jacobian;
static void reverseAD(const ExecutionTrace<A>& trace, const Jacobian& dTdA, static void reverseAD(const ExecutionTrace<A>& trace, const Jacobian& dTdA,
JacobianMap& jacobians) { JacobianMap& jacobians) {
trace.reverseAD(dTdA, jacobians); trace.reverseAD(dTdA, jacobians);
} }
}; };
@ -231,7 +226,7 @@ template<class A>
struct Select<2, A> { struct Select<2, A> {
typedef Eigen::Matrix<double, 2, traits::dimension<A>::value> Jacobian; typedef Eigen::Matrix<double, 2, traits::dimension<A>::value> Jacobian;
static void reverseAD(const ExecutionTrace<A>& trace, const Jacobian& dTdA, static void reverseAD(const ExecutionTrace<A>& trace, const Jacobian& dTdA,
JacobianMap& jacobians) { JacobianMap& jacobians) {
trace.reverseAD2(dTdA, jacobians); trace.reverseAD2(dTdA, jacobians);
} }
}; };
@ -247,16 +242,16 @@ struct Select<2, A> {
template<class T> template<class T>
class ExpressionNode { class ExpressionNode {
protected: protected:
size_t traceSize_; size_t traceSize_;
/// Constructor, traceSize is size of the execution trace of expression rooted here /// Constructor, traceSize is size of the execution trace of expression rooted here
ExpressionNode(size_t traceSize = 0) : ExpressionNode(size_t traceSize = 0) :
traceSize_(traceSize) { traceSize_(traceSize) {
} }
public: public:
/// Destructor /// Destructor
virtual ~ExpressionNode() { virtual ~ExpressionNode() {
@ -282,7 +277,7 @@ class ExpressionNode {
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace, virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
char* raw) const = 0; char* raw) const = 0;
}; };
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
@ -295,12 +290,12 @@ class ConstantExpression: public ExpressionNode<T> {
/// Constructor with a value, yielding a constant /// Constructor with a value, yielding a constant
ConstantExpression(const T& value) : ConstantExpression(const T& value) :
constant_(value) { constant_(value) {
} }
friend class Expression<T> ; friend class Expression<T> ;
public: public:
/// Return value /// Return value
virtual T value(const Values& values) const { virtual T value(const Values& values) const {
@ -309,7 +304,7 @@ class ConstantExpression: public ExpressionNode<T> {
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace, virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
char* raw) const { char* raw) const {
return constant_; return constant_;
} }
}; };
@ -325,13 +320,13 @@ class LeafExpression: public ExpressionNode<T> {
/// Constructor with a single key /// Constructor with a single key
LeafExpression(Key key) : LeafExpression(Key key) :
key_(key) { key_(key) {
} }
// todo: do we need a virtual destructor here too? // todo: do we need a virtual destructor here too?
friend class Expression<value_type> ; friend class Expression<value_type> ;
public: public:
/// Return keys that play in this expression /// Return keys that play in this expression
virtual std::set<Key> keys() const { virtual std::set<Key> keys() const {
@ -353,7 +348,7 @@ class LeafExpression: public ExpressionNode<T> {
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual const value_type& traceExecution(const Values& values, ExecutionTrace<value_type>& trace, virtual const value_type& traceExecution(const Values& values, ExecutionTrace<value_type>& trace,
char* raw) const { char* raw) const {
trace.setLeaf(key_); trace.setLeaf(key_);
return dynamic_cast<const value_type&>(values.at(key_)); return dynamic_cast<const value_type&>(values.at(key_));
} }
@ -371,13 +366,13 @@ class LeafExpression<T, DefaultChart<T> >: public ExpressionNode<T> {
/// Constructor with a single key /// Constructor with a single key
LeafExpression(Key key) : LeafExpression(Key key) :
key_(key) { key_(key) {
} }
// todo: do we need a virtual destructor here too? // todo: do we need a virtual destructor here too?
friend class Expression<T> ; friend class Expression<T> ;
public: public:
/// Return keys that play in this expression /// Return keys that play in this expression
virtual std::set<Key> keys() const { virtual std::set<Key> keys() const {
@ -398,7 +393,7 @@ class LeafExpression<T, DefaultChart<T> >: public ExpressionNode<T> {
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace, virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
char* raw) const { char* raw) const {
trace.setLeaf(key_); trace.setLeaf(key_);
return values.at<T>(key_); return values.at<T>(key_);
} }
@ -528,7 +523,7 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
virtual void startReverseAD(JacobianMap& jacobians) const { virtual void startReverseAD(JacobianMap& jacobians) const {
Base::Record::startReverseAD(jacobians); Base::Record::startReverseAD(jacobians);
Select<traits::dimension<T>::value, A>::reverseAD(This::trace, This::dTdA, Select<traits::dimension<T>::value, A>::reverseAD(This::trace, This::dTdA,
jacobians); 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
@ -540,7 +535,7 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
/// Version specialized to 2-dimensional output /// Version specialized to 2-dimensional output
typedef Eigen::Matrix<double, 2, traits::dimension<T>::value> Jacobian2T; typedef Eigen::Matrix<double, 2, traits::dimension<T>::value> Jacobian2T;
virtual void reverseAD2(const Jacobian2T& dFdT, virtual void reverseAD2(const Jacobian2T& dFdT,
JacobianMap& jacobians) const { JacobianMap& jacobians) const {
Base::Record::reverseAD2(dFdT, jacobians); Base::Record::reverseAD2(dFdT, jacobians);
This::trace.reverseAD2(dFdT * This::dTdA, jacobians); This::trace.reverseAD2(dFdT * This::dTdA, jacobians);
} }
@ -554,7 +549,7 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
// Iff the expression is functional, write all Records in raw buffer // Iff the expression is functional, write all Records in raw buffer
// Return value of type T is recorded in record->value // Return value of type T is recorded in record->value
record->Record::This::value = This::expression->traceExecution(values, record->Record::This::value = This::expression->traceExecution(values,
record->Record::This::trace, raw); record->Record::This::trace, raw);
// raw is never modified by traceExecution, but if traceExecution has // raw is never modified by traceExecution, but if traceExecution has
// written in the buffer, the next caller expects we advance the pointer // written in the buffer, the next caller expects we advance the pointer
raw += This::expression->traceSize(); raw += This::expression->traceSize();
@ -628,26 +623,26 @@ struct FunctionalNode {
template<class T, class A1> template<class T, class A1>
class UnaryExpression: public FunctionalNode<T, boost::mpl::vector<A1> >::type { class UnaryExpression: public FunctionalNode<T, boost::mpl::vector<A1> >::type {
public: public:
typedef boost::function<T(const A1&, typename OptionalJacobian<T, A1>::type)> Function; typedef boost::function<T(const A1&, typename OptionalJacobian<T, A1>::type)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1> >::type Base; typedef typename FunctionalNode<T, boost::mpl::vector<A1> >::type Base;
typedef typename Base::Record Record; typedef typename Base::Record Record;
private: private:
Function function_; Function function_;
/// Constructor with a unary function f, and input argument e /// Constructor with a unary function f, and input argument e
UnaryExpression(Function f, const Expression<A1>& e1) : UnaryExpression(Function f, const Expression<A1>& e1) :
function_(f) { function_(f) {
this->template reset<A1, 1>(e1.root()); this->template reset<A1, 1>(e1.root());
ExpressionNode<T>::traceSize_ = sizeof(Record) + e1.traceSize(); ExpressionNode<T>::traceSize_ = sizeof(Record) + e1.traceSize();
} }
friend class Expression<T> ; friend class Expression<T> ;
public: public:
/// Return value /// Return value
virtual T value(const Values& values) const { virtual T value(const Values& values) const {
@ -656,13 +651,13 @@ class UnaryExpression: public FunctionalNode<T, boost::mpl::vector<A1> >::type {
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace, virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
char* raw) const { char* raw) const {
Record* record = Base::trace(values, raw); Record* record = Base::trace(values, raw);
trace.setFunction(record); trace.setFunction(record);
return function_(record->template value<A1, 1>(), return function_(record->template value<A1, 1>(),
record->template jacobian<A1, 1>()); record->template jacobian<A1, 1>());
} }
}; };
@ -672,22 +667,22 @@ class UnaryExpression: public FunctionalNode<T, boost::mpl::vector<A1> >::type {
template<class T, class A1, class A2> template<class T, class A1, class A2>
class BinaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2> >::type { class BinaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2> >::type {
public: public:
typedef boost::function< typedef boost::function<
T(const A1&, const A2&, typename OptionalJacobian<T, A1>::type, T(const A1&, const A2&, typename OptionalJacobian<T, A1>::type,
typename OptionalJacobian<T, A2>::type)> Function; typename OptionalJacobian<T, A2>::type)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2> >::type Base; typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2> >::type Base;
typedef typename Base::Record Record; typedef typename Base::Record Record;
private: private:
Function function_; Function function_;
/// Constructor with a ternary function f, and three input arguments /// Constructor with a ternary function f, and three input arguments
BinaryExpression(Function f, const Expression<A1>& e1, BinaryExpression(Function f, const Expression<A1>& e1,
const Expression<A2>& e2) : const Expression<A2>& e2) :
function_(f) { function_(f) {
this->template reset<A1, 1>(e1.root()); this->template reset<A1, 1>(e1.root());
this->template reset<A2, 2>(e2.root()); this->template reset<A2, 2>(e2.root());
ExpressionNode<T>::traceSize_ = // ExpressionNode<T>::traceSize_ = //
@ -697,26 +692,26 @@ class BinaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2> >::t
friend class Expression<T> ; friend class Expression<T> ;
friend class ::ExpressionFactorBinaryTest; friend class ::ExpressionFactorBinaryTest;
public: public:
/// Return value /// Return value
virtual T value(const Values& values) const { virtual T value(const Values& values) const {
using boost::none; using boost::none;
return function_(this->template expression<A1, 1>()->value(values), return function_(this->template expression<A1, 1>()->value(values),
this->template expression<A2, 2>()->value(values), this->template expression<A2, 2>()->value(values),
none, none); none, none);
} }
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace, virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
char* raw) const { char* raw) const {
Record* record = Base::trace(values, raw); Record* record = Base::trace(values, raw);
trace.setFunction(record); trace.setFunction(record);
return function_(record->template value<A1, 1>(), return function_(record->template value<A1, 1>(),
record->template value<A2,2>(), record->template jacobian<A1, 1>(), record->template value<A2,2>(), record->template jacobian<A1, 1>(),
record->template jacobian<A2, 2>()); record->template jacobian<A2, 2>());
} }
}; };
@ -726,22 +721,22 @@ class BinaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2> >::t
template<class T, class A1, class A2, class A3> template<class T, class A1, class A2, class A3>
class TernaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type { class TernaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type {
public: public:
typedef boost::function< typedef boost::function<
T(const A1&, const A2&, const A3&, typename OptionalJacobian<T, A1>::type, T(const A1&, const A2&, const A3&, typename OptionalJacobian<T, A1>::type,
typename OptionalJacobian<T, A2>::type, typename OptionalJacobian<T, A3>::type)> Function; typename OptionalJacobian<T, A2>::type, typename OptionalJacobian<T, A3>::type)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type Base; typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type Base;
typedef typename Base::Record Record; typedef typename Base::Record Record;
private: private:
Function function_; Function function_;
/// Constructor with a ternary function f, and three input arguments /// Constructor with a ternary function f, and three input arguments
TernaryExpression(Function f, const Expression<A1>& e1, TernaryExpression(Function f, const Expression<A1>& e1,
const Expression<A2>& e2, const Expression<A3>& e3) : const Expression<A2>& e2, const Expression<A3>& e3) :
function_(f) { function_(f) {
this->template reset<A1, 1>(e1.root()); this->template reset<A1, 1>(e1.root());
this->template reset<A2, 2>(e2.root()); this->template reset<A2, 2>(e2.root());
this->template reset<A3, 3>(e3.root()); this->template reset<A3, 3>(e3.root());
@ -751,20 +746,20 @@ class TernaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2, A3>
friend class Expression<T> ; friend class Expression<T> ;
public: public:
/// Return value /// Return value
virtual T value(const Values& values) const { virtual T value(const Values& values) const {
using boost::none; using boost::none;
return function_(this->template expression<A1, 1>()->value(values), return function_(this->template expression<A1, 1>()->value(values),
this->template expression<A2, 2>()->value(values), this->template expression<A2, 2>()->value(values),
this->template expression<A3, 3>()->value(values), this->template expression<A3, 3>()->value(values),
none, none, none); none, none, none);
} }
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace, virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
char* raw) const { char* raw) const {
Record* record = Base::trace(values, raw); Record* record = Base::trace(values, raw);
trace.setFunction(record); trace.setFunction(record);
@ -778,4 +773,3 @@ class TernaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2, A3>
}; };
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
} }