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

Merged in feature/SimplerExpressions (pull request #135) 

Expressions without MPL
release/4.3a0
Frank Dellaert 2015-05-13 00:36:49 -07:00
parent eb8d1a91a6 a585e8ac09
commit e456923a74
28 changed files with 1268 additions and 1336 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
.cproject
View File

@ -532,14 +532,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimilarity3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testSimilarity3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLieConfig.run" path="nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -755,6 +747,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimilarity3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testSimilarity3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthCamera3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1301,7 +1301,6 @@
</target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1341,7 +1340,6 @@
</target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1349,7 +1347,6 @@
</target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1453,6 +1450,7 @@
</target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1763,6 +1761,7 @@
</target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1770,6 +1769,7 @@
</target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1777,6 +1777,7 @@
</target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1784,6 +1785,7 @@
</target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1975,7 +1977,6 @@
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2037,22 +2038,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testExpressionMeta.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testExpressionMeta.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCustomChartExpression.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testCustomChartExpression.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="schedulingExample.run" path="build/gtsam_unstable/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -2127,6 +2112,7 @@
</target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2134,6 +2120,7 @@
</target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2181,6 +2168,7 @@
</target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2188,6 +2176,7 @@
</target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2195,6 +2184,7 @@
</target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2210,6 +2200,7 @@
</target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2783,6 +2774,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testExecutionTrace.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testExecutionTrace.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testIMUSystem.run" path="build/gtsam_unstable/dynamics/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -3329,6 +3328,7 @@
</target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -3336,6 +3336,7 @@
</target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -3343,6 +3344,7 @@
</target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>

50
examples/SolverComparer.cpp
View File

@ -31,28 +31,29 @@
*
*/
#include <gtsam/base/timing.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <fstream>
#include <iostream>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/export.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/program_options.hpp>
#include <boost/range/algorithm/set_algorithm.hpp>
#include <boost/random.hpp>
#include <boost/serialization/export.hpp>
#include <fstream>
#include <iostream>
#ifdef GTSAM_USE_TBB
#include <tbb/tbb.h>
@ -72,23 +73,6 @@ typedef NoiseModelFactor1<Pose> NM1;
typedef NoiseModelFactor2<Pose,Pose> NM2;
typedef BearingRangeFactor<Pose,Point2> BR;
//GTSAM_VALUE_EXPORT(Value);
//GTSAM_VALUE_EXPORT(Pose);
//GTSAM_VALUE_EXPORT(Rot2);
//GTSAM_VALUE_EXPORT(Point2);
//GTSAM_VALUE_EXPORT(NonlinearFactor);
//GTSAM_VALUE_EXPORT(NoiseModelFactor);
//GTSAM_VALUE_EXPORT(NM1);
//GTSAM_VALUE_EXPORT(NM2);
//GTSAM_VALUE_EXPORT(BetweenFactor<Pose>);
//GTSAM_VALUE_EXPORT(PriorFactor<Pose>);
//GTSAM_VALUE_EXPORT(BR);
//GTSAM_VALUE_EXPORT(noiseModel::Base);
//GTSAM_VALUE_EXPORT(noiseModel::Isotropic);
//GTSAM_VALUE_EXPORT(noiseModel::Gaussian);
//GTSAM_VALUE_EXPORT(noiseModel::Diagonal);
//GTSAM_VALUE_EXPORT(noiseModel::Unit);
double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
// Compute degrees of freedom (observations - variables)
// In ocaml, +1 was added to the observations to account for the prior, but
@ -269,12 +253,12 @@ void runIncremental()
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
{
Key key1 = measurement->key1(), key2 = measurement->key2();
if((key1 >= firstStep && key1 < key2) || (key2 >= firstStep && key2 < key1)) {
if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
// We found an odometry starting at firstStep
firstPose = std::min(key1, key2);
break;
}
if((key2 >= firstStep && key1 < key2) || (key1 >= firstStep && key2 < key1)) {
if(((int)key2 >= firstStep && key1 < key2) || ((int)key1 >= firstStep && key2 < key1)) {
// We found an odometry joining firstStep with a previous pose
havePreviousPose = true;
firstPose = std::max(key1, key2);
@ -303,7 +287,9 @@ void runIncremental()
cout << "Playing forward time steps..." << endl;
for(size_t step = firstPose; nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || step <= lastStep); ++step)
for (size_t step = firstPose;
nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || (int)step <= lastStep);
++step)
{
Values newVariables;
NonlinearFactorGraph newFactors;

2
gtsam/3rdparty/ceres/eigen.h vendored
View File

@ -31,7 +31,7 @@
#ifndef CERES_INTERNAL_EIGEN_H_
#define CERES_INTERNAL_EIGEN_H_
#include <gtsam/3rdparty/gtsam_eigen_includes.h>
#include <Eigen/Core>
namespace ceres {

2
gtsam/3rdparty/ceres/fixed_array.h vendored
View File

@ -33,7 +33,7 @@
#define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_
#include <cstddef>
#include <gtsam/3rdparty/gtsam_eigen_includes.h>
#include <Eigen/Core>
#include <gtsam/3rdparty/ceres/macros.h>
#include <gtsam/3rdparty/ceres/manual_constructor.h>

2
gtsam/3rdparty/ceres/jet.h vendored
View File

@ -162,7 +162,7 @@
#include <limits>
#include <string>
#include <gtsam/3rdparty/gtsam_eigen_includes.h>
#include <Eigen/Core>
#include <gtsam/3rdparty/ceres/fpclassify.h>
namespace ceres {

15
gtsam/base/OptionalJacobian.h
View File

@ -168,5 +168,20 @@ public:
Jacobian* operator->(){ return pointer_; }
};
// forward declare
template <typename T> struct traits;
/**
* @brief: meta-function to generate JacobianTA optional reference
* Used mainly by Expressions
* @param T return type
* @param A argument type
*/
template<class T, class A>
struct MakeOptionalJacobian {
typedef OptionalJacobian<traits<T>::dimension,
traits<A>::dimension> type;
};
} // namespace gtsam

10
gtsam/base/tests/testTreeTraversal.cpp
View File

@ -45,11 +45,11 @@ struct TestForest {
};
TestForest makeTestForest() {
// 0 1
// / \
// 2 3
// |
// 4
// 0 1
// / |
// 2 3
// |
// 4
TestForest forest;
forest.roots_.push_back(boost::make_shared<TestNode>(0));
forest.roots_.push_back(boost::make_shared<TestNode>(1));

11
gtsam/discrete/DecisionTree-inl.h
View File

@ -467,7 +467,7 @@ namespace gtsam {
// find highest label among branches
boost::optional<L> highestLabel;
boost::optional<size_t> nrChoices;
size_t nrChoices = 0;
for (Iterator it = begin; it != end; it++) {
if (it->root_->isLeaf())
continue;
@ -475,22 +475,21 @@ namespace gtsam {
boost::dynamic_pointer_cast<const Choice>(it->root_);
if (!highestLabel || c->label() > *highestLabel) {
highestLabel.reset(c->label());
nrChoices.reset(c->nrChoices());
nrChoices = c->nrChoices();
}
}
// if label is already in correct order, just put together a choice on label
if (!highestLabel || !nrChoices || label > *highestLabel) {
if (!nrChoices || !highestLabel || label > *highestLabel) {
boost::shared_ptr<Choice> choiceOnLabel(new Choice(label, end - begin));
for (Iterator it = begin; it != end; it++)
choiceOnLabel->push_back(it->root_);
return Choice::Unique(choiceOnLabel);
} else {
// Set up a new choice on the highest label
boost::shared_ptr<Choice> choiceOnHighestLabel(
new Choice(*highestLabel, *nrChoices));
boost::shared_ptr<Choice> choiceOnHighestLabel(new Choice(*highestLabel, nrChoices));
// now, for all possible values of highestLabel
for (size_t index = 0; index < *nrChoices; index++) {
for (size_t index = 0; index < nrChoices; index++) {
// make a new set of functions for composing by iterating over the given
// functions, and selecting the appropriate branch.
std::vector<DecisionTree> functions;

2
gtsam/linear/NoiseModel.cpp
View File

@ -293,7 +293,7 @@ namespace internal {
// switch precisions and invsigmas to finite value
// TODO: why?? And, why not just ask s==0.0 below ?
static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) {
for (size_t i = 0; i < sigmas.size(); ++i)
for (Vector::Index i = 0; i < sigmas.size(); ++i)
if (!std::isfinite(1. / sigmas[i])) {
precisions[i] = 0.0;
invsigmas[i] = 0.0;

959
gtsam/nonlinear/Expression-inl.h
View File

@ -19,773 +19,244 @@
#pragma once
#include <gtsam/nonlinear/CallRecord.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/internal/ExpressionNode.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/bind.hpp>
#include <boost/type_traits/aligned_storage.hpp>
// template meta-programming headers
#include <boost/mpl/fold.hpp>
namespace MPL = boost::mpl::placeholders;
#include <typeinfo> // operator typeid
#include <map>
class ExpressionFactorBinaryTest;
// Forward declare for testing
#include <boost/range/adaptor/map.hpp>
#include <boost/range/algorithm.hpp>
namespace gtsam {
const unsigned TraceAlignment = 16;
template<typename T>
T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) {
// right now only word sized types are supported.
// Easy to extend if needed,
// by somehow inferring the unsigned integer of same size
BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t));
size_t & uiValue = reinterpret_cast<size_t &>(value);
size_t misAlignment = uiValue % requiredAlignment;
if (misAlignment) {
uiValue += requiredAlignment - misAlignment;
}
return value;
}
template<typename T>
T upAligned(T value, unsigned requiredAlignment = TraceAlignment) {
return upAlign(value, requiredAlignment);
void Expression<T>::print(const std::string& s) const {
std::cout << s << *root_ << std::endl;
}
template<typename T>
class Expression;
/**
* Expressions are designed to write their derivatives into an already allocated
* Jacobian of the correct size, of type VerticalBlockMatrix.
* The JacobianMap provides a mapping from keys to the underlying blocks.
*/
class JacobianMap {
const FastVector<Key>& keys_;
VerticalBlockMatrix& Ab_;
public:
JacobianMap(const FastVector<Key>& keys, VerticalBlockMatrix& Ab) :
keys_(keys), Ab_(Ab) {
}
/// Access via key
VerticalBlockMatrix::Block operator()(Key key) {
FastVector<Key>::const_iterator it = std::find(keys_.begin(), keys_.end(),
key);
DenseIndex block = it - keys_.begin();
return Ab_(block);
}
};
//-----------------------------------------------------------------------------
namespace internal {
template<bool UseBlock, typename Derived>
struct UseBlockIf {
static void addToJacobian(const Eigen::MatrixBase<Derived>& dTdA,
JacobianMap& jacobians, Key key) {
// block makes HUGE difference
jacobians(key).block<Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>(
0, 0) += dTdA;
}
;
};
/// Handle Leaf Case for Dynamic Matrix type (slower)
template<typename Derived>
struct UseBlockIf<false, Derived> {
static void addToJacobian(const Eigen::MatrixBase<Derived>& dTdA,
JacobianMap& jacobians, Key key) {
jacobians(key) += dTdA;
}
};
Expression<T>::Expression(const T& value) :
root_(new internal::ConstantExpression<T>(value)) {
}
/// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians
template<typename Derived>
void handleLeafCase(const Eigen::MatrixBase<Derived>& dTdA,
JacobianMap& jacobians, Key key) {
internal::UseBlockIf<
Derived::RowsAtCompileTime != Eigen::Dynamic
&& Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian(
dTdA, jacobians, key);
template<typename T>
Expression<T>::Expression(const Key& key) :
root_(new internal::LeafExpression<T>(key)) {
}
//-----------------------------------------------------------------------------
/**
* The ExecutionTrace class records a tree-structured expression's execution.
*
* The class looks a bit complicated but it is so for performance.
* It is a tagged union that obviates the need to create
* a ExecutionTrace subclass for Constants and Leaf Expressions. Instead
* the key for the leaf is stored in the space normally used to store a
* CallRecord*. Nothing is stored for a Constant.
*
* A full execution trace of a Binary(Unary(Binary(Leaf,Constant)),Leaf) would be:
* Trace(Function) ->
* BinaryRecord with two traces in it
* trace1(Function) ->
* UnaryRecord with one trace in it
* trace1(Function) ->
* BinaryRecord with two traces in it
* trace1(Leaf)
* trace2(Constant)
* trace2(Leaf)
* Hence, there are three Record structs, written to memory by traceExecution
*/
template<class T>
class ExecutionTrace {
template<typename T>
Expression<T>::Expression(const Symbol& symbol) :
root_(new internal::LeafExpression<T>(symbol)) {
}
template<typename T>
Expression<T>::Expression(unsigned char c, size_t j) :
root_(new internal::LeafExpression<T>(Symbol(c, j))) {
}
/// Construct a unary function expression
template<typename T>
template<typename A>
Expression<T>::Expression(typename UnaryFunction<A>::type function,
const Expression<A>& expression) :
root_(new internal::UnaryExpression<T, A>(function, expression)) {
}
/// Construct a binary function expression
template<typename T>
template<typename A1, typename A2>
Expression<T>::Expression(typename BinaryFunction<A1, A2>::type function,
const Expression<A1>& expression1, const Expression<A2>& expression2) :
root_(
new internal::BinaryExpression<T, A1, A2>(function, expression1,
expression2)) {
}
/// Construct a ternary function expression
template<typename T>
template<typename A1, typename A2, typename A3>
Expression<T>::Expression(typename TernaryFunction<A1, A2, A3>::type function,
const Expression<A1>& expression1, const Expression<A2>& expression2,
const Expression<A3>& expression3) :
root_(
new internal::TernaryExpression<T, A1, A2, A3>(function, expression1,
expression2, expression3)) {
}
/// Construct a nullary method expression
template<typename T>
template<typename A>
Expression<T>::Expression(const Expression<A>& expression,
T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const) :
root_(
new internal::UnaryExpression<T, A>(boost::bind(method, _1, _2),
expression)) {
}
/// Construct a unary method expression
template<typename T>
template<typename A1, typename A2>
Expression<T>::Expression(const Expression<A1>& expression1,
T (A1::*method)(const A2&, typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type) const,
const Expression<A2>& expression2) :
root_(
new internal::BinaryExpression<T, A1, A2>(
boost::bind(method, _1, _2, _3, _4), expression1, expression2)) {
}
/// Construct a binary method expression
template<typename T>
template<typename A1, typename A2, typename A3>
Expression<T>::Expression(const Expression<A1>& expression1,
T (A1::*method)(const A2&, const A3&,
typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type,
typename MakeOptionalJacobian<T, A3>::type) const,
const Expression<A2>& expression2, const Expression<A3>& expression3) :
root_(
new internal::TernaryExpression<T, A1, A2, A3>(
boost::bind(method, _1, _2, _3, _4, _5, _6), expression1,
expression2, expression3)) {
}
template<typename T>
std::set<Key> Expression<T>::keys() const {
return root_->keys();
}
template<typename T>
void Expression<T>::dims(std::map<Key, int>& map) const {
root_->dims(map);
}
template<typename T>
T Expression<T>::value(const Values& values,
boost::optional<std::vector<Matrix>&> H) const {
if (H) {
// Call private version that returns derivatives in H
KeysAndDims pair = keysAndDims();
return valueAndDerivatives(values, pair.first, pair.second, *H);
} else
// no derivatives needed, just return value
return root_->value(values);
}
template<typename T>
const boost::shared_ptr<internal::ExpressionNode<T> >& Expression<T>::root() const {
return root_;
}
template<typename T>
size_t Expression<T>::traceSize() const {
return root_->traceSize();
}
// Private methods:
template<typename T>
T Expression<T>::valueAndDerivatives(const Values& values,
const FastVector<Key>& keys, const FastVector<int>& dims,
std::vector<Matrix>& H) const {
// H should be pre-allocated
assert(H.size()==keys.size());
// Pre-allocate and zero VerticalBlockMatrix
static const int Dim = traits<T>::dimension;
enum {
Constant, Leaf, Function
} kind;
union {
Key key;
CallRecord<Dim>* ptr;
} content;
public:
/// Pointer always starts out as a Constant
ExecutionTrace() :
kind(Constant) {
}
/// Change pointer to a Leaf Record
void setLeaf(Key key) {
kind = Leaf;
content.key = key;
}
/// Take ownership of pointer to a Function Record
void setFunction(CallRecord<Dim>* record) {
kind = Function;
content.ptr = record;
}
/// Print
void print(const std::string& indent = "") const {
if (kind == Constant)
std::cout << indent << "Constant" << std::endl;
else if (kind == Leaf)
std::cout << indent << "Leaf, key = " << content.key << std::endl;
else if (kind == 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() {
if (kind != Function)
return boost::none;
else {
Record* p = dynamic_cast<Record*>(content.ptr);
return p ? boost::optional<Record*>(p) : boost::none;
}
}
/**
* *** This is the main entry point for reverse AD, called from Expression ***
* Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function)
*/
typedef Eigen::Matrix<double, Dim, Dim> JacobianTT;
void startReverseAD1(JacobianMap& jacobians) const {
if (kind == Leaf) {
// This branch will only be called on trivial Leaf expressions, i.e. Priors
static const JacobianTT I = JacobianTT::Identity();
handleLeafCase(I, jacobians, content.key);
} else if (kind == Function)
// This is the more typical entry point, starting the AD pipeline
// Inside startReverseAD2 the correctly dimensioned pipeline is chosen.
content.ptr->startReverseAD2(jacobians);
}
// Either add to Jacobians (Leaf) or propagate (Function)
template<typename DerivedMatrix>
void reverseAD1(const Eigen::MatrixBase<DerivedMatrix> & dTdA,
JacobianMap& jacobians) const {
if (kind == Leaf)
handleLeafCase(dTdA, jacobians, content.key);
else if (kind == Function)
content.ptr->reverseAD2(dTdA, jacobians);
}
VerticalBlockMatrix Ab(dims, Dim);
Ab.matrix().setZero();
internal::JacobianMap jacobianMap(keys, Ab);
/// Define type so we can apply it as a meta-function
typedef ExecutionTrace<T> type;
};
// Call unsafe version
T result = valueAndJacobianMap(values, jacobianMap);
/// Storage type for the execution trace.
/// It enforces the proper alignment in a portable way.
/// Provide a traceSize() sized array of this type to traceExecution as traceStorage.
typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage;
// Copy blocks into the vector of jacobians passed in
for (DenseIndex i = 0; i < static_cast<DenseIndex>(keys.size()); i++)
H[i] = Ab(i);
//-----------------------------------------------------------------------------
/**
* Expression node. The superclass for objects that do the heavy lifting
* An Expression<T> has a pointer to an ExpressionNode<T> underneath
* allowing Expressions to have polymorphic behaviour even though they
* are passed by value. This is the same way boost::function works.
* http://loki-lib.sourceforge.net/html/a00652.html
*/
template<class T>
class ExpressionNode {
return result;
}
protected:
size_t traceSize_;
/// Constructor, traceSize is size of the execution trace of expression rooted here
ExpressionNode(size_t traceSize = 0) :
traceSize_(traceSize) {
}
public:
/// Destructor
virtual ~ExpressionNode() {
}
/// Streaming
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os,
const ExpressionNode& node) {
os << "Expression of type " << typeid(T).name();
if (node.traceSize_>0) os << ", trace size = " << node.traceSize_;
os << "\n";
return os;
}
/// Return keys that play in this expression as a set
virtual std::set<Key> keys() const {
std::set<Key> keys;
return keys;
}
/// Return dimensions for each argument, as a map
virtual void dims(std::map<Key, int>& map) const {
}
// Return size needed for memory buffer in traceExecution
size_t traceSize() const {
return traceSize_;
}
/// Return value
virtual T value(const Values& values) const = 0;
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const = 0;
};
//-----------------------------------------------------------------------------
/// Constant Expression
template<class T>
class ConstantExpression: public ExpressionNode<T> {
/// The constant value
T constant_;
/// Constructor with a value, yielding a constant
ConstantExpression(const T& value) :
constant_(value) {
}
friend class Expression<T> ;
public:
/// Return value
virtual T value(const Values& values) const {
return constant_;
}
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const {
return constant_;
}
};
//-----------------------------------------------------------------------------
/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value
template<typename T>
class LeafExpression : public ExpressionNode<T> {
typedef T value_type;
T Expression<T>::traceExecution(const Values& values,
internal::ExecutionTrace<T>& trace, void* traceStorage) const {
return root_->traceExecution(values, trace,
static_cast<internal::ExecutionTraceStorage*>(traceStorage));
}
/// The key into values
Key key_;
template<typename T>
T Expression<T>::valueAndJacobianMap(const Values& values,
internal::JacobianMap& jacobians) const {
// The following piece of code is absolutely crucial for performance.
// We allocate a block of memory on the stack, which can be done at runtime
// with modern C++ compilers. The traceExecution then fills this memory
// with an execution trace, made up entirely of "Record" structs, see
// the FunctionalNode class in expression-inl.h
size_t size = traceSize();
/// Constructor with a single key
LeafExpression(Key key) :
key_(key) {
}
// todo: do we need a virtual destructor here too?
friend class Expression<T> ;
public:
/// Return keys that play in this expression
virtual std::set<Key> keys() const {
std::set<Key> keys;
keys.insert(key_);
return keys;
}
/// Return dimensions for each argument
virtual void dims(std::map<Key, int>& map) const {
map[key_] = traits<T>::dimension;
}
/// Return value
virtual T value(const Values& values) const {
return values.at<T>(key_);
}
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const {
trace.setLeaf(key_);
return values.at<T>(key_);
}
};
//-----------------------------------------------------------------------------
// Below we use the "Class Composition" technique described in the book
// C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost
// and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education.
// to recursively generate a class, that will be the base for function nodes.
//
// The class generated, for three arguments A1, A2, and A3 will be
//
// struct Base1 : Argument<T,A1,1>, FunctionalBase<T> {
// ... storage related to A1 ...
// ... methods that work on A1 ...
// };
//
// struct Base2 : Argument<T,A2,2>, Base1 {
// ... storage related to A2 ...
// ... methods that work on A2 and (recursively) on A1 ...
// };
//
// struct Base3 : Argument<T,A3,3>, Base2 {
// ... storage related to A3 ...
// ... methods that work on A3 and (recursively) on A2 and A1 ...
// };
//
// struct FunctionalNode : Base3 {
// Provides convenience access to storage in hierarchy by using
// static_cast<Argument<T, A, N> &>(*this)
// }
//
// All this magic happens when we generate the Base3 base class of FunctionalNode
// by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode
//
// Similarly, the inner Record struct will be
//
// struct Record1 : JacobianTrace<T,A1,1>, CallRecord<traits::dimension<T>::value> {
// ... storage related to A1 ...
// ... methods that work on A1 ...
// };
//
// struct Record2 : JacobianTrace<T,A2,2>, Record1 {
// ... storage related to A2 ...
// ... methods that work on A2 and (recursively) on A1 ...
// };
//
// struct Record3 : JacobianTrace<T,A3,3>, Record2 {
// ... storage related to A3 ...
// ... methods that work on A3 and (recursively) on A2 and A1 ...
// };
//
// struct Record : Record3 {
// Provides convenience access to storage in hierarchy by using
// static_cast<JacobianTrace<T, A, N> &>(*this)
// }
//
//-----------------------------------------------------------------------------
/// meta-function to generate fixed-size JacobianTA type
template<class T, class A>
struct Jacobian {
typedef Eigen::Matrix<double, traits<T>::dimension,
traits<A>::dimension> type;
};
/// meta-function to generate JacobianTA optional reference
template<class T, class A>
struct MakeOptionalJacobian {
typedef OptionalJacobian<traits<T>::dimension,
traits<A>::dimension> type;
};
/**
* Base case for recursive FunctionalNode class
*/
template<class T>
struct FunctionalBase: ExpressionNode<T> {
static size_t const N = 0; // number of arguments
struct Record {
void print(const std::string& indent) const {
}
void startReverseAD4(JacobianMap& jacobians) const {
}
template<typename SomeMatrix>
void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
}
};
/// Construct an execution trace for reverse AD
void trace(const Values& values, Record* record,
ExecutionTraceStorage*& traceStorage) const {
// base case: does not do anything
}
};
/**
* Building block for recursive FunctionalNode class
* The integer argument N is to guarantee a unique type signature,
* so we are guaranteed to be able to extract their values by static cast.
*/
template<class T, class A, size_t N>
struct Argument {
/// Expression that will generate value/derivatives for argument
boost::shared_ptr<ExpressionNode<A> > expression;
};
/**
* Building block for Recursive Record Class
* Records the evaluation of a single argument in a functional expression
*/
template<class T, class A, size_t N>
struct JacobianTrace {
A value;
ExecutionTrace<A> trace;
typename Jacobian<T, A>::type dTdA;
};
// Recursive Definition of Functional ExpressionNode
// The reason we inherit from Argument<T, A, N> is because we can then
// case to this unique signature to retrieve the expression at any level
template<class T, class A, class Base>
struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
static size_t const N = Base::N + 1; ///< Number of arguments in hierarchy
typedef Argument<T, A, N> This; ///< The storage we have direct access to
/// Return keys that play in this expression
virtual std::set<Key> keys() const {
std::set<Key> keys = Base::keys();
std::set<Key> myKeys = This::expression->keys();
keys.insert(myKeys.begin(), myKeys.end());
return keys;
}
/// Return dimensions for each argument
virtual void dims(std::map<Key, int>& map) const {
Base::dims(map);
This::expression->dims(map);
}
// Recursive Record Class for Functional Expressions
// The reason we inherit from JacobianTrace<T, A, N> is because we can then
// case to this unique signature to retrieve the value/trace at any level
struct Record: JacobianTrace<T, A, N>, Base::Record {
typedef T return_type;
typedef JacobianTrace<T, A, N> This;
/// Print to std::cout
void print(const std::string& indent) const {
Base::Record::print(indent);
static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
std::cout << This::dTdA.format(matlab) << std::endl;
This::trace.print(indent);
}
/// Start the reverse AD process
void startReverseAD4(JacobianMap& jacobians) const {
Base::Record::startReverseAD4(jacobians);
// This is the crucial point where the size of the AD pipeline is selected.
// One pipeline is started for each argument, but the number of rows in each
// pipeline is the same, namely the dimension of the output argument T.
// For example, if the entire expression is rooted by a binary function
// yielding a 2D result, then the matrix dTdA will have 2 rows.
// ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2
// which calls the correctly sized CallRecord::reverseAD3, which in turn
// calls reverseAD4 below.
This::trace.reverseAD1(This::dTdA, jacobians);
}
/// Given df/dT, multiply in dT/dA and continue reverse AD process
// Cols is always known at compile time
template<typename SomeMatrix>
void reverseAD4(const SomeMatrix & dFdT,
JacobianMap& jacobians) const {
Base::Record::reverseAD4(dFdT, jacobians);
This::trace.reverseAD1(dFdT * This::dTdA, jacobians);
}
};
/// Construct an execution trace for reverse AD
void trace(const Values& values, Record* record,
ExecutionTraceStorage*& traceStorage) const {
Base::trace(values, record, traceStorage); // recurse
// 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->Record::This::value = This::expression->traceExecution(values,
record->Record::This::trace, traceStorage);
// traceStorage is never modified by traceExecution, but if traceExecution has
// written in the buffer, the next caller expects we advance the pointer
traceStorage += This::expression->traceSize();
}
};
/**
* Recursive GenerateFunctionalNode class Generator
*/
template<class T, class TYPES>
struct FunctionalNode {
/// The following typedef generates the recursively defined Base class
typedef typename boost::mpl::fold<TYPES, FunctionalBase<T>,
GenerateFunctionalNode<T, MPL::_2, MPL::_1> >::type Base;
/**
* The type generated by this meta-function derives from Base
* and adds access functions as well as the crucial [trace] function
*/
struct type: public Base {
// Argument types and derived, note these are base 0 !
// These are currently not used - useful for Phoenix in future
#ifdef EXPRESSIONS_PHOENIX
typedef TYPES Arguments;
typedef typename boost::mpl::transform<TYPES, Jacobian<T, MPL::_1> >::type Jacobians;
typedef typename boost::mpl::transform<TYPES, OptionalJacobian<T, MPL::_1> >::type Optionals;
// Windows does not support variable length arrays, so memory must be dynamically
// allocated on Visual Studio. For more information see the issue below
// https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio
#ifdef _MSC_VER
internal::ExecutionTraceStorage* traceStorage = new internal::ExecutionTraceStorage[size];
#else
internal::ExecutionTraceStorage traceStorage[size];
#endif
/// Reset expression shared pointer
template<class A, size_t N>
void reset(const boost::shared_ptr<ExpressionNode<A> >& ptr) {
static_cast<Argument<T, A, N> &>(*this).expression = ptr;
}
internal::ExecutionTrace<T> trace;
T value(this->traceExecution(values, trace, traceStorage));
trace.startReverseAD1(jacobians);
/// Access Expression shared pointer
template<class A, size_t N>
boost::shared_ptr<ExpressionNode<A> > expression() const {
return static_cast<Argument<T, A, N> const &>(*this).expression;
}
#ifdef _MSC_VER
delete[] traceStorage;
#endif
/// Provide convenience access to Record storage and implement
/// the virtual function based interface of CallRecord using the CallRecordImplementor
struct Record: public internal::CallRecordImplementor<Record,
traits<T>::dimension>, public Base::Record {
using Base::Record::print;
using Base::Record::startReverseAD4;
using Base::Record::reverseAD4;
virtual ~Record() {
}
/// Access Value
template<class A, size_t N>
const A& value() const {
return static_cast<JacobianTrace<T, A, N> const &>(*this).value;
}
/// Access Jacobian
template<class A, size_t N>
typename Jacobian<T, A>::type& jacobian() {
return static_cast<JacobianTrace<T, A, N>&>(*this).dTdA;
}
};
/// Construct an execution trace for reverse AD
Record* trace(const Values& values,
ExecutionTraceStorage* traceStorage) const {
assert(reinterpret_cast<size_t>(traceStorage) % TraceAlignment == 0);
// Create the record and advance the pointer
Record* record = new (traceStorage) Record();
traceStorage += upAligned(sizeof(Record));
// Record the traces for all arguments
// After this, the traceStorage pointer is set to after what was written
Base::trace(values, record, traceStorage);
// Return the record for this function evaluation
return record;
}
};
};
//-----------------------------------------------------------------------------
/// Unary Function Expression
template<class T, class A1>
class UnaryExpression: public FunctionalNode<T, boost::mpl::vector<A1> >::type {
typedef typename MakeOptionalJacobian<T, A1>::type OJ1;
public:
typedef boost::function<T(const A1&, OJ1)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1> >::type Base;
typedef typename Base::Record Record;
private:
Function function_;
/// Constructor with a unary function f, and input argument e
UnaryExpression(Function f, const Expression<A1>& e1) :
function_(f) {
this->template reset<A1, 1>(e1.root());
ExpressionNode<T>::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize();
}
friend class Expression<T> ;
public:
/// Return value
virtual T value(const Values& values) const {
return function_(this->template expression<A1, 1>()->value(values), boost::none);
}
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const {
Record* record = Base::trace(values, traceStorage);
trace.setFunction(record);
return function_(record->template value<A1, 1>(),
record->template jacobian<A1, 1>());
}
};
//-----------------------------------------------------------------------------
/// Binary Expression
template<class T, class A1, class A2>
class BinaryExpression:
public FunctionalNode<T, boost::mpl::vector<A1, A2> >::type {
typedef typename MakeOptionalJacobian<T, A1>::type OJ1;
typedef typename MakeOptionalJacobian<T, A2>::type OJ2;
public:
typedef boost::function<T(const A1&, const A2&, OJ1, OJ2)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2> >::type Base;
typedef typename Base::Record Record;
private:
Function function_;
/// Constructor with a ternary function f, and three input arguments
BinaryExpression(Function f, const Expression<A1>& e1,
const Expression<A2>& e2) :
function_(f) {
this->template reset<A1, 1>(e1.root());
this->template reset<A2, 2>(e2.root());
ExpressionNode<T>::traceSize_ = //
upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize();
}
friend class Expression<T> ;
friend class ::ExpressionFactorBinaryTest;
public:
/// Return value
virtual T value(const Values& values) const {
using boost::none;
return function_(this->template expression<A1, 1>()->value(values),
this->template expression<A2, 2>()->value(values),
none, none);
}
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const {
Record* record = Base::trace(values, traceStorage);
trace.setFunction(record);
return function_(record->template value<A1, 1>(),
record->template value<A2,2>(), record->template jacobian<A1, 1>(),
record->template jacobian<A2, 2>());
}
};
//-----------------------------------------------------------------------------
/// Ternary Expression
template<class T, class A1, class A2, class A3>
class TernaryExpression:
public FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type {
typedef typename MakeOptionalJacobian<T, A1>::type OJ1;
typedef typename MakeOptionalJacobian<T, A2>::type OJ2;
typedef typename MakeOptionalJacobian<T, A3>::type OJ3;
public:
typedef boost::function<T(const A1&, const A2&, const A3&, OJ1, OJ2, OJ3)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type Base;
typedef typename Base::Record Record;
private:
Function function_;
/// Constructor with a ternary function f, and three input arguments
TernaryExpression(Function f, const Expression<A1>& e1,
const Expression<A2>& e2, const Expression<A3>& e3) :
function_(f) {
this->template reset<A1, 1>(e1.root());
this->template reset<A2, 2>(e2.root());
this->template reset<A3, 3>(e3.root());
ExpressionNode<T>::traceSize_ = //
upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize()
+ e3.traceSize();
}
friend class Expression<T> ;
public:
/// Return value
virtual T value(const Values& values) const {
using boost::none;
return function_(this->template expression<A1, 1>()->value(values),
this->template expression<A2, 2>()->value(values),
this->template expression<A3, 3>()->value(values),
none, none, none);
}
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const {
Record* record = Base::trace(values, traceStorage);
trace.setFunction(record);
return function_(
record->template value<A1, 1>(), record->template value<A2, 2>(),
record->template value<A3, 3>(), record->template jacobian<A1, 1>(),
record->template jacobian<A2, 2>(), record->template jacobian<A3, 3>());
}
};
//-----------------------------------------------------------------------------
return value;
}
template<typename T>
typename Expression<T>::KeysAndDims Expression<T>::keysAndDims() const {
std::map<Key, int> map;
dims(map);
size_t n = map.size();
KeysAndDims pair = std::make_pair(FastVector<Key>(n), FastVector<int>(n));
boost::copy(map | boost::adaptors::map_keys, pair.first.begin());
boost::copy(map | boost::adaptors::map_values, pair.second.begin());
return pair;
}
namespace internal {
// http://stackoverflow.com/questions/16260445/boost-bind-to-operator
template<class T>
struct apply_compose {
typedef T result_type;
static const int Dim = traits<T>::dimension;
T operator()(const T& x, const T& y, OptionalJacobian<Dim, Dim> H1 =
boost::none, OptionalJacobian<Dim, Dim> H2 = boost::none) const {
return x.compose(y, H1, H2);
}
};
}
// Global methods:
/// Construct a product expression, assumes T::compose(T) -> T
template<typename T>
Expression<T> operator*(const Expression<T>& expression1,
const Expression<T>& expression2) {
return Expression<T>(
boost::bind(internal::apply_compose<T>(), _1, _2, _3, _4), expression1,
expression2);
}
/// Construct an array of leaves
template<typename T>
std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start) {
std::vector<Expression<T> > unknowns;
unknowns.reserve(n);
for (size_t i = start; i < start + n; i++)
unknowns.push_back(Expression<T>(c, i));
return unknowns;
}
} // namespace gtsam

292
gtsam/nonlinear/Expression.h
View File

@ -19,21 +19,28 @@
#pragma once
#include <gtsam/nonlinear/Expression-inl.h>
#include <gtsam/nonlinear/internal/JacobianMap.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/OptionalJacobian.h>
#include <boost/bind.hpp>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/make_shared.hpp>
#include <map>
// Forward declare tests
class ExpressionFactorShallowTest;
namespace gtsam {
// Forward declare
// Forward declares
class Values;
template<typename T> class ExpressionFactor;
namespace internal {
template<typename T> class ExecutionTrace;
template<typename T> class ExpressionNode;
}
/**
* Expression class that supports automatic differentiation
*/
@ -48,110 +55,97 @@ public:
private:
// Paul's trick shared pointer, polymorphic root of entire expression tree
boost::shared_ptr<ExpressionNode<T> > root_;
boost::shared_ptr<internal::ExpressionNode<T> > root_;
public:
// Expressions wrap trees of functions that can evaluate their own derivatives.
// The meta-functions below are useful to specify the type of those functions.
// Example, a function taking a camera and a 3D point and yielding a 2D point:
// Expression<Point2>::BinaryFunction<SimpleCamera,Point3>::type
template<class A1>
struct UnaryFunction {
typedef boost::function<
T(const A1&, typename MakeOptionalJacobian<T, A1>::type)> type;
};
template<class A1, class A2>
struct BinaryFunction {
typedef boost::function<
T(const A1&, const A2&, typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type)> type;
};
template<class A1, class A2, class A3>
struct TernaryFunction {
typedef boost::function<
T(const A1&, const A2&, const A3&,
typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type,
typename MakeOptionalJacobian<T, A3>::type)> type;
};
/// Print
void print(const std::string& s) const {
std::cout << s << *root_ << std::endl;
}
void print(const std::string& s) const;
// Construct a constant expression
Expression(const T& value) :
root_(new ConstantExpression<T>(value)) {
}
/// Construct a constant expression
Expression(const T& value);
// Construct a leaf expression, with Key
Expression(const Key& key) :
root_(new LeafExpression<T>(key)) {
}
/// Construct a leaf expression, with Key
Expression(const Key& key);
// Construct a leaf expression, with Symbol
Expression(const Symbol& symbol) :
root_(new LeafExpression<T>(symbol)) {
}
/// Construct a leaf expression, with Symbol
Expression(const Symbol& symbol);
// Construct a leaf expression, creating Symbol
Expression(unsigned char c, size_t j) :
root_(new LeafExpression<T>(Symbol(c, j))) {
}
/// Construct a leaf expression, creating Symbol
Expression(unsigned char c, size_t j);
/// Construct a unary function expression
template<typename A>
Expression(typename UnaryFunction<A>::type function,
const Expression<A>& expression);
/// Construct a binary function expression
template<typename A1, typename A2>
Expression(typename BinaryFunction<A1, A2>::type function,
const Expression<A1>& expression1, const Expression<A2>& expression2);
/// Construct a ternary function expression
template<typename A1, typename A2, typename A3>
Expression(typename TernaryFunction<A1, A2, A3>::type function,
const Expression<A1>& expression1, const Expression<A2>& expression2,
const Expression<A3>& expression3);
/// Construct a nullary method expression
template<typename A>
Expression(const Expression<A>& expression,
T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const) :
root_(new UnaryExpression<T, A>(boost::bind(method, _1, _2), expression)) {
}
/// Construct a unary function expression
template<typename A>
Expression(typename UnaryExpression<T, A>::Function function,
const Expression<A>& expression) :
root_(new UnaryExpression<T, A>(function, expression)) {
}
T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const);
/// Construct a unary method expression
template<typename A1, typename A2>
Expression(const Expression<A1>& expression1,
T (A1::*method)(const A2&, typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type) const,
const Expression<A2>& expression2) :
root_(
new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),
expression1, expression2)) {
}
/// Construct a binary function expression
template<typename A1, typename A2>
Expression(typename BinaryExpression<T, A1, A2>::Function function,
const Expression<A1>& expression1, const Expression<A2>& expression2) :
root_(new BinaryExpression<T, A1, A2>(function, expression1, expression2)) {
}
const Expression<A2>& expression2);
/// Construct a binary method expression
template<typename A1, typename A2, typename A3>
Expression(const Expression<A1>& expression1,
T (A1::*method)(const A2&, const A3&,
typename TernaryExpression<T, A1, A2, A3>::OJ1,
typename TernaryExpression<T, A1, A2, A3>::OJ2,
typename TernaryExpression<T, A1, A2, A3>::OJ3) const,
const Expression<A2>& expression2, const Expression<A3>& expression3) :
root_(
new TernaryExpression<T, A1, A2, A3>(
boost::bind(method, _1, _2, _3, _4, _5, _6), expression1,
expression2, expression3)) {
}
typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type,
typename MakeOptionalJacobian<T, A3>::type) const,
const Expression<A2>& expression2, const Expression<A3>& expression3);
/// Construct a ternary function expression
template<typename A1, typename A2, typename A3>
Expression(typename TernaryExpression<T, A1, A2, A3>::Function function,
const Expression<A1>& expression1, const Expression<A2>& expression2,
const Expression<A3>& expression3) :
root_(
new TernaryExpression<T, A1, A2, A3>(function, expression1,
expression2, expression3)) {
}
/// Return root
const boost::shared_ptr<ExpressionNode<T> >& root() const {
return root_;
}
// Return size needed for memory buffer in traceExecution
size_t traceSize() const {
return root_->traceSize();
/// Destructor
virtual ~Expression() {
}
/// Return keys that play in this expression
std::set<Key> keys() const {
return root_->keys();
}
std::set<Key> keys() const;
/// Return dimensions for each argument, as a map
void dims(std::map<Key, int>& map) const {
root_->dims(map);
}
void dims(std::map<Key, int>& map) const;
/**
* @brief Return value and optional derivatives, reverse AD version
@ -159,16 +153,7 @@ public:
* The order of the Jacobians is same as keys in either keys() or dims()
*/
T value(const Values& values, boost::optional<std::vector<Matrix>&> H =
boost::none) const {
if (H) {
// Call private version that returns derivatives in H
KeysAndDims pair = keysAndDims();
return value(values, pair.first, pair.second, *H);
} else
// no derivatives needed, just return value
return root_->value(values);
}
boost::none) const;
/**
* @return a "deep" copy of this Expression
@ -179,116 +164,55 @@ public:
return boost::make_shared<Expression>(*this);
}
/// Return root
const boost::shared_ptr<internal::ExpressionNode<T> >& root() const;
/// Return size needed for memory buffer in traceExecution
size_t traceSize() const;
private:
/// Vaguely unsafe keys and dimensions in same order
/// Keys and dimensions in same order
typedef std::pair<FastVector<Key>, FastVector<int> > KeysAndDims;
KeysAndDims keysAndDims() const {
std::map<Key, int> map;
dims(map);
size_t n = map.size();
KeysAndDims pair = std::make_pair(FastVector<Key>(n), FastVector<int>(n));
boost::copy(map | boost::adaptors::map_keys, pair.first.begin());
boost::copy(map | boost::adaptors::map_values, pair.second.begin());
return pair;
}
KeysAndDims keysAndDims() const;
/// private version that takes keys and dimensions, returns derivatives
T value(const Values& values, const FastVector<Key>& keys,
const FastVector<int>& dims, std::vector<Matrix>& H) const {
// H should be pre-allocated
assert(H.size()==keys.size());
// Pre-allocate and zero VerticalBlockMatrix
static const int Dim = traits<T>::dimension;
VerticalBlockMatrix Ab(dims, Dim);
Ab.matrix().setZero();
JacobianMap jacobianMap(keys, Ab);
// Call unsafe version
T result = value(values, jacobianMap);
// Copy blocks into the vector of jacobians passed in
for (DenseIndex i = 0; i < static_cast<DenseIndex>(keys.size()); i++)
H[i] = Ab(i);
return result;
}
T valueAndDerivatives(const Values& values, const FastVector<Key>& keys,
const FastVector<int>& dims, std::vector<Matrix>& H) const;
/// trace execution, very unsafe
T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const {
return root_->traceExecution(values, trace, traceStorage);
}
T traceExecution(const Values& values, internal::ExecutionTrace<T>& trace,
void* traceStorage) const;
/**
* @brief Return value and derivatives, reverse AD version
* This very unsafe method needs a JacobianMap with correctly allocated
* and initialized VerticalBlockMatrix, hence is declared private.
*/
T value(const Values& values, JacobianMap& jacobians) const {
// The following piece of code is absolutely crucial for performance.
// We allocate a block of memory on the stack, which can be done at runtime
// with modern C++ compilers. The traceExecution then fills this memory
// with an execution trace, made up entirely of "Record" structs, see
// the FunctionalNode class in expression-inl.h
size_t size = traceSize();
// Windows does not support variable length arrays, so memory must be dynamically
// allocated on Visual Studio. For more information see the issue below
// https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio
#ifdef _MSC_VER
ExecutionTraceStorage* traceStorage = new ExecutionTraceStorage[size];
#else
ExecutionTraceStorage traceStorage[size];
#endif
ExecutionTrace<T> trace;
T value(traceExecution(values, trace, traceStorage));
trace.startReverseAD1(jacobians);
#ifdef _MSC_VER
delete[] traceStorage;
#endif
return value;
}
/// brief Return value and derivatives, reverse AD version
T valueAndJacobianMap(const Values& values,
internal::JacobianMap& jacobians) const;
// be very selective on who can access these private methods:
friend class ExpressionFactor<T> ;
friend class internal::ExpressionNode<T>;
// and add tests
friend class ::ExpressionFactorShallowTest;
};
// http://stackoverflow.com/questions/16260445/boost-bind-to-operator
template<class T>
struct apply_compose {
typedef T result_type;
static const int Dim = traits<T>::dimension;
T operator()(const T& x, const T& y, OptionalJacobian<Dim, Dim> H1 =
boost::none, OptionalJacobian<Dim, Dim> H2 = boost::none) const {
return x.compose(y, H1, H2);
}
};
/// Construct a product expression, assumes T::compose(T) -> T
/**
* Construct a product expression, assumes T::compose(T) -> T
* Example:
* Expression<Point2> a(0), b(1), c = a*b;
*/
template<typename T>
Expression<T> operator*(const Expression<T>& expression1,
const Expression<T>& expression2) {
return Expression<T>(boost::bind(apply_compose<T>(), _1, _2, _3, _4),
expression1, expression2);
}
Expression<T> operator*(const Expression<T>& e1, const Expression<T>& e2);
/// Construct an array of leaves
/**
* Construct an array of unknown expressions with successive symbol keys
* Example:
* createUnknowns<Pose2>(3,'x') creates unknown expressions for x0,x1,x2
*/
template<typename T>
std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start = 0) {
std::vector<Expression<T> > unknowns;
unknowns.reserve(n);
for (size_t i = start; i < start + n; i++)
unknowns.push_back(Expression<T>(c, i));
return unknowns;
}
std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start = 0);
}
} // namespace gtsam
#include <gtsam/nonlinear/Expression-inl.h>

