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

Merge remote-tracking branch 'origin/feature/evenFasterExpressions' into feature/typedefPoint3

release/4.3a0
dellaert 2016-02-11 22:39:39 -08:00
parent 2060b09a2b 5c3dd7914b
commit c781ca46db
10 changed files with 99 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -110,8 +110,8 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg,
delta = gfg.optimize(*params.ordering, params.getEliminationFunction());
} else if (params.isSequential()) {
// Sequential QR or Cholesky (decided by params.getEliminationFunction())
delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(),
boost::none, params.orderingType)->optimize();
delta = gfg.eliminateSequential(*params.ordering,
params.getEliminationFunction(), boost::none, params.orderingType)->optimize();
} else if (params.isIterative()) {
// Conjugate Gradient -> needs params.iterativeParams

6
gtsam/nonlinear/NonlinearOptimizerParams.cpp
View File

@ -165,7 +165,8 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve
}
/* ************************************************************************* */
std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::OrderingType type) const{
std::string NonlinearOptimizerParams::orderingTypeTranslator(
Ordering::OrderingType type) const {
switch (type) {
case Ordering::METIS:
return "METIS";
@ -181,7 +182,8 @@ std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::OrderingT
}
/* ************************************************************************* */
Ordering::OrderingType NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const{
Ordering::OrderingType NonlinearOptimizerParams::orderingTypeTranslator(
const std::string& type) const {
if (type == "METIS")
return Ordering::METIS;
if (type == "COLAMD")

78
gtsam/nonlinear/internal/ExpressionNode.h
View File

@ -269,9 +269,13 @@ public:
// Inner Record Class
struct Record: public CallRecordImplementor<Record, traits<T>::dimension> {
A1 value1;
ExecutionTrace<A1> trace1;
typename Jacobian<T, A1>::type dTdA1;
ExecutionTrace<A1> trace1;
A1 value1;
/// Construct record by calling argument expression
Record(const Values& values, const ExpressionNode<A1>& expression1, ExecutionTraceStorage* ptr)
: value1(expression1.traceExecution(values, trace1, ptr + upAligned(sizeof(Record)))) {}
/// Print to std::cout
void print(const std::string& indent) const {
@ -305,20 +309,15 @@ public:
ExecutionTraceStorage* ptr) const {
assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
// Create the record at the start of the traceStorage and advance the pointer
Record* record = new (ptr) Record();
ptr += upAligned(sizeof(Record));
// Record the traces for all arguments
// After this, the traceStorage pointer is set to after what was written
// Create a Record in the memory pointed to by ptr
// Calling the construct will record the traces for all arguments
// Write an Expression<A> execution trace in record->trace
// Iff Constant or Leaf, this will not write to traceStorage, only to trace.
// Iff the expression is functional, write all Records in traceStorage buffer
// Return value of type T is recorded in record->value
record->value1 = expression1_->traceExecution(values, record->trace1, ptr);
Record* record = new (ptr) Record(values, *expression1_, ptr);
// We have written in the buffer, the next caller expects we advance the pointer
ptr += expression1_->traceSize();
// Our trace parameter is set to point to the Record
trace.setFunction(record);
// Finally, the function call fills in the Jacobian dTdA1
@ -384,14 +383,21 @@ public:
// Inner Record Class
struct Record: public CallRecordImplementor<Record, traits<T>::dimension> {
A1 value1;
ExecutionTrace<A1> trace1;
typename Jacobian<T, A1>::type dTdA1;
A2 value2;
ExecutionTrace<A2> trace2;
typename Jacobian<T, A2>::type dTdA2;
ExecutionTrace<A1> trace1;
ExecutionTrace<A2> trace2;
A1 value1;
A2 value2;
/// Construct record by calling argument expressions
Record(const Values& values, const ExpressionNode<A1>& expression1,
const ExpressionNode<A2>& expression2, ExecutionTraceStorage* ptr)
: value1(expression1.traceExecution(values, trace1, ptr += upAligned(sizeof(Record)))),
value2(expression2.traceExecution(values, trace2, ptr += expression1.traceSize())) {}
/// Print to std::cout
void print(const std::string& indent) const {
std::cout << indent << "BinaryExpression::Record {" << std::endl;
@ -418,12 +424,7 @@ public:
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* ptr) const {
assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
Record* record = new (ptr) Record();
ptr += upAligned(sizeof(Record));
record->value1 = expression1_->traceExecution(values, record->trace1, ptr);
ptr += expression1_->traceSize();
record->value2 = expression2_->traceExecution(values, record->trace2, ptr);
ptr += expression2_->traceSize();
Record* record = new (ptr) Record(values, *expression1_, *expression2_, ptr);
trace.setFunction(record);
return function_(record->value1, record->value2, record->dTdA1, record->dTdA2);
}
@ -492,18 +493,26 @@ public:
// Inner Record Class
struct Record: public CallRecordImplementor<Record, traits<T>::dimension> {
A1 value1;
ExecutionTrace<A1> trace1;
typename Jacobian<T, A1>::type dTdA1;
A2 value2;
ExecutionTrace<A2> trace2;
typename Jacobian<T, A2>::type dTdA2;
A3 value3;
ExecutionTrace<A3> trace3;
typename Jacobian<T, A3>::type dTdA3;
ExecutionTrace<A1> trace1;
ExecutionTrace<A2> trace2;
ExecutionTrace<A3> trace3;
A1 value1;
A2 value2;
A3 value3;
/// Construct record by calling 3 argument expressions
Record(const Values& values, const ExpressionNode<A1>& expression1,
const ExpressionNode<A2>& expression2,
const ExpressionNode<A3>& expression3, ExecutionTraceStorage* ptr)
: value1(expression1.traceExecution(values, trace1, ptr += upAligned(sizeof(Record)))),
value2(expression2.traceExecution(values, trace2, ptr += expression1.traceSize())),
value3(expression3.traceExecution(values, trace3, ptr += expression2.traceSize())) {}
/// Print to std::cout
void print(const std::string& indent) const {
std::cout << indent << "TernaryExpression::Record {" << std::endl;
@ -533,14 +542,7 @@ public:
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* ptr) const {
assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
Record* record = new (ptr) Record();
ptr += upAligned(sizeof(Record));
record->value1 = expression1_->traceExecution(values, record->trace1, ptr);
ptr += expression1_->traceSize();
record->value2 = expression2_->traceExecution(values, record->trace2, ptr);
ptr += expression2_->traceSize();
record->value3 = expression3_->traceExecution(values, record->trace3, ptr);
ptr += expression3_->traceSize();
Record* record = new (ptr) Record(values, *expression1_, *expression2_, *expression3_, ptr);
trace.setFunction(record);
return function_(record->value1, record->value2, record->value3,
record->dTdA1, record->dTdA2, record->dTdA3);

28
tests/testExpressionFactor.cpp
View File

@ -186,25 +186,8 @@ 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(Point2));
EXPECT_LONGS_EQUAL(40, sizeof(Cal3_S2));
EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace<Point2>));
EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace<Cal3_S2>));
EXPECT_LONGS_EQUAL(2*5*8, sizeof(internal::Jacobian<Point2,Cal3_S2>::type));
EXPECT_LONGS_EQUAL(2*2*8, sizeof(internal::Jacobian<Point2,Point2>::type));
size_t expectedRecordSize = sizeof(Cal3_S2)
+ sizeof(internal::ExecutionTrace<Cal3_S2>)
+ +sizeof(internal::Jacobian<Point2, Cal3_S2>::type) + sizeof(Point2)
+ sizeof(internal::ExecutionTrace<Point2>)
+ sizeof(internal::Jacobian<Point2, Point2>::type);
EXPECT_LONGS_EQUAL(expectedRecordSize + 8, sizeof(Binary::Record));
// Check size
size_t size = binary.traceSize();
CHECK(size);
EXPECT_LONGS_EQUAL(expectedRecordSize + 8, size);
// Use Variable Length Array, allocated on stack by gcc
// Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla
internal::ExecutionTraceStorage traceStorage[size];
@ -261,18 +244,7 @@ TEST(ExpressionFactor, Shallow) {
// traceExecution of shallow tree
typedef internal::UnaryExpression<Point2, Point3> Unary;
typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary::Record);
EXPECT_LONGS_EQUAL(96, sizeof(Unary::Record));
#ifdef GTSAM_USE_QUATERNIONS
EXPECT_LONGS_EQUAL(352, sizeof(Binary::Record));
LONGS_EQUAL(96+352, expectedTraceSize);
#else
EXPECT_LONGS_EQUAL(384, sizeof(Binary::Record));
LONGS_EQUAL(96+384, expectedTraceSize);
#endif
size_t size = expression.traceSize();
CHECK(size);
EXPECT_LONGS_EQUAL(expectedTraceSize, size);
internal::ExecutionTraceStorage traceStorage[size];
internal::ExecutionTrace<Point2> trace;
Point2 value = expression.traceExecution(values, trace, traceStorage);