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

Merged in feature/BAD_addressIssue151 (pull request #38) 

first proposal to address issue #151
release/4.3a0
Frank Dellaert 2014-11-22 21:49:43 +01:00
parent 018e66be7f d00aeb7e70
commit 22fa6d3f70
7 changed files with 455 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
.cproject
View File

@ -600,6 +600,7 @@
</target> </target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget> <buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -607,6 +608,7 @@
</target> </target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget> <buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -654,6 +656,7 @@
</target> </target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget> <buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -661,6 +664,7 @@
</target> </target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget> <buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -668,6 +672,7 @@
</target> </target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget> <buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -683,6 +688,7 @@
</target> </target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget> <buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1114,6 +1120,7 @@
</target> </target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget> <buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1343,6 +1350,46 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1425,7 +1472,6 @@
</target> </target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget> <buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1465,7 +1511,6 @@
</target> </target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget> <buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1473,7 +1518,6 @@
</target> </target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget> <buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1487,46 +1531,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -1784,6 +1788,7 @@
</target> </target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget> <buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1791,6 +1796,7 @@
</target> </target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget> <buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1798,6 +1804,7 @@
</target> </target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget> <buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1805,6 +1812,7 @@
</target> </target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget> <buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2619,6 +2627,7 @@
</target> </target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget> <buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2626,6 +2635,7 @@
</target> </target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget> <buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2633,6 +2643,7 @@
</target> </target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget> <buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2742,6 +2753,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testCallRecord.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testCallRecord.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -3152,7 +3171,6 @@
</target> </target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget> <buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>

2
gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file Pose2SLAMExample.cpp * @file Pose2SLAMExampleExpressions.cpp
* @brief Expressions version of Pose2SLAMExample.cpp * @brief Expressions version of Pose2SLAMExample.cpp
* @date Oct 2, 2014 * @date Oct 2, 2014
* @author Frank Dellaert * @author Frank Dellaert

190
gtsam_unstable/nonlinear/CallRecord.h Normal file
View File

@ -0,0 +1,190 @@
/* ----------------------------------------------------------------------------
* 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 CallRecord.h
* @date November 21, 2014
* @author Frank Dellaert
* @author Paul Furgale
* @author Hannes Sommer
* @brief Internals for Expression.h, not for general consumption
*/
#pragma once
#include <gtsam/base/VerticalBlockMatrix.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/Matrix.h>
#include <boost/mpl/transform.hpp>
namespace gtsam {
class JacobianMap;
// forward declaration
//-----------------------------------------------------------------------------
/**
* MaxVirtualStaticRows defines how many separate virtual reverseAD with specific
* static rows (1..MaxVirtualStaticRows) methods will be part of the CallRecord interface.
*/
const int MaxVirtualStaticRows = 4;
namespace internal {
/**
* ConvertToDynamicIf converts to a dense matrix with dynamic rows iff
* ConvertToDynamicRows (colums stay as they are) otherwise
* it just passes dense Eigen matrices through.
*/
template<bool ConvertToDynamicRows>
struct ConvertToDynamicRowsIf {
template<typename Derived>
static Eigen::Matrix<double, Eigen::Dynamic, Derived::ColsAtCompileTime> convert(
const Eigen::MatrixBase<Derived> & x) {
return x;
}
};
template<>
struct ConvertToDynamicRowsIf<false> {
template<int Rows, int Cols>
static const Eigen::Matrix<double, Rows, Cols> & convert(
const Eigen::Matrix<double, Rows, Cols> & x) {
return x;
}
};
/**
* Recursive definition of an interface having several purely
* virtual _reverseAD(const Eigen::Matrix<double, Rows, Cols> &, JacobianMap&)
* with Rows in 1..MaxSupportedStaticRows
*/
template<int MaxSupportedStaticRows, int Cols>
struct ReverseADInterface: ReverseADInterface<MaxSupportedStaticRows - 1, Cols> {
using ReverseADInterface<MaxSupportedStaticRows - 1, Cols>::_reverseAD;
virtual void _reverseAD(
const Eigen::Matrix<double, MaxSupportedStaticRows, Cols> & dFdT,
JacobianMap& jacobians) const = 0;
};
template<int Cols>
struct ReverseADInterface<0, Cols> {
virtual void _reverseAD(
const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT,
JacobianMap& jacobians) const = 0;
virtual void _reverseAD(const Matrix & dFdT,
JacobianMap& jacobians) const = 0;
};
/**
* ReverseADImplementor is a utility class used by CallRecordImplementor to
* implementing the recursive ReverseADInterface interface.
*/
template<typename Derived, int MaxSupportedStaticRows, int Cols>
struct ReverseADImplementor: ReverseADImplementor<Derived,
MaxSupportedStaticRows - 1, Cols> {
private:
using ReverseADImplementor<Derived, MaxSupportedStaticRows - 1, Cols>::_reverseAD;
virtual void _reverseAD(
const Eigen::Matrix<double, MaxSupportedStaticRows, Cols> & dFdT,
JacobianMap& jacobians) const {
static_cast<const Derived *>(this)->reverseAD(dFdT, jacobians);
}
friend struct internal::ReverseADImplementor<Derived,
MaxSupportedStaticRows + 1, Cols>;
};
template<typename Derived, int Cols>
struct ReverseADImplementor<Derived, 0, Cols> : virtual internal::ReverseADInterface<
MaxVirtualStaticRows, Cols> {
private:
using internal::ReverseADInterface<MaxVirtualStaticRows, Cols>::_reverseAD;
const Derived & derived() const {
return static_cast<const Derived&>(*this);
}
virtual void _reverseAD(
const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT,
JacobianMap& jacobians) const {
derived().reverseAD(dFdT, jacobians);
}
virtual void _reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const {
derived().reverseAD(dFdT, jacobians);
}
friend struct internal::ReverseADImplementor<Derived, 1, Cols>;
};
} // namespace internal
/**
* The CallRecord class stores the Jacobians of applying a function
* with respect to each of its arguments. It also stores an execution trace
* (defined below) for each of its arguments.
*
* It is implemented in the function-style ExpressionNode's nested Record class below.
*/
template<int Cols>
struct CallRecord: virtual private internal::ReverseADInterface<
MaxVirtualStaticRows, Cols> {
inline void print(const std::string& indent) const {
_print(indent);
}
inline void startReverseAD(JacobianMap& jacobians) const {
_startReverseAD(jacobians);
}
template<int Rows>
inline void reverseAD(const Eigen::Matrix<double, Rows, Cols> & dFdT,
JacobianMap& jacobians) const {
_reverseAD(
internal::ConvertToDynamicRowsIf<(Rows > MaxVirtualStaticRows)>::convert(
dFdT), jacobians);
}
inline void reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const {
_reverseAD(dFdT, jacobians);
}
virtual ~CallRecord() {
}
private:
virtual void _print(const std::string& indent) const = 0;
virtual void _startReverseAD(JacobianMap& jacobians) const = 0;
using internal::ReverseADInterface<MaxVirtualStaticRows, Cols>::_reverseAD;
};
namespace internal {
/**
* The CallRecordImplementor implements the CallRecord interface for a Derived class by
* delegating to its corresponding (templated) non-virtual methods.
*/
template<typename Derived, int Cols>
struct CallRecordImplementor: public CallRecord<Cols>,
private ReverseADImplementor<Derived, MaxVirtualStaticRows, Cols> {
private:
const Derived & derived() const {
return static_cast<const Derived&>(*this);
}
virtual void _print(const std::string& indent) const {
derived().print(indent);
}
virtual void _startReverseAD(JacobianMap& jacobians) const {
derived().startReverseAD(jacobians);
}
template<typename D, int R, int C> friend struct ReverseADImplementor;
};
} // namespace internal
} // gtsam

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

@ -19,29 +19,21 @@
#pragma once #pragma once
#include <gtsam_unstable/nonlinear/CallRecord.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h> #include <gtsam/base/Manifold.h>
#include <gtsam/base/VerticalBlockMatrix.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <boost/bind.hpp> #include <boost/bind.hpp>
// template meta-programming headers // template meta-programming headers
#include <boost/mpl/vector.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/pop_front.hpp>
#include <boost/mpl/fold.hpp> #include <boost/mpl/fold.hpp>
#include <boost/mpl/empty_base.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/at.hpp>
namespace MPL = boost::mpl::placeholders; namespace MPL = boost::mpl::placeholders;
#include <new> // for placement new #include <map>
class ExpressionFactorBinaryTest; class ExpressionFactorBinaryTest;
// Forward declare for testing // Forward declare for testing
@ -71,29 +63,6 @@ public:
} }
}; };
//-----------------------------------------------------------------------------
/**
* The CallRecord class stores the Jacobians of applying a function
* with respect to each of its arguments. It also stores an executation trace
* (defined below) for each of its arguments.
*
* It is sub-classed in the function-style ExpressionNode sub-classes below.
*/
template<int COLS>
struct CallRecord {
static size_t const N = 0;
virtual void print(const std::string& indent) const {
}
virtual void startReverseAD(JacobianMap& jacobians) const {
}
virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const {
}
typedef Eigen::Matrix<double, 2, COLS> Jacobian2T;
virtual void reverseAD2(const Jacobian2T& dFdT,
JacobianMap& jacobians) const {
}
};
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians /// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians
template<int ROWS, int COLS> template<int ROWS, int COLS>
@ -101,10 +70,10 @@ void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA,
JacobianMap& jacobians, Key key) { JacobianMap& jacobians, Key key) {
jacobians(key).block<ROWS, COLS>(0, 0) += dTdA; // block makes HUGE difference jacobians(key).block<ROWS, COLS>(0, 0) += dTdA; // block makes HUGE difference
} }
/// Handle Leaf Case for Dynamic Matrix type (slower) /// Handle Leaf Case for Dynamic ROWS Matrix type (slower)
template<> template<int COLS>
inline void handleLeafCase( inline void handleLeafCase(
const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& dTdA, const Eigen::Matrix<double, Eigen::Dynamic, COLS>& dTdA,
JacobianMap& jacobians, Key key) { JacobianMap& jacobians, Key key) {
jacobians(key) += dTdA; jacobians(key) += dTdA;
} }
@ -193,45 +162,19 @@ public:
content.ptr->startReverseAD(jacobians); content.ptr->startReverseAD(jacobians);
} }
// Either add to Jacobians (Leaf) or propagate (Function) // Either add to Jacobians (Leaf) or propagate (Function)
void reverseAD(const Matrix& dTdA, JacobianMap& jacobians) const { template<typename DerivedMatrix>
void reverseAD(const Eigen::MatrixBase<DerivedMatrix> & dTdA,
JacobianMap& jacobians) const {
if (kind == Leaf) if (kind == Leaf)
handleLeafCase(dTdA, jacobians, content.key); handleLeafCase(dTdA.eval(), jacobians, content.key);
else if (kind == Function) else if (kind == Function)
content.ptr->reverseAD(dTdA, jacobians); content.ptr->reverseAD(dTdA.eval(), jacobians);
}
// Either add to Jacobians (Leaf) or propagate (Function)
typedef Eigen::Matrix<double, 2, Dim> Jacobian2T;
void reverseAD2(const Jacobian2T& 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 /// Define type so we can apply it as a meta-function
typedef ExecutionTrace<T> type; typedef ExecutionTrace<T> type;
}; };
/// Primary template calls the generic Matrix reverseAD pipeline
template<size_t ROWS, class A>
struct Select {
typedef Eigen::Matrix<double, ROWS, traits::dimension<A>::value> Jacobian;
static void reverseAD(const ExecutionTrace<A>& trace, const Jacobian& dTdA,
JacobianMap& jacobians) {
trace.reverseAD(dTdA, jacobians);
}
};
/// Partially specialized template calls the 2-dimensional output version
template<class A>
struct Select<2, A> {
typedef Eigen::Matrix<double, 2, traits::dimension<A>::value> Jacobian;
static void reverseAD(const ExecutionTrace<A>& trace, const Jacobian& dTdA,
JacobianMap& jacobians) {
trace.reverseAD2(dTdA, jacobians);
}
};
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/** /**
* Expression node. The superclass for objects that do the heavy lifting * Expression node. The superclass for objects that do the heavy lifting
@ -479,8 +422,15 @@ template<class T>
struct FunctionalBase: ExpressionNode<T> { struct FunctionalBase: ExpressionNode<T> {
static size_t const N = 0; // number of arguments static size_t const N = 0; // number of arguments
typedef CallRecord<traits::dimension<T>::value> Record; struct Record {
void print(const std::string& indent) const {
}
void startReverseAD(JacobianMap& jacobians) const {
}
template<typename SomeMatrix>
void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
}
};
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
void trace(const Values& values, Record* record, char*& raw) const { void trace(const Values& values, Record* record, char*& raw) const {
// base case: does not do anything // base case: does not do anything
@ -539,7 +489,7 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
typedef JacobianTrace<T, A, N> This; typedef JacobianTrace<T, A, N> This;
/// Print to std::cout /// Print to std::cout
virtual void print(const std::string& indent) const { void print(const std::string& indent) const {
Base::Record::print(indent); Base::Record::print(indent);
static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
std::cout << This::dTdA.format(matlab) << std::endl; std::cout << This::dTdA.format(matlab) << std::endl;
@ -547,25 +497,18 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
} }
/// Start the reverse AD process /// Start the reverse AD process
virtual void startReverseAD(JacobianMap& jacobians) const { void startReverseAD(JacobianMap& jacobians) const {
Base::Record::startReverseAD(jacobians); Base::Record::startReverseAD(jacobians);
Select<traits::dimension<T>::value, A>::reverseAD(This::trace, This::dTdA, This::trace.reverseAD(This::dTdA, jacobians);
jacobians);
} }
/// Given df/dT, multiply in dT/dA and continue reverse AD process /// Given df/dT, multiply in dT/dA and continue reverse AD process
virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { template<int Rows, int Cols>
void reverseAD(const Eigen::Matrix<double, Rows, Cols> & dFdT,
JacobianMap& jacobians) const {
Base::Record::reverseAD(dFdT, jacobians); Base::Record::reverseAD(dFdT, jacobians);
This::trace.reverseAD(dFdT * This::dTdA, jacobians); This::trace.reverseAD(dFdT * This::dTdA, jacobians);
} }
/// Version specialized to 2-dimensional output
typedef Eigen::Matrix<double, 2, traits::dimension<T>::value> Jacobian2T;
virtual void reverseAD2(const Jacobian2T& dFdT,
JacobianMap& jacobians) const {
Base::Record::reverseAD2(dFdT, jacobians);
This::trace.reverseAD2(dFdT * This::dTdA, jacobians);
}
}; };
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD
@ -619,8 +562,16 @@ struct FunctionalNode {
return static_cast<Argument<T, A, N> const &>(*this).expression; return static_cast<Argument<T, A, N> const &>(*this).expression;
} }
/// Provide convenience access to Record storage /// Provide convenience access to Record storage and implement
struct Record: public Base::Record { /// the virtual function based interface of CallRecord using the CallRecordImplementor
struct Record: public internal::CallRecordImplementor<Record,
traits::dimension<T>::value>, public Base::Record {
using Base::Record::print;
using Base::Record::startReverseAD;
using Base::Record::reverseAD;
virtual ~Record() {
}
/// Access Value /// Access Value
template<class A, size_t N> template<class A, size_t N>
@ -633,7 +584,6 @@ struct FunctionalNode {
typename Jacobian<T, A>::type& jacobian() { typename Jacobian<T, A>::type& jacobian() {
return static_cast<JacobianTrace<T, A, N>&>(*this).dTdA; return static_cast<JacobianTrace<T, A, N>&>(*this).dTdA;
} }
}; };
/// Construct an execution trace for reverse AD /// Construct an execution trace for reverse AD

2
gtsam_unstable/nonlinear/Expression.h
View File

@ -116,7 +116,7 @@ public:
return root_->traceSize(); return root_->traceSize();
} }
/// trace execution, very unsafe, for testing purposes only /// trace execution, very unsafe, for testing purposes only //TODO this is not only used for testing, but in value() below!
T traceExecution(const Values& values, ExecutionTrace<T>& trace, T traceExecution(const Values& values, ExecutionTrace<T>& trace,
char* raw) const { char* raw) const {
return root_->traceExecution(values, trace, raw); return root_->traceExecution(values, trace, raw);

165
gtsam_unstable/nonlinear/tests/testCallRecord.cpp Normal file
View File

@ -0,0 +1,165 @@
/* ----------------------------------------------------------------------------
* 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 testCallRecord.cpp
* @date November 21, 2014
* @author Frank Dellaert
* @author Paul Furgale
* @author Hannes Sommer
* @brief unit tests for CallRecord class
*/
#include <gtsam_unstable/nonlinear/CallRecord.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
static const int Cols = 3;
int dynamicIfAboveMax(int i){
if(i > MaxVirtualStaticRows){
return Eigen::Dynamic;
}
else return i;
}
struct CallConfig {
int compTimeRows;
int compTimeCols;
int runTimeRows;
int runTimeCols;
CallConfig() {}
CallConfig(int rows, int cols):
compTimeRows(dynamicIfAboveMax(rows)),
compTimeCols(cols),
runTimeRows(rows),
runTimeCols(cols)
{
}
CallConfig(int compTimeRows, int compTimeCols, int runTimeRows, int runTimeCols):
compTimeRows(compTimeRows),
compTimeCols(compTimeCols),
runTimeRows(runTimeRows),
runTimeCols(runTimeCols)
{
}
bool equals(const CallConfig & c, double /*tol*/) const {
return
this->compTimeRows == c.compTimeRows &&
this->compTimeCols == c.compTimeCols &&
this->runTimeRows == c.runTimeRows &&
this->runTimeCols == c.runTimeCols;
}
void print(const std::string & prefix) const {
std::cout << prefix << "{" << compTimeRows << ", " << compTimeCols << ", " << runTimeRows << ", " << runTimeCols << "}\n" ;
}
};
struct Record: public internal::CallRecordImplementor<Record, Cols> {
virtual ~Record() {
}
void print(const std::string& indent) const {
}
void startReverseAD(JacobianMap& jacobians) const {
}
mutable CallConfig cc;
private:
template<typename SomeMatrix>
void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
cc.compTimeRows = SomeMatrix::RowsAtCompileTime;
cc.compTimeCols = SomeMatrix::ColsAtCompileTime;
cc.runTimeRows = dFdT.rows();
cc.runTimeCols = dFdT.cols();
}
template<typename Derived, int Rows, int OtherCols>
friend struct internal::ReverseADImplementor;
};
JacobianMap & NJM= *static_cast<JacobianMap *>(NULL);
/* ************************************************************************* */
typedef Eigen::Matrix<double, Eigen::Dynamic, Cols> DynRowMat;
TEST(CallRecord, virtualReverseAdDispatching) {
Record record;
{
const int Rows = 1;
record.CallRecord::reverseAD(Eigen::Matrix<double, Rows, Cols>(), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols))));
}
{
const int Rows = 2;
record.CallRecord::reverseAD(Eigen::Matrix<double, Rows, Cols>(), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols))));
}
{
const int Rows = 3;
record.CallRecord::reverseAD(Eigen::Matrix<double, Rows, Cols>(), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols))));
}
{
const int Rows = MaxVirtualStaticRows;
record.CallRecord::reverseAD(Eigen::Matrix<double, Rows, Cols>(), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols))));
}
{
const int Rows = MaxVirtualStaticRows + 1;
record.CallRecord::reverseAD(Eigen::Matrix<double, Rows, Cols>(), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols))));
}
{
const int Rows = MaxVirtualStaticRows + 2;
record.CallRecord::reverseAD(Eigen::Matrix<double, Rows, Cols>(), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols))));
}
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

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

@ -38,7 +38,6 @@ template<class T> struct Incomplete;
typedef mpl::vector<Pose3, Point3> MyTypes; typedef mpl::vector<Pose3, Point3> MyTypes;
typedef FunctionalNode<Point2, MyTypes>::type Generated; typedef FunctionalNode<Point2, MyTypes>::type Generated;
//Incomplete<Generated> incomplete; //Incomplete<Generated> incomplete;
BOOST_MPL_ASSERT((boost::is_same< Matrix2, Generated::Record::Jacobian2T >));
// Try generating vectors of ExecutionTrace // Try generating vectors of ExecutionTrace
typedef mpl::vector<ExecutionTrace<Pose3>, ExecutionTrace<Point3> > ExpectedTraces; typedef mpl::vector<ExecutionTrace<Pose3>, ExecutionTrace<Point3> > ExpectedTraces;