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

Merged in feature/expressions (pull request #45) 

Pull request for debugging Expression<double> issue
release/4.3a0
Frank Dellaert 2014-11-24 18:40:31 +01:00
parent f567f5a5dd db6c9ff378
commit ea02e577f5
12 changed files with 341 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
.cproject
View File

@ -592,6 +592,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>
@ -599,6 +600,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>
@ -646,6 +648,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>
@ -653,6 +656,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>
@ -660,6 +664,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>
@ -675,6 +680,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>
@ -1122,6 +1128,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>
@ -1351,6 +1358,46 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1433,7 +1480,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>
@ -1473,7 +1519,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>
@ -1481,7 +1526,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>
@ -1495,46 +1539,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1792,6 +1796,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>
@ -1799,6 +1804,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>
@ -1806,6 +1812,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>
@ -1813,6 +1820,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>
@ -2627,6 +2635,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>
@ -2634,6 +2643,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>
@ -2641,6 +2651,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>
@ -2758,6 +2769,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBasisDecompositions.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testBasisDecompositions.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">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -3168,7 +3187,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>

2
gtsam/base/Manifold.h
View File

@ -235,7 +235,7 @@ struct DefaultChart<double> {
static double retract(double origin, const vector& d) {
return origin + d[0];
}
static const int getDimension(double) {
static int getDimension(double) {
return 1;
}
};

12
gtsam/geometry/Point3.cpp
View File

@ -111,6 +111,18 @@ double Point3::norm(boost::optional<Matrix&> H) const {
return r;
}
/* ************************************************************************* */
double Point3::norm(boost::optional<Eigen::Matrix<double,1,3>&> H) const {
double r = norm();
if (H) {
if (fabs(r) > 1e-10)
*H << x_ / r, y_ / r, z_ / r;
else
*H << 1, 1, 1; // really infinity, why 1 ?
}
return r;
}
/* ************************************************************************* */
Point3 Point3::normalize(boost::optional<Matrix&> H) const {
Point3 normalized = *this / norm();

3
gtsam/geometry/Point3.h
View File

@ -186,6 +186,9 @@ namespace gtsam {
/** Distance of the point from the origin, with Jacobian */
double norm(boost::optional<Matrix&> H) const;
/** Distance of the point from the origin, with Jacobian */
double norm(boost::optional<Eigen::Matrix<double,1,3>&> H) const;
/** normalize, with optional Jacobian */
Point3 normalize(boost::optional<Matrix&> H = boost::none) const;

16
gtsam_unstable/nonlinear/CallRecord.h
View File

@ -46,7 +46,7 @@ namespace internal {
* it just passes dense Eigen matrices through.
*/
template<bool ConvertToDynamicRows>
struct ConvertToDynamicRowsIf {
struct ConvertToVirtualFunctionSupportedMatrixType {
template<typename Derived>
static Eigen::Matrix<double, Eigen::Dynamic, Derived::ColsAtCompileTime> convert(
const Eigen::MatrixBase<Derived> & x) {
@ -55,7 +55,13 @@ struct ConvertToDynamicRowsIf {
};
template<>
struct ConvertToDynamicRowsIf<false> {
struct ConvertToVirtualFunctionSupportedMatrixType<false> {
template<typename Derived>
static const Eigen::Matrix<double, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> convert(
const Eigen::MatrixBase<Derived> & x) {
return x;
}
// special treatment of matrices that don't need conversion
template<int Rows, int Cols>
static const Eigen::Matrix<double, Rows, Cols> & convert(
const Eigen::Matrix<double, Rows, Cols> & x) {
@ -143,11 +149,11 @@ struct CallRecord: virtual private internal::ReverseADInterface<
_startReverseAD(jacobians);
}
template<int Rows>
inline void reverseAD(const Eigen::Matrix<double, Rows, Cols> & dFdT,
template<typename Derived>
inline void reverseAD(const Eigen::MatrixBase<Derived> & dFdT,
JacobianMap& jacobians) const {
_reverseAD(
internal::ConvertToDynamicRowsIf<(Rows > MaxVirtualStaticRows)>::convert(
internal::ConvertToVirtualFunctionSupportedMatrixType<(Derived::RowsAtCompileTime > MaxVirtualStaticRows)>::convert(
dFdT), jacobians);
}

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

@ -64,18 +64,36 @@ public:
};
//-----------------------------------------------------------------------------
/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians
template<int ROWS, int COLS>
void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA,
JacobianMap& jacobians, Key key) {
jacobians(key).block<ROWS, COLS>(0, 0) += dTdA; // block makes HUGE difference
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;
}
};
}
/// Handle Leaf Case for Dynamic ROWS Matrix type (slower)
template<int COLS>
inline void handleLeafCase(
const Eigen::Matrix<double, Eigen::Dynamic, COLS>& dTdA,
/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians
template<typename Derived>
void handleLeafCase(const Eigen::MatrixBase<Derived>& dTdA,
JacobianMap& jacobians, Key key) {
jacobians(key) += dTdA;
internal::UseBlockIf<
Derived::RowsAtCompileTime != Eigen::Dynamic &&
Derived::ColsAtCompileTime != Eigen::Dynamic,
Derived>
::addToJacobian(dTdA, jacobians, key);
}
//-----------------------------------------------------------------------------
@ -166,9 +184,9 @@ public:
void reverseAD(const Eigen::MatrixBase<DerivedMatrix> & dTdA,
JacobianMap& jacobians) const {
if (kind == Leaf)
handleLeafCase(dTdA.eval(), jacobians, content.key);
handleLeafCase(dTdA, jacobians, content.key);
else if (kind == Function)
content.ptr->reverseAD(dTdA.eval(), jacobians);
content.ptr->reverseAD(dTdA, jacobians);
}
/// Define type so we can apply it as a meta-function

13
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -81,6 +81,8 @@ public:
*/
virtual Vector unwhitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const {
// TODO(PTF) Is this a place for custom charts?
DefaultChart<T> chart;
if (H) {
// H should be pre-allocated
assert(H->size()==size());
@ -95,18 +97,19 @@ public:
T value = expression_.value(x, map); // <<< Reverse AD happens here !
// Copy blocks into the vector of jacobians passed in
for (DenseIndex i = 0; i < size(); i++)
for (DenseIndex i = 0; i < static_cast<DenseIndex>(size()); i++)
H->at(i) = Ab(i);
return measurement_.localCoordinates(value);
return chart.local(measurement_, value);
} else {
const T& value = expression_.value(x);
return measurement_.localCoordinates(value);
return chart.local(measurement_, value);
}
}
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
// TODO(PTF) Is this a place for custom charts?
DefaultChart<T> chart;
// Only linearize if the factor is active
if (!active(x))
return boost::shared_ptr<JacobianFactor>();
@ -128,7 +131,7 @@ public:
// Evaluate error to get Jacobians and RHS vector b
T value = expression_.value(x, map); // <<< Reverse AD happens here !
Ab(size()).col(0) = -measurement_.localCoordinates(value);
Ab(size()).col(0) = -chart.local(measurement_, value);
// Whiten the corresponding system, Ab already contains RHS
Vector dummy(Dim);

26
gtsam_unstable/nonlinear/expressions.h Normal file
View File

@ -0,0 +1,26 @@
/**
* @file expressions.h
* @brief Common expressions, both linear and non-linear
* @date Nov 23, 2014
* @author Frank Dellaert
*/
#pragma once
#include <gtsam_unstable/nonlinear/Expression.h>
#include <boost/bind.hpp>
namespace gtsam {
// Generics
template<class T>
Expression<T> between(const Expression<T>& t1, const Expression<T>& t2) {
return Expression<T>(t1, &T::between, t2);
}
typedef Expression<double> double_;
typedef Expression<Vector3> Vector3_;
} // \namespace gtsam

119
gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp Normal file
View File

@ -0,0 +1,119 @@
/* ----------------------------------------------------------------------------
* 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 testBasisDecompositions.cpp
* @date November 23, 2014
* @author Frank Dellaert
* @brief unit tests for Basis Decompositions w Expressions
*/
#include <gtsam_unstable/nonlinear/expressions.h>
#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/list_of.hpp>
using boost::assign::list_of;
using namespace std;
using namespace gtsam;
noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
/// Fourier
template<int N>
class Fourier {
public:
typedef Eigen::Matrix<double, N, 1> Coefficients;
typedef Eigen::Matrix<double, 1, N> Jacobian;
private:
double x_;
Jacobian H_;
public:
/// Constructor
Fourier(double x) :
x_(x) {
H_(0, 0) = 1;
for (size_t i = 1; i < N; i += 2) {
H_(0, i) = cos(i * x);
H_(0, i + 1) = sin(i * x);
}
}
/// Given coefficients c, predict value for x
double operator()(const Coefficients& c, boost::optional<Jacobian&> H) {
if (H)
(*H) = H_;
return H_ * c;
}
};
//******************************************************************************
TEST(BasisDecompositions, Fourier) {
Fourier<3> fx(0);
Eigen::Matrix<double, 1, 3> expectedH, actualH;
Vector3 c(1.5661, 1.2717, 1.2717);
expectedH = numericalDerivative11<double, Vector3>(
boost::bind(&Fourier<3>::operator(), fx, _1, boost::none), c);
EXPECT_DOUBLES_EQUAL(c[0]+c[1], fx(c,actualH), 1e-9);
EXPECT(assert_equal((Matrix)expectedH, actualH));
}
//******************************************************************************
TEST(BasisDecompositions, FourierExpression) {
// Create linear factor graph
GaussianFactorGraph g;
Key key(1);
Vector3_ c(key);
for (size_t i = 0; i < 16; i++) {
double x = i * M_PI / 8, y = exp(sin(x) + cos(x));
// Manual JacobianFactor
Matrix A(1, 3);
A << 1, cos(x), sin(x);
Vector b(1);
b << y;
JacobianFactor f1(key, A, b, model);
// With ExpressionFactor
Expression<double> expression(Fourier<3>(x), c);
ExpressionFactor<double> f2(model, y, expression);
g.add(f1);
}
// Solve
VectorValues actual = g.optimize();
// Check
Vector3 expected(1.5661, 1.2717, 1.2717);
EXPECT(assert_equal((Vector) expected, actual.at(key),1e-4));
}
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************

79
gtsam_unstable/nonlinear/tests/testExpression.cpp
View File

@ -73,27 +73,84 @@ TEST(Expression, Leaves) {
}
/* ************************************************************************* */
// Unary(Leaf)
namespace unary {
Point2 f0(const Point3& p, boost::optional<Matrix23&> H) {
return Point2();
}
LieScalar f1(const Point3& p, boost::optional<Eigen::Matrix<double, 1, 3>&> H) {
return LieScalar(0.0);
}
double f2(const Point3& p, boost::optional<Eigen::Matrix<double, 1, 3>&> H) {
return 0.0;
}
Expression<Point3> p(1);
set<Key> expected = list_of(1);
}
TEST(Expression, Unary0) {
using namespace unary;
Expression<Point2> e(f0, p);
EXPECT(expected == e.keys());
}
TEST(Expression, Unary1) {
using namespace unary;
Expression<double> e(f1, p);
EXPECT(expected == e.keys());
}
TEST(Expression, Unary2) {
using namespace unary;
Expression<double> e(f2, p);
EXPECT(expected == e.keys());
}
/* ************************************************************************* */
//Nullary Method
TEST(Expression, NullaryMethod) {
//TEST(Expression, NullaryMethod) {
// Expression<Point3> p(67);
// Expression<LieScalar> norm(p, &Point3::norm);
// Values values;
// values.insert(67,Point3(3,4,5));
// Augmented<LieScalar> a = norm.augmented(values);
// EXPECT(a.value() == sqrt(50));
// JacobianMap expected;
// expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50));
// EXPECT(assert_equal(expected.at(67),a.jacobians().at(67)));
//}
// Create expression
Expression<Point3> p(67);
Expression<double> norm(p, &Point3::norm);
// Create Values
Values values;
values.insert(67, Point3(3, 4, 5));
// Pre-allocate JacobianMap
FastVector<Key> keys;
keys.push_back(67);
FastVector<int> dims;
dims.push_back(3);
VerticalBlockMatrix Ab(dims, 1);
JacobianMap map(keys, Ab);
// Get value and Jacobian
double actual = norm.value(values, map);
// Check all
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,Ab(0)));
}
/* ************************************************************************* */
// Binary(Leaf,Leaf)
namespace binary {
// Create leaves
double doubleF(const Pose3& pose, const Point3& point,
boost::optional<Eigen::Matrix<double, 1, 6>&> H1,
boost::optional<Eigen::Matrix<double, 1, 3>&> H2) {
return 0.0;
}
Expression<Pose3> x(1);
Expression<Point3> p(2);
Expression<Point3> p_cam(x, &Pose3::transform_to, p);
}
/* ************************************************************************* */
// Check that creating an expression to double compiles
TEST(Expression, BinaryToDouble) {
using namespace binary;
Expression<double> p_cam(doubleF, x, p);
}
/* ************************************************************************* */
// keys
TEST(Expression, BinaryKeys) {
set<Key> expected = list_of(1)(2);

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

@ -22,8 +22,6 @@
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>

10
gtsam_unstable/slam/expressions.h
View File

@ -7,20 +7,12 @@
#pragma once
#include <gtsam_unstable/nonlinear/Expression.h>
#include <gtsam_unstable/nonlinear/expressions.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <boost/bind.hpp>
namespace gtsam {
// Generics
template<class T>
Expression<T> between(const Expression<T>& t1, const Expression<T>& t2) {
return Expression<T>(t1, &T::between, t2);
}
// 2D Geometry
typedef Expression<Point2> Point2_;