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

Merge branch 'feature/BAD_raw' into feature/BAD: the entire allocation tree is now allocated all at once, in Expression::reverse, on the stack. the traceExecution method then takes a raw pointer, and the "placement new" is used to grab that existing memory, avoiding all mallocs altogether. The only mallocs that still happen - that are responsible for almost all time within reverse - are the leaves inserting general matrices into a map to pass back to linearize.

release/4.3a0
dellaert 2014-10-11 13:50:49 +02:00
parent e09e24964a ecf6462a25
commit 252024cc9b
3 changed files with 174 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -21,9 +21,10 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <new> // for placement new
struct TestBinaryExpression;
namespace gtsam {
@ -43,10 +44,7 @@ typedef std::map<Key, Matrix> JacobianMap;
*/
template<int COLS>
struct CallRecord {
/// Make sure destructor is virtual
virtual ~CallRecord() {
}
virtual void print(const std::string& indent) const = 0;
virtual void startReverseAD(JacobianMap& jacobians) const = 0;
virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0;
typedef Eigen::Matrix<double, 2, COLS> Jacobian2T;
@ -76,11 +74,6 @@ public:
ExecutionTrace() :
type(Constant) {
}
/// Destructor cleans up pointer if Function
~ExecutionTrace() {
if (type == Function)
delete content.ptr;
}
/// Change pointer to a Leaf Record
void setLeaf(Key key) {
type = Leaf;
@ -91,6 +84,17 @@ public:
type = Function;
content.ptr = record;
}
/// Print
void print(const std::string& indent = "") const {
if (type == Constant)
std::cout << indent << "Constant" << std::endl;
else if (type == Leaf)
std::cout << indent << "Leaf, key = " << content.key << std::endl;
else if (type == Function) {
std::cout << indent << "Function" << std::endl;
content.ptr->print(indent + " ");
}
}
/// Return record pointer, quite unsafe, used only for testing
template<class Record>
boost::optional<Record*> record() {
@ -158,14 +162,6 @@ struct Select<2, A> {
}
};
//template <class Derived>
//struct TypedTrace {
// virtual void startReverseAD(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 {
//};
//-----------------------------------------------------------------------------
/**
* Value and Jacobians
@ -261,7 +257,7 @@ public:
}
/// debugging
void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) {
virtual void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) {
BOOST_FOREACH(const Pair& term, jacobians_)
std::cout << "(" << keyFormatter(term.first) << ", " << term.second.rows()
<< "x" << term.second.cols() << ") ";
@ -291,6 +287,7 @@ template<class T>
class ExpressionNode {
protected:
ExpressionNode() {
}
@ -309,9 +306,14 @@ public:
/// Return value and derivatives
virtual Augmented<T> forward(const Values& values) const = 0;
// Return size needed for memory buffer in traceExecution
virtual size_t traceSize() const {
return 0;
}
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values,
ExecutionTrace<T>& p) const = 0;
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
char* raw) const = 0;
};
//-----------------------------------------------------------------------------
@ -348,7 +350,8 @@ public:
}
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& p) const {
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
char* raw) const {
return constant_;
}
};
@ -388,8 +391,9 @@ public:
}
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& p) const {
p.setLeaf(key_);
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
char* raw) const {
trace.setLeaf(key_);
return values.at<T>(key_);
}
@ -443,7 +447,12 @@ public:
struct Record: public CallRecord<T::dimension> {
ExecutionTrace<A1> trace1;
JacobianTA dTdA1;
/// print to std::cout
virtual void print(const std::string& indent) const {
static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
std::cout << dTdA1.format(matlab) << std::endl;
trace1.print(indent);
}
/// Start the reverse AD process
virtual void startReverseAD(JacobianMap& jacobians) const {
Select<T::dimension, A1>::reverseAD(trace1, dTdA1, jacobians);
@ -460,12 +469,19 @@ public:
}
};
// Return size needed for memory buffer in traceExecution
virtual size_t traceSize() const {
return sizeof(Record) + expressionA1_->traceSize();
}
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& p) const {
Record* record = new Record();
p.setFunction(record);
A1 a = this->expressionA1_->traceExecution(values, record->trace1);
return function_(a, record->dTdA1);
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
char* raw) const {
Record* record = new (raw) Record();
trace.setFunction(record);
raw = (char*) (record + 1);
A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw);
return function_(a1, record->dTdA1);
}
};
@ -534,7 +550,14 @@ public:
ExecutionTrace<A2> trace2;
JacobianTA1 dTdA1;
JacobianTA2 dTdA2;
/// print to std::cout
virtual void print(const std::string& indent) const {
static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
std::cout << indent << dTdA1.format(matlab) << std::endl;
trace1.print(indent);
std::cout << indent << dTdA2.format(matlab) << std::endl;
trace2.print(indent);
}
/// Start the reverse AD process
virtual void startReverseAD(JacobianMap& jacobians) const {
Select<T::dimension, A1>::reverseAD(trace1, dTdA1, jacobians);
@ -554,12 +577,22 @@ public:
}
};
// Return size needed for memory buffer in traceExecution
virtual size_t traceSize() const {
return sizeof(Record) + expressionA1_->traceSize()
+ expressionA2_->traceSize();
}
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& p) const {
Record* record = new Record();
p.setFunction(record);
A1 a1 = this->expressionA1_->traceExecution(values, record->trace1);
A2 a2 = this->expressionA2_->traceExecution(values, record->trace2);
/// The raw buffer is [Record | A1 raw | A2 raw]
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
char* raw) const {
Record* record = new (raw) Record();
trace.setFunction(record);
raw = (char*) (record + 1);
A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw);
raw = raw + expressionA1_->traceSize();
A2 a2 = this->expressionA2_->traceExecution(values, record->trace2, raw);
return function_(a1, a2, record->dTdA1, record->dTdA2);
}
@ -643,7 +676,16 @@ public:
JacobianTA1 dTdA1;
JacobianTA2 dTdA2;
JacobianTA3 dTdA3;
/// print to std::cout
virtual void print(const std::string& indent) const {
static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
std::cout << dTdA1.format(matlab) << std::endl;
trace1.print(indent);
std::cout << dTdA2.format(matlab) << std::endl;
trace2.print(indent);
std::cout << dTdA3.format(matlab) << std::endl;
trace3.print(indent);
}
/// Start the reverse AD process
virtual void startReverseAD(JacobianMap& jacobians) const {
Select<T::dimension, A1>::reverseAD(trace1, dTdA1, jacobians);
@ -666,13 +708,23 @@ public:
}
};
// Return size needed for memory buffer in traceExecution
virtual size_t traceSize() const {
return sizeof(Record) + expressionA1_->traceSize()
+ expressionA2_->traceSize() + expressionA2_->traceSize();
}
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& p) const {
Record* record = new Record();
p.setFunction(record);
A1 a1 = this->expressionA1_->traceExecution(values, record->trace1);
A2 a2 = this->expressionA2_->traceExecution(values, record->trace2);
A3 a3 = this->expressionA3_->traceExecution(values, record->trace3);
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
char* raw) const {
Record* record = new (raw) Record();
trace.setFunction(record);
raw = (char*) (record + 1);
A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw);
raw = raw + expressionA1_->traceSize();
A2 a2 = this->expressionA2_->traceExecution(values, record->trace2, raw);
raw = raw + expressionA2_->traceSize();
A3 a3 = this->expressionA3_->traceExecution(values, record->trace3, raw);
return function_(a1, a2, a3, record->dTdA1, record->dTdA2, record->dTdA3);
}

34
gtsam_unstable/nonlinear/Expression.h
View File

@ -113,17 +113,39 @@ public:
return root_->value(values);
}
/// Return value and derivatives
Augmented<T> augmented(const Values& values) const {
#define REVERSE_AD
#ifdef REVERSE_AD
/// Return value and derivatives, forward AD version
Augmented<T> forward(const Values& values) const {
return root_->forward(values);
}
// Return size needed for memory buffer in traceExecution
size_t traceSize() const {
return root_->traceSize();
}
/// trace execution, very unsafe, for testing purposes only
T traceExecution(const Values& values, ExecutionTrace<T>& trace,
char* raw) const {
return root_->traceExecution(values, trace, raw);
}
/// Return value and derivatives, reverse AD version
Augmented<T> reverse(const Values& values) const {
size_t size = traceSize();
char raw[size];
ExecutionTrace<T> trace;
T value = root_->traceExecution(values, trace);
T value(traceExecution(values, trace, raw));
Augmented<T> augmented(value);
trace.startReverseAD(augmented.jacobians());
return augmented;
}
/// Return value and derivatives
Augmented<T> augmented(const Values& values) const {
#ifdef EXPRESSION_FORWARD_AD
return forward(values);
#else
return root_->forward(values);
return reverse(values);
#endif
}

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

@ -131,31 +131,37 @@ TEST(ExpressionFactor, binary) {
values.insert(1, Cal3_S2());
values.insert(2, Point2(0, 0));
// traceRaw will fill raw with [Trace<Point2> | Binary::Record]
EXPECT_LONGS_EQUAL(8, sizeof(double));
EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace<Point2>));
EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace<Cal3_S2>));
EXPECT_LONGS_EQUAL(2*5*8, sizeof(Binary::JacobianTA1));
EXPECT_LONGS_EQUAL(2*2*8, sizeof(Binary::JacobianTA2));
size_t expectedRecordSize = 16 + 2 * 16 + 80 + 32;
EXPECT_LONGS_EQUAL(expectedRecordSize, sizeof(Binary::Record));
// Check size
size_t size = tester.binary_.traceSize();
CHECK(size);
EXPECT_LONGS_EQUAL(expectedRecordSize, size);
// Use Variable Length Array, allocated on stack by gcc
// Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla
char raw[size];
ExecutionTrace<Point2> trace;
Point2 value = tester.binary_.traceExecution(values, trace, raw);
// trace.print();
// Expected Jacobians
Matrix25 expected25;
expected25 << 0, 0, 0, 1, 0, 0, 0, 0, 0, 1;
Matrix2 expected22;
expected22 << 1, 0, 0, 1;
// Do old trace
ExecutionTrace<Point2> trace;
tester.binary_.traceExecution(values, trace);
// Check matrices
boost::optional<Binary::Record*> p = trace.record<Binary::Record>();
CHECK(p);
EXPECT( assert_equal(expected25, (Matrix)(*p)->dTdA1, 1e-9));
EXPECT( assert_equal(expected22, (Matrix)(*p)->dTdA2, 1e-9));
// // Check raw memory trace
// double raw[10];
// tester.binary_.traceRaw(values, 0);
//
// // Check matrices
// boost::optional<Binary::Record*> p = trace.record<Binary::Record>();
// CHECK(p);
// EXPECT( assert_equal(expected25, (Matrix)(*p)->dTdA1, 1e-9));
// EXPECT( assert_equal(expected22, (Matrix)(*p)->dTdA2, 1e-9));
}
/* ************************************************************************* */
// Unary(Binary(Leaf,Leaf))
@ -173,11 +179,38 @@ TEST(ExpressionFactor, shallow) {
GaussianFactor::shared_ptr expected = old.linearize(values);
// Create leaves
Pose3_ x(1);
Point3_ p(2);
Pose3_ x_(1);
Point3_ p_(2);
// Concise version
ExpressionFactor<Point2> f2(model, measured, project(transform_to(x, p)));
// Construct expression, concise evrsion
Point2_ expression = project(transform_to(x_, p_));
// traceExecution of shallow tree
typedef UnaryExpression<Point2, Point3> Unary;
typedef BinaryExpression<Point3, Pose3, Point3> Binary;
EXPECT_LONGS_EQUAL(80, sizeof(Unary::Record));
EXPECT_LONGS_EQUAL(272, sizeof(Binary::Record));
size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary::Record);
LONGS_EQUAL(352, expectedTraceSize);
size_t size = expression.traceSize();
CHECK(size);
EXPECT_LONGS_EQUAL(expectedTraceSize, size);
char raw[size];
ExecutionTrace<Point2> trace;
Point2 value = expression.traceExecution(values, trace, raw);
// trace.print();
// Expected Jacobians
Matrix23 expected23;
expected23 << 1, 0, 0, 0, 1, 0;
// Check matrices
boost::optional<Unary::Record*> p = trace.record<Unary::Record>();
CHECK(p);
EXPECT( assert_equal(expected23, (Matrix)(*p)->dTdA1, 1e-9));
// Linearization
ExpressionFactor<Point2> f2(model, measured, expression);
EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
EXPECT_LONGS_EQUAL(2, f2.dim());
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);