10
gtsam/nonlinear/ExpressionFactor.h
View File

@ -65,8 +65,8 @@ public:
*/
virtual Vector unwhitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const {
if (H) {
const T value = expression_.value(x, keys_, dims_, *H);
if (H) {
const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
return traits<T>::Local(measurement_, value);
} else {
const T value = expression_.value(x);
@ -89,13 +89,13 @@ public:
// Wrap keys and VerticalBlockMatrix into structure passed to expression_
VerticalBlockMatrix& Ab = factor->matrixObject();
JacobianMap jacobianMap(keys_, Ab);
internal::JacobianMap jacobianMap(keys_, Ab);
// Zero out Jacobian so we can simply add to it
Ab.matrix().setZero();
// Get value and Jacobians, writing directly into JacobianFactor
T value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here !
T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here !
// Evaluate error and set RHS vector b
Ab(size()).col(0) = -traits<T>::Local(measurement_, value);
@ -109,5 +109,5 @@ public:
};
// ExpressionFactor
} // \ namespace gtsam
}// \ namespace gtsam

9
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -25,9 +25,10 @@
#include <boost/algorithm/string.hpp>
#include <boost/range/adaptor/map.hpp>
#include <fstream>
#include <limits>
#include <string>
#include <cmath>
#include <fstream>
using namespace std;
@ -178,7 +179,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem(
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
damped += boost::make_shared<JacobianFactor>(key_vector.first, A, b,
model);
} catch (std::exception e) {
} catch (const std::exception& e) {
// Don't attempt any damping if no key found in diagonal
continue;
}
@ -247,7 +248,7 @@ void LevenbergMarquardtOptimizer::iterate() {
double modelFidelity = 0.0;
bool step_is_successful = false;
bool stopSearchingLambda = false;
double newError;
double newError = numeric_limits<double>::infinity();
Values newValues;
VectorValues delta;
@ -255,7 +256,7 @@ void LevenbergMarquardtOptimizer::iterate() {
try {
delta = solve(dampedSystem, state_.values, params_);
systemSolvedSuccessfully = true;
} catch (IndeterminantLinearSystemException) {
} catch (const IndeterminantLinearSystemException& e) {
systemSolvedSuccessfully = false;
}

4
gtsam/nonlinear/expressionTesting.h
View File

@ -88,7 +88,7 @@ void testFactorJacobians(TestResult& result_, const std::string& name_,
// Check cast result and then equality
CHECK(actual);
EXPECT( assert_equal(expected, *actual, tolerance));
EXPECT(assert_equal(expected, *actual, tolerance));
}
}
@ -112,7 +112,7 @@ void testExpressionJacobians(TestResult& result_, const std::string& name_,
expression.value(values), expression);
testFactorJacobians(result_, name_, f, values, nd_step, tolerance);
}
}
} // namespace internal
} // namespace gtsam
/// \brief Check the Jacobians produced by an expression against finite differences.

