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

2
examples/RangeISAMExample_plaza2.cpp
View File

@ -83,7 +83,7 @@ vector<RangeTriple> readTriples() {
while (is) {
double t, sender, range;
size_t receiver;
size_t receiver;
is >> t >> sender >> receiver >> range;
triples.push_back(RangeTriple(t, receiver, range));
}

2
gtsam/base/Matrix.cpp
View File

@ -189,7 +189,7 @@ void print(const Matrix& A, const string &s, ostream& stream) {
0, // flags
" ", // coeffSeparator
";\n", // rowSeparator
" \t", // rowPrefix
" \t", // rowPrefix
"", // rowSuffix
"[\n", // matPrefix
"\n ]" // matSuffix

18
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -28,8 +28,8 @@ namespace gtsam {
template<class FACTORGRAPH>
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
EliminateableFactorGraph<FACTORGRAPH>::eliminateSequential(
OptionalOrdering ordering, const Eliminate& function,
OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
OptionalOrdering ordering, const Eliminate& function,
OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
{
if(ordering && variableIndex) {
gttic(eliminateSequential);
@ -65,8 +65,8 @@ namespace gtsam {
template<class FACTORGRAPH>
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal(
OptionalOrdering ordering, const Eliminate& function,
OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
OptionalOrdering ordering, const Eliminate& function,
OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
{
if(ordering && variableIndex) {
gttic(eliminateMultifrontal);
@ -86,16 +86,16 @@ namespace gtsam {
// If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
// for no variable index first so that it's always computed if we need to call COLAMD because
// no Ordering is provided.
return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType);
return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType);
}
else /*if(!ordering)*/ {
// If no Ordering provided, compute one and call this function again. We are guaranteed to
// have a VariableIndex already here because we computed one if needed in the previous 'else'
// block.
if (orderingType == Ordering::METIS)
return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType);
else
return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType);
if (orderingType == Ordering::METIS)
return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType);
else
return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType);
}
}

18
gtsam/inference/EliminateableFactorGraph.h
View File

@ -94,8 +94,8 @@ namespace gtsam {
/// Typedef for an optional variable index as an argument to elimination functions
typedef boost::optional<const VariableIndex&> OptionalVariableIndex;
/// Typedef for an optional ordering type
typedef boost::optional<Ordering::OrderingType> OptionalOrderingType;
/// Typedef for an optional ordering type
typedef boost::optional<Ordering::OrderingType> OptionalOrderingType;
/** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not
* provided, the ordering provided by COLAMD will be used.
@ -104,10 +104,10 @@ namespace gtsam {
* \code
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateCholesky);
* \endcode
*
* <b> Example - METIS ordering for elimination
* \code
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(OrderingType::METIS);
*
* <b> Example - METIS ordering for elimination
* \code
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(OrderingType::METIS);
*
* <b> Example - Full QR elimination in specified order:
* \code
@ -125,7 +125,7 @@ namespace gtsam {
OptionalOrdering ordering = boost::none,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none,
OptionalOrderingType orderingType = boost::none) const;
OptionalOrderingType orderingType = boost::none) const;
/** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not
* provided, the ordering will be computed using either COLAMD or METIS, dependeing on
@ -151,8 +151,8 @@ namespace gtsam {
boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
OptionalOrdering ordering = boost::none,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none,
OptionalOrderingType orderingType = boost::none) const;
OptionalVariableIndex variableIndex = boost::none,
OptionalOrderingType orderingType = boost::none) const;
/** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net
* and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$,

6
gtsam/linear/JacobianFactor.cpp
View File

@ -349,11 +349,11 @@ void JacobianFactor::print(const string& s,
const KeyFormatter& formatter) const {
static const Eigen::IOFormat matlab(
Eigen::StreamPrecision, // precision
0, // flags
0, // flags
" ", // coeffSeparator
";\n", // rowSeparator
"\t", // rowPrefix
"", // rowSuffix
"\t", // rowPrefix
"", // rowSuffix
"[\n", // matPrefix
"\n ]" // matSuffix
);

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

54
gtsam/nonlinear/NonlinearOptimizerParams.cpp
View File

@ -110,14 +110,14 @@ void NonlinearOptimizerParams::print(const std::string& str) const {
switch (orderingType){
case Ordering::COLAMD:
std::cout << " ordering: COLAMD\n";
break;
std::cout << " ordering: COLAMD\n";
break;
case Ordering::METIS:
std::cout << " ordering: METIS\n";
break;
std::cout << " ordering: METIS\n";
break;
default:
std::cout << " ordering: custom\n";
break;
std::cout << " ordering: custom\n";
break;
}
std::cout.flush();
@ -165,29 +165,31 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve
}
/* ************************************************************************* */
std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::OrderingType type) const{
switch (type) {
case Ordering::METIS:
return "METIS";
case Ordering::COLAMD:
return "COLAMD";
default:
if (ordering)
return "CUSTOM";
else
throw std::invalid_argument(
"Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering");
}
std::string NonlinearOptimizerParams::orderingTypeTranslator(
Ordering::OrderingType type) const {
switch (type) {
case Ordering::METIS:
return "METIS";
case Ordering::COLAMD:
return "COLAMD";
default:
if (ordering)
return "CUSTOM";
else
throw std::invalid_argument(
"Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering");
}
}
/* ************************************************************************* */
Ordering::OrderingType NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const{
if (type == "METIS")
return Ordering::METIS;
if (type == "COLAMD")
return Ordering::COLAMD;
throw std::invalid_argument(
"Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering");
Ordering::OrderingType NonlinearOptimizerParams::orderingTypeTranslator(
const std::string& type) const {
if (type == "METIS")
return Ordering::METIS;
if (type == "COLAMD")
return Ordering::COLAMD;
throw std::invalid_argument(
"Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering");
}

6
gtsam/nonlinear/NonlinearOptimizerParams.h
View File

@ -154,16 +154,16 @@ public:
void setOrdering(const Ordering& ordering) {
this->ordering = ordering;
this->orderingType = Ordering::CUSTOM;
this->orderingType = Ordering::CUSTOM;
}
std::string getOrderingType() const {
return orderingTypeTranslator(orderingType);
return orderingTypeTranslator(orderingType);
}
// Note that if you want to use a custom ordering, you must set the ordering directly, this will switch to custom type
void setOrderingType(const std::string& ordering){
orderingType = orderingTypeTranslator(ordering);
orderingType = orderingTypeTranslator(ordering);
}
private:

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

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -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;
@ -531,19 +540,12 @@ public:
/// Construct an execution trace for reverse AD, see UnaryExpression for explanation
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* ptr) const {
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);
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);