20
gtsam/nonlinear/CallRecord.h → gtsam/nonlinear/internal/CallRecord.h
View File

@ -20,18 +20,9 @@
#pragma once
#include <gtsam/base/VerticalBlockMatrix.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/Matrix.h>
#include <boost/mpl/transform.hpp>
#include <gtsam/nonlinear/internal/JacobianMap.h>
namespace gtsam {
class JacobianMap;
// forward declaration
//-----------------------------------------------------------------------------
namespace internal {
/**
@ -64,8 +55,6 @@ struct ConvertToVirtualFunctionSupportedMatrixType<false> {
}
};
} // namespace internal
/**
* The CallRecord is an abstract base class for the any class that stores
* the Jacobians of applying a function with respect to each of its arguments,
@ -94,9 +83,8 @@ struct CallRecord {
inline void reverseAD2(const Eigen::MatrixBase<Derived> & dFdT,
JacobianMap& jacobians) const {
_reverseAD3(
internal::ConvertToVirtualFunctionSupportedMatrixType<
(Derived::RowsAtCompileTime > 5)>::convert(dFdT),
jacobians);
ConvertToVirtualFunctionSupportedMatrixType<
(Derived::RowsAtCompileTime > 5)>::convert(dFdT), jacobians);
}
// This overload supports matrices with both rows and columns dynamically sized.
@ -140,7 +128,6 @@ private:
*/
const int CallRecordMaxVirtualStaticRows = 5;
namespace internal {
/**
* The CallRecordImplementor implements the CallRecord interface for a Derived class by
* delegating to its corresponding (templated) non-virtual methods.
@ -193,5 +180,4 @@ private:
};
} // namespace internal
} // gtsam

166
gtsam/nonlinear/internal/ExecutionTrace.h Normal file
View File

@ -0,0 +1,166 @@
/* ----------------------------------------------------------------------------
* 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)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ExecutionTrace.h
* @date May 11, 2015
* @author Frank Dellaert
* @brief Execution trace for expressions
*/
#pragma once
#include <gtsam/nonlinear/internal/JacobianMap.h>
#include <gtsam/inference/Key.h>
#include <gtsam/base/Manifold.h>
#include <boost/type_traits/aligned_storage.hpp>
#include <Eigen/Core>
namespace gtsam {
namespace internal {
template<int T> struct CallRecord;
/// Storage type for the execution trace.
/// It enforces the proper alignment in a portable way.
/// Provide a traceSize() sized array of this type to traceExecution as traceStorage.
static const unsigned TraceAlignment = 16;
typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage;
template<bool UseBlock, typename Derived>
struct UseBlockIf {
static void addToJacobian(const Eigen::MatrixBase<Derived>& dTdA,
JacobianMap& jacobians, Key key) {
// block makes HUGE difference
jacobians(key).block<Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>(
0, 0) += dTdA;
}
};
/// Handle Leaf Case for Dynamic Matrix type (slower)
template<typename Derived>
struct UseBlockIf<false, Derived> {
static void addToJacobian(const Eigen::MatrixBase<Derived>& dTdA,
JacobianMap& jacobians, Key key) {
jacobians(key) += dTdA;
}
};
/// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians
template<typename Derived>
void handleLeafCase(const Eigen::MatrixBase<Derived>& dTdA,
JacobianMap& jacobians, Key key) {
UseBlockIf<
Derived::RowsAtCompileTime != Eigen::Dynamic
&& Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian(
dTdA, jacobians, key);
}
/**
* The ExecutionTrace class records a tree-structured expression's execution.
*
* The class looks a bit complicated but it is so for performance.
* It is a tagged union that obviates the need to create
* a ExecutionTrace subclass for Constants and Leaf Expressions. Instead
* the key for the leaf is stored in the space normally used to store a
* CallRecord*. Nothing is stored for a Constant.
*
* A full execution trace of a Binary(Unary(Binary(Leaf,Constant)),Leaf) would be:
* Trace(Function) ->
* BinaryRecord with two traces in it
* trace1(Function) ->
* UnaryRecord with one trace in it
* trace1(Function) ->
* BinaryRecord with two traces in it
* trace1(Leaf)
* trace2(Constant)
* trace2(Leaf)
* Hence, there are three Record structs, written to memory by traceExecution
*/
template<class T>
class ExecutionTrace {
static const int Dim = traits<T>::dimension;
enum {
Constant, Leaf, Function
} kind;
union {
Key key;
CallRecord<Dim>* ptr;
} content;
public:
/// Pointer always starts out as a Constant
ExecutionTrace() :
kind(Constant) {
}
/// Change pointer to a Leaf Record
void setLeaf(Key key) {
kind = Leaf;
content.key = key;
}
/// Take ownership of pointer to a Function Record
void setFunction(CallRecord<Dim>* record) {
kind = Function;
content.ptr = record;
}
/// Print
void print(const std::string& indent = "") const {
if (kind == Constant)
std::cout << indent << "Constant" << std::endl;
else if (kind == Leaf)
std::cout << indent << "Leaf, key = " << content.key << std::endl;
else if (kind == 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() {
if (kind != Function)
return boost::none;
else {
Record* p = dynamic_cast<Record*>(content.ptr);
return p ? boost::optional<Record*>(p) : boost::none;
}
}
/**
* *** This is the main entry point for reverse AD, called from Expression ***
* Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function)
*/
typedef Eigen::Matrix<double, Dim, Dim> JacobianTT;
void startReverseAD1(JacobianMap& jacobians) const {
if (kind == Leaf) {
// This branch will only be called on trivial Leaf expressions, i.e. Priors
static const JacobianTT I = JacobianTT::Identity();
handleLeafCase(I, jacobians, content.key);
} else if (kind == Function)
// This is the more typical entry point, starting the AD pipeline
// Inside startReverseAD2 the correctly dimensioned pipeline is chosen.
content.ptr->startReverseAD2(jacobians);
}
// Either add to Jacobians (Leaf) or propagate (Function)
template<typename DerivedMatrix>
void reverseAD1(const Eigen::MatrixBase<DerivedMatrix> & dTdA,
JacobianMap& jacobians) const {
if (kind == Leaf)
handleLeafCase(dTdA, jacobians, content.key);
else if (kind == Function)
content.ptr->reverseAD2(dTdA, jacobians);
}
/// Define type so we can apply it as a meta-function
typedef ExecutionTrace<T> type;
};
} // namespace internal
} // namespace gtsam

516
gtsam/nonlinear/internal/ExpressionNode.h Normal file
View File

@ -0,0 +1,516 @@
/* ----------------------------------------------------------------------------
* 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)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ExpressionNode.h
* @date May 10, 2015
* @author Frank Dellaert
* @author Paul Furgale
* @brief ExpressionNode class
*/
#pragma once
#include <gtsam/nonlinear/internal/ExecutionTrace.h>
#include <gtsam/nonlinear/internal/CallRecord.h>
#include <gtsam/nonlinear/Values.h>
#include <typeinfo> // operator typeid
#include <ostream>
#include <map>
class ExpressionFactorBinaryTest;
// Forward declare for testing
namespace gtsam {
namespace internal {
template<typename T>
T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) {
// right now only word sized types are supported.
// Easy to extend if needed,
// by somehow inferring the unsigned integer of same size
BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t));
size_t & uiValue = reinterpret_cast<size_t &>(value);
size_t misAlignment = uiValue % requiredAlignment;
if (misAlignment) {
uiValue += requiredAlignment - misAlignment;
}
return value;
}
template<typename T>
T upAligned(T value, unsigned requiredAlignment = TraceAlignment) {
return upAlign(value, requiredAlignment);
}
//-----------------------------------------------------------------------------
/**
* Expression node. The superclass for objects that do the heavy lifting
* An Expression<T> has a pointer to an ExpressionNode<T> underneath
* allowing Expressions to have polymorphic behaviour even though they
* are passed by value. This is the same way boost::function works.
* http://loki-lib.sourceforge.net/html/a00652.html
*/
template<class T>
class ExpressionNode {
protected:
size_t traceSize_;
/// Constructor, traceSize is size of the execution trace of expression rooted here
ExpressionNode(size_t traceSize = 0) :
traceSize_(traceSize) {
}
public:
/// Destructor
virtual ~ExpressionNode() {
}
/// Streaming
GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os,
const ExpressionNode& node) {
os << "Expression of type " << typeid(T).name();
if (node.traceSize_ > 0)
os << ", trace size = " << node.traceSize_;
os << "\n";
return os;
}
/// Return keys that play in this expression as a set
virtual std::set<Key> keys() const {
std::set<Key> keys;
return keys;
}
/// Return dimensions for each argument, as a map
virtual void dims(std::map<Key, int>& map) const {
}
// Return size needed for memory buffer in traceExecution
size_t traceSize() const {
return traceSize_;
}
/// Return value
virtual T value(const Values& values) const = 0;
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const = 0;
};
//-----------------------------------------------------------------------------
/// Constant Expression
template<class T>
class ConstantExpression: public ExpressionNode<T> {
/// The constant value
T constant_;
/// Constructor with a value, yielding a constant
ConstantExpression(const T& value) :
constant_(value) {
}
friend class Expression<T> ;
public:
/// Destructor
virtual ~ConstantExpression() {
}
/// Return value
virtual T value(const Values& values) const {
return constant_;
}
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const {
return constant_;
}
};
//-----------------------------------------------------------------------------
/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value
template<typename T>
class LeafExpression: public ExpressionNode<T> {
typedef T value_type;
/// The key into values
Key key_;
/// Constructor with a single key
LeafExpression(Key key) :
key_(key) {
}
friend class Expression<T> ;
public:
/// Destructor
virtual ~LeafExpression() {
}
/// Return keys that play in this expression
virtual std::set<Key> keys() const {
std::set<Key> keys;
keys.insert(key_);
return keys;
}
/// Return dimensions for each argument
virtual void dims(std::map<Key, int>& map) const {
map[key_] = traits<T>::dimension;
}
/// Return value
virtual T value(const Values& values) const {
return values.at<T>(key_);
}
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const {
trace.setLeaf(key_);
return values.at<T>(key_);
}
};
//-----------------------------------------------------------------------------
/// meta-function to generate fixed-size JacobianTA type
template<class T, class A>
struct Jacobian {
typedef Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension> type;
};
// Eigen format for printing Jacobians
static const Eigen::IOFormat kMatlabFormat(0, 1, " ", "; ", "", "", "[", "]");
//-----------------------------------------------------------------------------
/// Unary Function Expression
template<class T, class A1>
class UnaryExpression: public ExpressionNode<T> {
typedef typename Expression<T>::template UnaryFunction<A1>::type Function;
boost::shared_ptr<ExpressionNode<A1> > expression1_;
Function function_;
/// Constructor with a unary function f, and input argument e1
UnaryExpression(Function f, const Expression<A1>& e1) :
expression1_(e1.root()), function_(f) {
ExpressionNode<T>::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize();
}
friend class Expression<T> ;
public:
/// Destructor
virtual ~UnaryExpression() {
}
/// Return value
virtual T value(const Values& values) const {
return function_(expression1_->value(values), boost::none);
}
/// Return keys that play in this expression
virtual std::set<Key> keys() const {
return expression1_->keys();
}
/// Return dimensions for each argument
virtual void dims(std::map<Key, int>& map) const {
expression1_->dims(map);
}
// Inner Record Class
struct Record: public CallRecordImplementor<Record, traits<T>::dimension> {
A1 value1;
ExecutionTrace<A1> trace1;
typename Jacobian<T, A1>::type dTdA1;
/// Print to std::cout
void print(const std::string& indent) const {
std::cout << indent << "UnaryExpression::Record {" << std::endl;
std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl;
trace1.print(indent);
std::cout << indent << "}" << std::endl;
}
/// Start the reverse AD process
void startReverseAD4(JacobianMap& jacobians) const {
// This is the crucial point where the size of the AD pipeline is selected.
// One pipeline is started for each argument, but the number of rows in each
// pipeline is the same, namely the dimension of the output argument T.
// For example, if the entire expression is rooted by a binary function
// yielding a 2D result, then the matrix dTdA will have 2 rows.
// ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2
// which calls the correctly sized CallRecord::reverseAD3, which in turn
// calls reverseAD4 below.
trace1.reverseAD1(dTdA1, jacobians);
}
/// Given df/dT, multiply in dT/dA and continue reverse AD process
template<typename SomeMatrix>
void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
trace1.reverseAD1(dFdT * dTdA1, jacobians);
}
};
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
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
// 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);
// ptr is never modified by traceExecution, but if traceExecution has
// written in the buffer, the next caller expects we advance the pointer
ptr += expression1_->traceSize();
trace.setFunction(record);
return function_(record->value1, record->dTdA1);
}
};
//-----------------------------------------------------------------------------
/// Binary Expression
template<class T, class A1, class A2>
class BinaryExpression: public ExpressionNode<T> {
typedef typename Expression<T>::template BinaryFunction<A1, A2>::type Function;
boost::shared_ptr<ExpressionNode<A1> > expression1_;
boost::shared_ptr<ExpressionNode<A2> > expression2_;
Function function_;
/// Constructor with a binary function f, and two input arguments
BinaryExpression(Function f, const Expression<A1>& e1,
const Expression<A2>& e2) :
expression1_(e1.root()), expression2_(e2.root()), function_(f) {
ExpressionNode<T>::traceSize_ = //
upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize();
}
friend class Expression<T> ;
friend class ::ExpressionFactorBinaryTest;
public:
/// Destructor
virtual ~BinaryExpression() {
}
/// Return value
virtual T value(const Values& values) const {
using boost::none;
return function_(expression1_->value(values), expression2_->value(values),
none, none);
}
/// Return keys that play in this expression
virtual std::set<Key> keys() const {
std::set<Key> keys = expression1_->keys();
std::set<Key> myKeys = expression2_->keys();
keys.insert(myKeys.begin(), myKeys.end());
return keys;
}
/// Return dimensions for each argument
virtual void dims(std::map<Key, int>& map) const {
expression1_->dims(map);
expression2_->dims(map);
}
// 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;
/// Print to std::cout
void print(const std::string& indent) const {
std::cout << indent << "BinaryExpression::Record {" << std::endl;
std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl;
trace1.print(indent);
std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl;
trace2.print(indent);
std::cout << indent << "}" << std::endl;
}
/// Start the reverse AD process, see comments in Base
void startReverseAD4(JacobianMap& jacobians) const {
trace1.reverseAD1(dTdA1, jacobians);
trace2.reverseAD1(dTdA2, jacobians);
}
/// Given df/dT, multiply in dT/dA and continue reverse AD process
template<typename SomeMatrix>
void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
trace1.reverseAD1(dFdT * dTdA1, jacobians);
trace2.reverseAD1(dFdT * dTdA2, jacobians);
}
};
/// Construct an execution trace for reverse AD, see UnaryExpression for explanation
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);
record->value2 = expression2_->traceExecution(values, record->trace2, ptr);
ptr += expression1_->traceSize() + expression2_->traceSize();
trace.setFunction(record);
return function_(record->value1, record->value2, record->dTdA1,
record->dTdA2);
}
};
//-----------------------------------------------------------------------------
/// Ternary Expression
template<class T, class A1, class A2, class A3>
class TernaryExpression: public ExpressionNode<T> {
typedef typename Expression<T>::template TernaryFunction<A1, A2, A3>::type Function;
boost::shared_ptr<ExpressionNode<A1> > expression1_;
boost::shared_ptr<ExpressionNode<A2> > expression2_;
boost::shared_ptr<ExpressionNode<A3> > expression3_;
Function function_;
/// Constructor with a ternary function f, and two input arguments
TernaryExpression(Function f, const Expression<A1>& e1,
const Expression<A2>& e2, const Expression<A3>& e3) :
expression1_(e1.root()), expression2_(e2.root()), expression3_(e3.root()), //
function_(f) {
ExpressionNode<T>::traceSize_ = upAligned(sizeof(Record)) + //
e1.traceSize() + e2.traceSize() + e3.traceSize();
}
friend class Expression<T> ;
public:
/// Destructor
virtual ~TernaryExpression() {
}
/// Return value
virtual T value(const Values& values) const {
using boost::none;
return function_(expression1_->value(values), expression2_->value(values),
expression3_->value(values), none, none, none);
}
/// Return keys that play in this expression
virtual std::set<Key> keys() const {
std::set<Key> keys = expression1_->keys();
std::set<Key> myKeys = expression2_->keys();
keys.insert(myKeys.begin(), myKeys.end());
myKeys = expression3_->keys();
keys.insert(myKeys.begin(), myKeys.end());
return keys;
}
/// Return dimensions for each argument
virtual void dims(std::map<Key, int>& map) const {
expression1_->dims(map);
expression2_->dims(map);
expression3_->dims(map);
}
// 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;
/// Print to std::cout
void print(const std::string& indent) const {
std::cout << indent << "TernaryExpression::Record {" << std::endl;
std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl;
trace1.print(indent);
std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl;
trace2.print(indent);
std::cout << indent << dTdA3.format(kMatlabFormat) << std::endl;
trace3.print(indent);
std::cout << indent << "}" << std::endl;
}
/// Start the reverse AD process, see comments in Base
void startReverseAD4(JacobianMap& jacobians) const {
trace1.reverseAD1(dTdA1, jacobians);
trace2.reverseAD1(dTdA2, jacobians);
trace3.reverseAD1(dTdA3, jacobians);
}
/// Given df/dT, multiply in dT/dA and continue reverse AD process
template<typename SomeMatrix>
void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
trace1.reverseAD1(dFdT * dTdA1, jacobians);
trace2.reverseAD1(dFdT * dTdA2, jacobians);
trace3.reverseAD1(dFdT * dTdA3, jacobians);
}
};
/// Construct an execution trace for reverse AD, see UnaryExpression for explanation
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);
record->value2 = expression2_->traceExecution(values, record->trace2, ptr);
record->value3 = expression3_->traceExecution(values, record->trace3, ptr);
ptr += expression1_->traceSize() + expression2_->traceSize()
+ expression3_->traceSize();
trace.setFunction(record);
return function_(record->value1, record->value2, record->value3,
record->dTdA1, record->dTdA2, record->dTdA3);
}
};
} // namespace internal
} // namespace gtsam

54
gtsam/nonlinear/internal/JacobianMap.h Normal file
View File

@ -0,0 +1,54 @@
/* ----------------------------------------------------------------------------
* 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)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file JacobianMap.h
* @date May 11, 2015
* @author Frank Dellaert
* @brief JacobianMap for returning derivatives from expressions
*/
#pragma once
#include <gtsam/inference/Key.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/VerticalBlockMatrix.h>
namespace gtsam {
namespace internal {
// A JacobianMap is the primary mechanism by which derivatives are returned.
// Expressions are designed to write their derivatives into an already allocated
// Jacobian of the correct size, of type VerticalBlockMatrix.
// The JacobianMap provides a mapping from keys to the underlying blocks.
class JacobianMap {
private:
typedef FastVector<Key> Keys;
const FastVector<Key>& keys_;
VerticalBlockMatrix& Ab_;
public:
/// Construct a JacobianMap for writing into a VerticalBlockMatrix Ab
JacobianMap(const Keys& keys, VerticalBlockMatrix& Ab) :
keys_(keys), Ab_(Ab) {
}
/// Access blocks of via key
VerticalBlockMatrix::Block operator()(Key key) {
Keys::const_iterator it = std::find(keys_.begin(), keys_.end(), key);
DenseIndex block = it - keys_.begin();
return Ab_(block);
}
};
} // namespace internal
} // namespace gtsam

5
gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
View File

@ -265,9 +265,10 @@ TEST(AdaptAutoDiff, Snavely) {
typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor;
Expression<Point2> expression(Adaptor(), P, X);
#ifdef GTSAM_USE_QUATERNIONS
EXPECT_LONGS_EQUAL(400,expression.traceSize()); // Todo, should be zero
EXPECT_LONGS_EQUAL(384,expression.traceSize()); // Todo, should be zero
#else
EXPECT_LONGS_EQUAL(432,expression.traceSize()); // Todo, should be zero
EXPECT_LONGS_EQUAL(sizeof(internal::BinaryExpression<Point2, Camera, Point3>::Record),
expression.traceSize()); // Todo, should be zero
#endif
set<Key> expected = list_of(1)(2);
EXPECT(expected == expression.keys());

10
gtsam/nonlinear/tests/testCallRecord.cpp
View File

@ -18,7 +18,7 @@
* @brief unit tests for CallRecord class
*/
#include <gtsam/nonlinear/CallRecord.h>
#include <gtsam/nonlinear/internal/CallRecord.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
@ -32,7 +32,7 @@ static const int Cols = 3;
int dynamicIfAboveMax(int i){
if(i > CallRecordMaxVirtualStaticRows){
if(i > internal::CallRecordMaxVirtualStaticRows){
return Eigen::Dynamic;
}
else return i;
@ -80,13 +80,13 @@ struct Record: public internal::CallRecordImplementor<Record, Cols> {
}
void print(const std::string& indent) const {
}
void startReverseAD4(JacobianMap& jacobians) const {
void startReverseAD4(internal::JacobianMap& jacobians) const {
}
mutable CallConfig cc;
private:
template<typename SomeMatrix>
void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
void reverseAD4(const SomeMatrix & dFdT, internal::JacobianMap& jacobians) const {
cc.compTimeRows = SomeMatrix::RowsAtCompileTime;
cc.compTimeCols = SomeMatrix::ColsAtCompileTime;
cc.runTimeRows = dFdT.rows();
@ -97,7 +97,7 @@ struct Record: public internal::CallRecordImplementor<Record, Cols> {
friend struct internal::CallRecordImplementor;
};
JacobianMap & NJM= *static_cast<JacobianMap *>(NULL);
internal::JacobianMap & NJM= *static_cast<internal::JacobianMap *>(NULL);
/* ************************************************************************* */
typedef Eigen::Matrix<double, Eigen::Dynamic, Cols> DynRowMat;

39
gtsam/nonlinear/tests/testExecutionTrace.cpp Normal file
View File

@ -0,0 +1,39 @@
/* ----------------------------------------------------------------------------
* 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)
* See LICENSE for the license information
* -------------------------------1------------------------------------------- */
/**
* @file testExecutionTrace.cpp
* @date May 11, 2015
* @author Frank Dellaert
* @brief unit tests for Expression internals
*/
#include <gtsam/nonlinear/internal/ExecutionTrace.h>
#include <gtsam/geometry/Point2.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
// Constant
TEST(ExecutionTrace, construct) {
internal::ExecutionTrace<Point2> trace;
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

51
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -42,7 +42,7 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3);
/* ************************************************************************* */
// Constant
TEST(Expression, constant) {
TEST(Expression, Constant) {
Expression<Rot3> R(someR);
Values values;
Rot3 actual = R.value(values);
@ -68,24 +68,27 @@ TEST(Expression, Leaves) {
Point3 somePoint(1, 2, 3);
values.insert(Symbol('p', 10), somePoint);
std::vector<Expression<Point3> > points = createUnknowns<Point3>(10, 'p', 1);
EXPECT(assert_equal(somePoint,points.back().value(values)));
EXPECT(assert_equal(somePoint, points.back().value(values)));
}
/* ************************************************************************* */
// Unary(Leaf)
namespace unary {
Point2 f0(const Point3& p, OptionalJacobian<2,3> H) {
Point2 f1(const Point3& p, OptionalJacobian<2, 3> H) {
return Point2();
}
double f2(const Point3& p, OptionalJacobian<1, 3> H) {
return 0.0;
}
Vector f3(const Point3& p, OptionalJacobian<Eigen::Dynamic, 3> H) {
return p.vector();
}
Expression<Point3> p(1);
set<Key> expected = list_of(1);
}
TEST(Expression, Unary0) {
TEST(Expression, Unary1) {
using namespace unary;
Expression<Point2> e(f0, p);
Expression<Point2> e(f1, p);
EXPECT(expected == e.keys());
}
TEST(Expression, Unary2) {
@ -94,6 +97,12 @@ TEST(Expression, Unary2) {
EXPECT(expected == e.keys());
}
/* ************************************************************************* */
// Unary(Leaf), dynamic
TEST(Expression, Unary3) {
using namespace unary;
// Expression<Vector> e(f3, p);
}
/* ************************************************************************* */
//Nullary Method
TEST(Expression, NullaryMethod) {
@ -118,7 +127,7 @@ TEST(Expression, NullaryMethod) {
EXPECT(actual == sqrt(50));
Matrix expected(1, 3);
expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0);
EXPECT(assert_equal(expected,H[0]));
EXPECT(assert_equal(expected, H[0]));
}
/* ************************************************************************* */
// Binary(Leaf,Leaf)
@ -149,12 +158,12 @@ TEST(Expression, BinaryKeys) {
TEST(Expression, BinaryDimensions) {
map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3);
binary::p_cam.dims(actual);
EXPECT(actual==expected);
EXPECT(actual == expected);
}
/* ************************************************************************* */
// dimensions
TEST(Expression, BinaryTraceSize) {
typedef BinaryExpression<Point3, Pose3, Point3> Binary;
typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary;
size_t expectedTraceSize = sizeof(Binary::Record);
EXPECT_LONGS_EQUAL(expectedTraceSize, binary::p_cam.traceSize());
}
@ -180,17 +189,26 @@ TEST(Expression, TreeKeys) {
TEST(Expression, TreeDimensions) {
map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3)(3, 5);
tree::uv_hat.dims(actual);
EXPECT(actual==expected);
EXPECT(actual == expected);
}
/* ************************************************************************* */
// TraceSize
TEST(Expression, TreeTraceSize) {
typedef UnaryExpression<Point2, Point3> Unary;
typedef BinaryExpression<Point3, Pose3, Point3> Binary1;
typedef BinaryExpression<Point2, Point2, Cal3_S2> Binary2;
size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary1::Record)
+ sizeof(Binary2::Record);
EXPECT_LONGS_EQUAL(expectedTraceSize, tree::uv_hat.traceSize());
typedef internal::BinaryExpression<Point3, Pose3, Point3> Binary1;
EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Binary1::Record)),
tree::p_cam.traceSize());
typedef internal::UnaryExpression<Point2, Point3> Unary;
EXPECT_LONGS_EQUAL(
internal::upAligned(sizeof(Unary::Record)) + tree::p_cam.traceSize(),
tree::projection.traceSize());
EXPECT_LONGS_EQUAL(0, tree::K.traceSize());
typedef internal::BinaryExpression<Point2, Cal3_S2, Point2> Binary2;
EXPECT_LONGS_EQUAL(
internal::upAligned(sizeof(Binary2::Record)) + tree::K.traceSize()
+ tree::projection.traceSize(), tree::uv_hat.traceSize());
}
/* ************************************************************************* */
@ -234,7 +252,8 @@ TEST(Expression, compose3) {
/* ************************************************************************* */
// Test with ternary function
Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) {
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2,
OptionalJacobian<3, 3> H3) {
// return dummy derivatives (not correct, but that's ok for testing here)
if (H1)
*H1 = eye(3);

45
gtsam/nonlinear/tests/testExpressionFactor.cpp
View File

@ -169,7 +169,7 @@ static Point2 myUncal(const Cal3_S2& K, const Point2& p,
// Binary(Leaf,Leaf)
TEST(ExpressionFactor, Binary) {
typedef BinaryExpression<Point2, Cal3_S2, Point2> Binary;
typedef internal::BinaryExpression<Point2, Cal3_S2, Point2> Binary;
Cal3_S2_ K_(1);
Point2_ p_(2);
@ -184,11 +184,15 @@ TEST(ExpressionFactor, Binary) {
EXPECT_LONGS_EQUAL(8, sizeof(double));
EXPECT_LONGS_EQUAL(16, sizeof(Point2));
EXPECT_LONGS_EQUAL(40, sizeof(Cal3_S2));
EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace<Point2>));
EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace<Cal3_S2>));
EXPECT_LONGS_EQUAL(2*5*8, sizeof(Jacobian<Point2,Cal3_S2>::type));
EXPECT_LONGS_EQUAL(2*2*8, sizeof(Jacobian<Point2,Point2>::type));
size_t expectedRecordSize = 16 + 16 + 40 + 2 * 16 + 80 + 32;
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
@ -197,8 +201,8 @@ TEST(ExpressionFactor, Binary) {
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
ExecutionTraceStorage traceStorage[size];
ExecutionTrace<Point2> trace;
internal::ExecutionTraceStorage traceStorage[size];
internal::ExecutionTrace<Point2> trace;
Point2 value = binary.traceExecution(values, trace, traceStorage);
EXPECT(assert_equal(Point2(),value, 1e-9));
// trace.print();
@ -212,9 +216,8 @@ TEST(ExpressionFactor, Binary) {
// Check matrices
boost::optional<Binary::Record*> r = trace.record<Binary::Record>();
CHECK(r);
EXPECT(
assert_equal(expected25, (Matrix) (*r)-> jacobian<Cal3_S2, 1>(), 1e-9));
EXPECT( assert_equal(expected22, (Matrix) (*r)->jacobian<Point2, 2>(), 1e-9));
EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9));
EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9));
}
/* ************************************************************************* */
// Unary(Binary(Leaf,Leaf))
@ -250,22 +253,22 @@ TEST(ExpressionFactor, Shallow) {
LONGS_EQUAL(3,dims[1]);
// traceExecution of shallow tree
typedef UnaryExpression<Point2, Point3> Unary;
typedef BinaryExpression<Point3, Pose3, Point3> Binary;
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(112, sizeof(Unary::Record));
EXPECT_LONGS_EQUAL(96, sizeof(Unary::Record));
#ifdef GTSAM_USE_QUATERNIONS
EXPECT_LONGS_EQUAL(352, sizeof(Binary::Record));
LONGS_EQUAL(112+352, expectedTraceSize);
LONGS_EQUAL(96+352, expectedTraceSize);
#else
EXPECT_LONGS_EQUAL(400, sizeof(Binary::Record));
LONGS_EQUAL(112+400, expectedTraceSize);
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);
ExecutionTraceStorage traceStorage[size];
ExecutionTrace<Point2> trace;
internal::ExecutionTraceStorage traceStorage[size];
internal::ExecutionTrace<Point2> trace;
Point2 value = expression.traceExecution(values, trace, traceStorage);
EXPECT(assert_equal(Point2(),value, 1e-9));
// trace.print();
@ -277,7 +280,7 @@ TEST(ExpressionFactor, Shallow) {
// Check matrices
boost::optional<Unary::Record*> r = trace.record<Unary::Record>();
CHECK(r);
EXPECT(assert_equal(expected23, (Matrix)(*r)->jacobian<Point3, 1>(), 1e-9));
EXPECT(assert_equal(expected23, (Matrix)(*r)->dTdA1, 1e-9));
// Linearization
ExpressionFactor<Point2> f2(model, measured, expression);
@ -327,7 +330,7 @@ TEST(ExpressionFactor, tree) {
boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
EXPECT( assert_equal(*expected, *gf2, 1e-9));
TernaryExpression<Point2, Pose3, Point3, Cal3_S2>::Function fff = project6;
Expression<Point2>::TernaryFunction<Pose3, Point3, Cal3_S2>::type fff = project6;
// Try ternary version
ExpressionFactor<Point2> f3(model, measured, project3(x, p, K));

14
gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp
View File

@ -88,11 +88,13 @@ int main(int argc, char** argv){
}
Values initial_pose_values = initial_estimate.filter<Pose3>();
if(output_poses){
init_pose3Out.open(init_poseOutput.c_str(),ios::out);
for(int i = 1; i<=initial_pose_values.size(); i++){
init_pose3Out << i << " " << initial_pose_values.at<Pose3>(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0,
" ", " ")) << endl;
if (output_poses) {
init_pose3Out.open(init_poseOutput.c_str(), ios::out);
for (size_t i = 1; i <= initial_pose_values.size(); i++) {
init_pose3Out
<< i << " "
<< initial_pose_values.at<Pose3>(Symbol('x', i)).matrix().format(
Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl;
}
}
@ -141,7 +143,7 @@ int main(int argc, char** argv){
if(output_poses){
pose3Out.open(poseOutput.c_str(),ios::out);
for(int i = 1; i<=pose_values.size(); i++){
for(size_t i = 1; i<=pose_values.size(); i++){
pose3Out << i << " " << pose_values.at<Pose3>(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0,
" ", " ")) << endl;
}

247
gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp
View File

@ -1,247 +0,0 @@
/* ----------------------------------------------------------------------------
* 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)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testExpressionMeta.cpp
* @date October 14, 2014
* @author Frank Dellaert
* @brief Test meta-programming constructs for Expressions
*/
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
#include <algorithm>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
namespace mpl = boost::mpl;
#include <boost/mpl/assert.hpp>
#include <boost/mpl/equal.hpp>
template<class T> struct Incomplete;
// Check generation of FunctionalNode
typedef mpl::vector<Pose3, Point3> MyTypes;
typedef FunctionalNode<Point2, MyTypes>::type Generated;
//Incomplete<Generated> incomplete;
// Try generating vectors of ExecutionTrace
typedef mpl::vector<ExecutionTrace<Pose3>, ExecutionTrace<Point3> > ExpectedTraces;
typedef mpl::transform<MyTypes, ExecutionTrace<MPL::_1> >::type MyTraces;
BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces >));
template<class T>
struct MakeTrace {
typedef ExecutionTrace<T> type;
};
typedef mpl::transform<MyTypes, MakeTrace<MPL::_1> >::type MyTraces1;
BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces1 >));
// Try generating vectors of Expression types
typedef mpl::vector<Expression<Pose3>, Expression<Point3> > ExpectedExpressions;
typedef mpl::transform<MyTypes, Expression<MPL::_1> >::type Expressions;
BOOST_MPL_ASSERT((mpl::equal< ExpectedExpressions, Expressions >));
// Try generating vectors of Jacobian types
typedef mpl::vector<Matrix26, Matrix23> ExpectedJacobians;
typedef mpl::transform<MyTypes, Jacobian<Point2, MPL::_1> >::type Jacobians;
BOOST_MPL_ASSERT((mpl::equal< ExpectedJacobians, Jacobians >));
// Try accessing a Jacobian
typedef mpl::at_c<Jacobians, 1>::type Jacobian23; // base zero !
BOOST_MPL_ASSERT((boost::is_same< Matrix23, Jacobian23>));
/* ************************************************************************* */
// Boost Fusion includes allow us to create/access values from MPL vectors
#include <boost/fusion/include/mpl.hpp>
#include <boost/fusion/include/at_c.hpp>
// Create a value and access it
TEST(ExpressionFactor, JacobiansValue) {
using boost::fusion::at_c;
Matrix23 expected;
Jacobians jacobians;
at_c<1>(jacobians) << 1, 2, 3, 4, 5, 6;
Matrix23 actual = at_c<1>(jacobians);
CHECK(actual.cols() == expected.cols());
CHECK(actual.rows() == expected.rows());
}
/* ************************************************************************* */
// Test out polymorphic transform
#include <boost/fusion/include/make_vector.hpp>
#include <boost/fusion/include/transform.hpp>
#include <boost/utility/result_of.hpp>
struct triple {
template<class > struct result; // says we will provide result
template<class F>
struct result<F(int)> {
typedef double type; // result for int argument
};
template<class F>
struct result<F(const int&)> {
typedef double type; // result for int argument
};
template<class F>
struct result<F(const double &)> {
typedef double type; // result for double argument
};
template<class F>
struct result<F(double)> {
typedef double type; // result for double argument
};
// actual function
template<typename T>
typename result<triple(T)>::type operator()(const T& x) const {
return (double) x;
}
};
// Test out polymorphic transform
TEST(ExpressionFactor, Triple) {
typedef boost::fusion::vector<int, double> IntDouble;
IntDouble H = boost::fusion::make_vector(1, 2.0);
// Only works if I use Double2
typedef boost::fusion::result_of::transform<IntDouble, triple>::type Result;
typedef boost::fusion::vector<double, double> Double2;
Double2 D = boost::fusion::transform(H, triple());
using boost::fusion::at_c;
DOUBLES_EQUAL(1.0, at_c<0>(D), 1e-9);
DOUBLES_EQUAL(2.0, at_c<1>(D), 1e-9);
}
/* ************************************************************************* */
#include <boost/fusion/include/invoke.hpp>
#include <boost/functional/value_factory.hpp>
#include <boost/fusion/functional/adapter/fused.hpp>
#include <boost/fusion/adapted/mpl.hpp>
// Test out invoke
TEST(ExpressionFactor, Invoke) {
EXPECT_LONGS_EQUAL(2, invoke(plus<int>(),boost::fusion::make_vector(1,1)));
// Creating a Pose3 (is there another way?)
boost::fusion::vector<Rot3, Point3> pair;
Pose3 pose = boost::fusion::invoke(boost::value_factory<Pose3>(), pair);
}
/* ************************************************************************* */
// debug const issue (how to make read/write arguments for invoke)
struct test {
typedef void result_type;
void operator()(int& a, int& b) const {
a = 6;
b = 7;
}
};
TEST(ExpressionFactor, ConstIssue) {
int a, b;
boost::fusion::invoke(test(),
boost::fusion::make_vector(boost::ref(a), boost::ref(b)));
LONGS_EQUAL(6, a);
LONGS_EQUAL(7, b);
}
/* ************************************************************************* */
// Test out invoke on a given GTSAM function
// then construct prototype for it's derivatives
TEST(ExpressionFactor, InvokeDerivatives) {
// This is the method in Pose3:
// Point3 transform_to(const Point3& p) const;
// Point3 transform_to(const Point3& p,
// boost::optional<Matrix36&> Dpose, boost::optional<Matrix3&> Dpoint) const;
// Let's assign it it to a boost function object
// cast is needed because Pose3::transform_to is overloaded
// typedef boost::function<Point3(const Pose3&, const Point3&)> F;
// F f = static_cast<Point3 (Pose3::*)(
// const Point3&) const >(&Pose3::transform_to);
//
// // Create arguments
// Pose3 pose;
// Point3 point;
// typedef boost::fusion::vector<Pose3, Point3> Arguments;
// Arguments args = boost::fusion::make_vector(pose, point);
//
// // Create fused function (takes fusion vector) and call it
// boost::fusion::fused<F> g(f);
// Point3 actual = g(args);
// CHECK(assert_equal(point,actual));
//
// // We can *immediately* do this using invoke
// Point3 actual2 = boost::fusion::invoke(f, args);
// CHECK(assert_equal(point,actual2));
// Now, let's create the optional Jacobian arguments
typedef Point3 T;
typedef boost::mpl::vector<Pose3, Point3> TYPES;
typedef boost::mpl::transform<TYPES, MakeOptionalJacobian<T, MPL::_1> >::type Optionals;
// Unfortunately this is moot: we need a pointer to a function with the
// optional derivatives; I don't see a way of calling a function that we
// did not get access to by the caller passing us a pointer.
// Let's test below whether we can have a proxy object
}
struct proxy {
typedef Point3 result_type;
Point3 operator()(const Pose3& pose, const Point3& point) const {
return pose.transform_to(point);
}
Point3 operator()(const Pose3& pose, const Point3& point,
OptionalJacobian<3,6> Dpose,
OptionalJacobian<3,3> Dpoint) const {
return pose.transform_to(point, Dpose, Dpoint);
}
};
TEST(ExpressionFactor, InvokeDerivatives2) {
// without derivatives
Pose3 pose;
Point3 point;
Point3 actual = boost::fusion::invoke(proxy(),
boost::fusion::make_vector(pose, point));
CHECK(assert_equal(point,actual));
// with derivatives, does not work, const issue again
Matrix36 Dpose;
Matrix3 Dpoint;
Point3 actual2 = boost::fusion::invoke(proxy(),
boost::fusion::make_vector(pose, point, boost::ref(Dpose),
boost::ref(Dpoint)));
CHECK(assert_equal(point,actual2));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

4
gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp
View File

@ -73,9 +73,7 @@ TEST( InvDepthFactorVariant1, optimize) {
// Optimize the graph to recover the actual landmark position
LevenbergMarquardtParams params;
Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
Vector6 actual = result.at<Vector6>(landmarkKey);
// Vector6 actual = result.at<Vector6>(landmarkKey);
// values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
// result.at<Pose3>(poseKey1).print("Pose1 After:\n");
// pose1.print("Pose1 Truth:\n");

4
wrap/tests/testWrap.cpp
View File

@ -41,10 +41,6 @@ static string topdir = "TOPSRCDIR_NOT_CONFIGURED"; // If TOPSRCDIR is not define
typedef vector<string> strvec;
// NOTE: as this path is only used to generate makefiles, it is hardcoded here for testing
// In practice, this path will be an absolute system path, which makes testing it more annoying
static const std::string headerPath = "/not_really_a_real_path/borg/gtsam/wrap";
/* ************************************************************************* */
TEST( wrap, ArgumentList ) {
ArgumentList args;

3
wrap/utilities.cpp
View File

@ -91,7 +91,8 @@ bool files_equal(const string& expected, const string& actual, bool skipheader)
cerr << "<<< DIFF OUTPUT (if none, white-space differences only):\n";
stringstream command;
command << "diff --ignore-all-space " << expected << " " << actual << endl;
system(command.str().c_str());
if (system(command.str().c_str())<0)
throw "command '" + command.str() + "' failed";
cerr << ">>>\n";
}
return equal;