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

Merge remote-tracking branch 'origin/feature/BAD_ceres' into feature/BAD: MASSIVE edit with ceres-style AutoDiff now Adaptable to expressions, as well as traits that specify group/manifold predicates and dimensions. A lot of the edits were because of the latter, and especially the traits-induced changes in numericalDerivative.

release/4.3a0
dellaert 2014-10-22 01:36:10 +02:00
parent 2f61653316 113b9d2e74
commit 4f9a751f83
79 changed files with 4856 additions and 1253 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

202
.cproject
View File

@ -600,6 +600,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>
@ -607,6 +608,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>
@ -654,6 +656,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>
@ -661,6 +664,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>
@ -668,6 +672,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>
@ -683,6 +688,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>
@ -800,6 +806,54 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInertialNavFactor_GlobalVelocity.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInertialNavFactor_GlobalVelocity.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthFactorVariant3.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInvDepthFactorVariant3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthFactorVariant1.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInvDepthFactorVariant1.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEquivInertialNavFactor_GlobalVel.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testEquivInertialNavFactor_GlobalVel.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthFactorVariant2.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInvDepthFactorVariant2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRelativeElevationFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testRelativeElevationFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraph.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -896,6 +950,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianBayesTree.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeCameraExpression.run" path="build/gtsam_unstable/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1042,6 +1104,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>
@ -1271,6 +1334,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>
@ -1353,7 +1456,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>
@ -1393,7 +1495,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>
@ -1401,7 +1502,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>
@ -1415,46 +1515,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>
@ -1712,6 +1772,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>
@ -1719,6 +1780,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>
@ -1726,6 +1788,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>
@ -1733,6 +1796,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>
@ -2065,6 +2129,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot2.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testRot2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot3Q.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testRot3Q.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2161,6 +2241,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVelocityConstraint.run" path="build/gtsam_unstable/dynamics/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVelocityConstraint.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDiscreteBayesTree.run" path="build/gtsam/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j1</buildArguments>
@ -2321,6 +2409,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNumericalDerivative.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testNumericalDerivative.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -2459,6 +2555,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>
@ -2466,6 +2563,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>
@ -2473,6 +2571,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>
@ -2534,6 +2633,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testManifold.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testManifold.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testParticleFactor.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -2566,6 +2673,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testAdaptAutoDiff.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testAdaptAutoDiff.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>
@ -2968,7 +3083,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>

14
gtsam/base/LieMatrix.h
View File

@ -174,4 +174,18 @@ private:
}
};
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<LieMatrix> : public std::true_type {
};
template<>
struct dimension<LieMatrix> : public Dynamic {
};
}
} // \namespace gtsam

18
gtsam/base/LieScalar.h
View File

@ -111,4 +111,22 @@ namespace gtsam {
private:
double d_;
};
// Define GTSAM traits
namespace traits {
template<>
struct is_group<LieScalar> : public std::true_type {
};
template<>
struct is_manifold<LieScalar> : public std::true_type {
};
template<>
struct dimension<LieScalar> : public std::integral_constant<int, 1> {
};
}
} // \namespace gtsam

15
gtsam/base/LieVector.h
View File

@ -128,6 +128,19 @@ private:
ar & boost::serialization::make_nvp("Vector",
boost::serialization::base_object<Vector>(*this));
}
};
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<LieVector> : public std::true_type {
};
template<>
struct dimension<LieVector> : public Dynamic {
};
}
} // \namespace gtsam

229
gtsam/base/Manifold.h
View File

@ -13,25 +13,19 @@
* @file Manifold.h
* @brief Base class and basic functions for Manifold types
* @author Alex Cunningham
* @author Frank Dellaert
*/
#pragma once
#include <string>
#include <gtsam/base/Matrix.h>
#include <boost/static_assert.hpp>
#include <type_traits>
#include <string>
namespace gtsam {
/**
* Concept check class for Manifold types
* Requires a mapping between a linear tangent space and the underlying
* manifold, of which Lie is a specialization.
*
* The necessary functions to implement for Manifold are defined
* below with additional details as to the interface. The
* concept checking function in class Manifold will check whether or not
* the function exists and throw compile-time errors.
*
* A manifold defines a space in which there is a notion of a linear tangent space
* that can be centered around a given point on the manifold. These nonlinear
* spaces may have such properties as wrapping around (as is the case with rotations),
@ -45,7 +39,216 @@ namespace gtsam {
* There may be multiple possible retractions for a given manifold, which can be chosen
* between depending on the computational complexity. The important criteria for
* the creation for the retract and localCoordinates functions is that they be
* inverse operations.
* inverse operations. The new notion of a Chart guarantees that.
*
*/
// Traits, same style as Boost.TypeTraits
// All meta-functions below ever only declare a single type
// or a type/value/value_type
namespace traits {
// is group, by default this is false
template<typename T>
struct is_group: public std::false_type {
};
// identity, no default provided, by default given by default constructor
template<typename T>
struct identity {
static T value() {
return T();
}
};
// is manifold, by default this is false
template<typename T>
struct is_manifold: public std::false_type {
};
// dimension, can return Eigen::Dynamic (-1) if not known at compile time
template<typename T>
struct dimension;
/**
* zero<T>::value is intended to be the origin of a canonical coordinate system
* with canonical(t) == DefaultChart<T>(zero<T>::value).apply(t)
* Below we provide the group identity as zero *in case* it is a group
*/
template<typename T> struct zero: public identity<T> {
BOOST_STATIC_ASSERT(is_group<T>::value);
};
// double
template<>
struct is_group<double> : public std::true_type {
};
template<>
struct is_manifold<double> : public std::true_type {
};
template<>
struct dimension<double> : public std::integral_constant<int, 1> {
};
template<>
struct zero<double> {
static double value() {
return 0;
}
};
// Fixed size Eigen::Matrix type
template<int M, int N, int Options>
struct is_group<Eigen::Matrix<double, M, N, Options> > : public std::true_type {
};
template<int M, int N, int Options>
struct is_manifold<Eigen::Matrix<double, M, N, Options> > : public std::true_type {
};
// TODO: Could be more sophisticated using Eigen traits and SFINAE?
typedef std::integral_constant<int, Eigen::Dynamic> Dynamic;
template<int Options>
struct dimension<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Options> > : public Dynamic {
};
template<int M, int Options>
struct dimension<Eigen::Matrix<double, M, Eigen::Dynamic, Options> > : public Dynamic {
};
template<int N, int Options>
struct dimension<Eigen::Matrix<double, Eigen::Dynamic, N, Options> > : public Dynamic {
};
template<int M, int N, int Options>
struct dimension<Eigen::Matrix<double, M, N, Options> > : public std::integral_constant<
int, M * N> {
};
template<int M, int N, int Options>
struct zero<Eigen::Matrix<double, M, N, Options> > : public std::integral_constant<
int, M * N> {
BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
"traits::zero is only supported for fixed-size matrices");
static Eigen::Matrix<double, M, N, Options> value() {
return Eigen::Matrix<double, M, N, Options>::Zero();
}
};
} // \ namespace traits
// Chart is a map from T -> vector, retract is its inverse
template<typename T>
struct DefaultChart {
BOOST_STATIC_ASSERT(traits::is_manifold<T>::value);
typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
T t_;
DefaultChart(const T& t) :
t_(t) {
}
vector apply(const T& other) {
return t_.localCoordinates(other);
}
T retract(const vector& d) {
return t_.retract(d);
}
};
/**
* Canonical<T>::value is a chart around zero<T>::value
* An example is Canonical<Rot3>
*/
template<typename T> struct Canonical {
typedef T type;
typedef typename DefaultChart<T>::vector vector;
DefaultChart<T> chart;
Canonical() :
chart(traits::zero<T>::value()) {
}
// Convert t of type T into canonical coordinates
vector apply(const T& t) {
return chart.apply(t);
}
// Convert back from canonical coordinates to T
T retract(const vector& v) {
return chart.retract(v);
}
};
// double
template<>
struct DefaultChart<double> {
typedef Eigen::Matrix<double, 1, 1> vector;
double t_;
DefaultChart(double t) :
t_(t) {
}
vector apply(double other) {
vector d;
d << other - t_;
return d;
}
double retract(const vector& d) {
return t_ + d[0];
}
};
// Fixed size Eigen::Matrix type
template<int M, int N, int Options>
struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
typedef Eigen::Matrix<double, M, N, Options> T;
typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
"DefaultChart has not been implemented yet for dynamically sized matrices");
T t_;
DefaultChart(const T& t) :
t_(t) {
}
vector apply(const T& other) {
T diff = other - t_;
Eigen::Map<vector> map(diff.data());
return vector(map);
}
T retract(const vector& d) {
Eigen::Map<const T> map(d.data());
return t_ + map;
}
};
// Dynamically sized Vector
template<>
struct DefaultChart<Vector> {
typedef Vector T;
typedef T vector;
T t_;
DefaultChart(const T& t) :
t_(t) {
}
vector apply(const T& other) {
return other - t_;
}
T retract(const vector& d) {
return t_ + d;
}
};
/**
* Old Concept check class for Manifold types
* Requires a mapping between a linear tangent space and the underlying
* manifold, of which Lie is a specialization.
*
* The necessary functions to implement for Manifold are defined
* below with additional details as to the interface. The
* concept checking function in class Manifold will check whether or not
* the function exists and throw compile-time errors.
*
* Returns dimensionality of the tangent space, which may be smaller than the number
* of nonlinear parameters.
@ -61,7 +264,7 @@ namespace gtsam {
* By convention, we use capital letters to designate a static function
* @tparam T is a Lie type, like Point2, Pose3, etc.
*/
template <class T>
template<class T>
class ManifoldConcept {
private:
/** concept checking function - implement the functions this demands */
@ -89,7 +292,7 @@ private:
}
};
} // namespace gtsam
} // \ namespace gtsam
/**
* Macros for using the ManifoldConcept

1066
gtsam/base/numericalDerivative.h
View File

File diff suppressed because it is too large Load Diff

108
gtsam/base/tests/testNumericalDerivative.cpp
View File

@ -15,115 +15,123 @@
* @date Apr 8, 2011
*/
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
double f(const LieVector& x) {
double f(const Vector2& x) {
assert(x.size() == 2);
return sin(x(0)) + cos(x(1));
}
/* ************************************************************************* */
TEST(testNumericalDerivative, numericalHessian) {
LieVector center = ones(2);
//
TEST(testNumericalDerivative, numericalGradient) {
Vector2 x(1, 1);
Matrix expected = (Matrix(2,2) <<
-sin(center(0)), 0.0,
0.0, -cos(center(1)));
Vector expected(2);
expected << cos(x(1)), -sin(x(0));
Matrix actual = numericalHessian(f, center);
Vector actual = numericalGradient<Vector2>(f, x);
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
double f2(const LieVector& x) {
TEST(testNumericalDerivative, numericalHessian) {
Vector2 x(1, 1);
Matrix expected(2, 2);
expected << -sin(x(0)), 0.0, 0.0, -cos(x(1));
Matrix actual = numericalHessian<Vector2>(f, x);
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
double f2(const Vector2& x) {
assert(x.size() == 2);
return sin(x(0)) * cos(x(1));
}
/* ************************************************************************* */
//
TEST(testNumericalDerivative, numericalHessian2) {
Vector v_center = (Vector(2) << 0.5, 1.0);
LieVector center(v_center);
Vector2 v(0.5, 1.0);
Vector2 x(v);
Matrix expected = (Matrix(2,2) <<
-cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)),
-cos(center(0))*sin(center(1)), -sin(center(0))*cos(center(1)));
Matrix expected = (Matrix(2, 2) << -cos(x(1)) * sin(x(0)), -sin(x(1))
* cos(x(0)), -cos(x(0)) * sin(x(1)), -sin(x(0)) * cos(x(1)));
Matrix actual = numericalHessian(f2, center);
Matrix actual = numericalHessian(f2, x);
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
double f3(const LieVector& x1, const LieVector& x2) {
assert(x1.size() == 1 && x2.size() == 1);
return sin(x1(0)) * cos(x2(0));
double f3(double x1, double x2) {
return sin(x1) * cos(x2);
}
/* ************************************************************************* */
//
TEST(testNumericalDerivative, numericalHessian211) {
Vector v_center1 = (Vector(1) << 1.0);
Vector v_center2 = (Vector(1) << 5.0);
LieVector center1(v_center1), center2(v_center2);
double x1 = 1, x2 = 5;
Matrix expected11 = (Matrix(1, 1) << -sin(center1(0))*cos(center2(0)));
Matrix actual11 = numericalHessian211(f3, center1, center2);
Matrix expected11 = (Matrix(1, 1) << -sin(x1) * cos(x2));
Matrix actual11 = numericalHessian211<double, double>(f3, x1, x2);
EXPECT(assert_equal(expected11, actual11, 1e-5));
Matrix expected12 = (Matrix(1, 1) <<-cos(center1(0))*sin(center2(0)));
Matrix actual12 = numericalHessian212(f3, center1, center2);
Matrix expected12 = (Matrix(1, 1) << -cos(x1) * sin(x2));
Matrix actual12 = numericalHessian212<double, double>(f3, x1, x2);
EXPECT(assert_equal(expected12, actual12, 1e-5));
Matrix expected22 = (Matrix(1, 1) <<-sin(center1(0))*cos(center2(0)));
Matrix actual22 = numericalHessian222(f3, center1, center2);
Matrix expected22 = (Matrix(1, 1) << -sin(x1) * cos(x2));
Matrix actual22 = numericalHessian222<double, double>(f3, x1, x2);
EXPECT(assert_equal(expected22, actual22, 1e-5));
}
/* ************************************************************************* */
double f4(const LieVector& x, const LieVector& y, const LieVector& z) {
assert(x.size() == 1 && y.size() == 1 && z.size() == 1);
return sin(x(0)) * cos(y(0)) * z(0)*z(0);
double f4(double x, double y, double z) {
return sin(x) * cos(y) * z * z;
}
/* ************************************************************************* */
//
TEST(testNumericalDerivative, numericalHessian311) {
Vector v_center1 = (Vector(1) << 1.0);
Vector v_center2 = (Vector(1) << 2.0);
Vector v_center3 = (Vector(1) << 3.0);
LieVector center1(v_center1), center2(v_center2), center3(v_center3);
double x = center1(0), y = center2(0), z = center3(0);
Matrix expected11 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z);
Matrix actual11 = numericalHessian311(f4, center1, center2, center3);
double x = 1, y = 2, z = 3;
Matrix expected11 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z);
Matrix actual11 = numericalHessian311<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected11, actual11, 1e-5));
Matrix expected12 = (Matrix(1, 1) << -cos(x)*sin(y)*z*z);
Matrix actual12 = numericalHessian312(f4, center1, center2, center3);
Matrix expected12 = (Matrix(1, 1) << -cos(x) * sin(y) * z * z);
Matrix actual12 = numericalHessian312<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected12, actual12, 1e-5));
Matrix expected13 = (Matrix(1, 1) << cos(x)*cos(y)*2*z);
Matrix actual13 = numericalHessian313(f4, center1, center2, center3);
Matrix expected13 = (Matrix(1, 1) << cos(x) * cos(y) * 2 * z);
Matrix actual13 = numericalHessian313<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected13, actual13, 1e-5));
Matrix expected22 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z);
Matrix actual22 = numericalHessian322(f4, center1, center2, center3);
Matrix expected22 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z);
Matrix actual22 = numericalHessian322<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected22, actual22, 1e-5));
Matrix expected23 = (Matrix(1, 1) << -sin(x)*sin(y)*2*z);
Matrix actual23 = numericalHessian323(f4, center1, center2, center3);
Matrix expected23 = (Matrix(1, 1) << -sin(x) * sin(y) * 2 * z);
Matrix actual23 = numericalHessian323<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected23, actual23, 1e-5));
Matrix expected33 = (Matrix(1, 1) << sin(x)*cos(y)*2);
Matrix actual33 = numericalHessian333(f4, center1, center2, center3);
Matrix expected33 = (Matrix(1, 1) << sin(x) * cos(y) * 2);
Matrix actual33 = numericalHessian333<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected33, actual33, 1e-5));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

26
gtsam/geometry/Cal3Bundler.h
View File

@ -36,8 +36,6 @@ private:
double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 3;
/// @name Standard Constructors
/// @{
@ -169,6 +167,26 @@ private:
/// @}
};
};
} // namespace gtsam
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<Cal3Bundler> : public std::true_type {
};
template<>
struct dimension<Cal3Bundler> : public std::integral_constant<int, 3> {
};
template<>
struct zero<Cal3Bundler> {
static Cal3Bundler value() {
return Cal3Bundler(0, 0, 0);
}
};
}
} // namespace gtsam

20
gtsam/geometry/Cal3DS2.h
View File

@ -46,8 +46,6 @@ protected:
double p1_, p2_ ; // tangential distortion
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 9;
Matrix K() const ;
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
@ -146,11 +144,9 @@ public:
/// Return dimensions of calibration manifold object
static size_t Dim() { return 9; } //TODO: make a final dimension variable
private:
/// @}
/// @name Advanced Interface
/// @{
private:
/** Serialization function */
friend class boost::serialization::access;
@ -170,10 +166,20 @@ private:
ar & BOOST_SERIALIZATION_NVP(p2_);
}
};
/// @}
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<Cal3DS2> : public std::true_type {
};
template<>
struct dimension<Cal3DS2> : public std::integral_constant<int, 9> {
};
}
}

22
gtsam/geometry/Cal3Unified.h
View File

@ -126,10 +126,6 @@ public:
private:
/// @}
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
@ -140,9 +136,25 @@ private:
ar & BOOST_SERIALIZATION_NVP(xi_);
}
/// @}
};
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<Cal3Unified> : public std::true_type {
};
template<>
struct dimension<Cal3Unified> : public std::integral_constant<int, 10> {
};
template<>
struct zero<Cal3Unified> {
static Cal3Unified value() { return Cal3Unified();}
};
}
}

24
gtsam/geometry/Cal3_S2.h
View File

@ -36,8 +36,6 @@ private:
double fx_, fy_, s_, u0_, v0_;
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 5;
typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
@ -200,12 +198,12 @@ public:
/// return DOF, dimensionality of tangent space
inline size_t dim() const {
return dimension;
return 5;
}
/// return DOF, dimensionality of tangent space
static size_t Dim() {
return dimension;
return 5;
}
/// Given 5-dim tangent vector, create new calibration
@ -242,4 +240,22 @@ private:
};
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<Cal3_S2> : public std::true_type {
};
template<>
struct dimension<Cal3_S2> : public std::integral_constant<int, 5> {
};
template<>
struct zero<Cal3_S2> {
static Cal3_S2 value() { return Cal3_S2();}
};
}
} // \ namespace gtsam

18
gtsam/geometry/EssentialMatrix.h
View File

@ -196,5 +196,23 @@ private:
};
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<EssentialMatrix> : public std::true_type {
};
template<>
struct dimension<EssentialMatrix> : public std::integral_constant<int, 5> {
};
template<>
struct zero<EssentialMatrix> {
static EssentialMatrix value() { return EssentialMatrix();}
};
}
} // gtsam

35
gtsam/geometry/PinholeCamera.h
View File

@ -303,7 +303,7 @@ public:
return K_.uncalibrate(pn);
}
typedef Eigen::Matrix<double,2,Calibration::dimension> Matrix2K;
typedef Eigen::Matrix<double,2,traits::dimension<Calibration>::value> Matrix2K;
/** project a point from world coordinate to the image
* @param pw is a point in world coordinates
@ -602,10 +602,6 @@ private:
Dpi_pn * Dpn_point;
}
/// @}
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
@ -614,6 +610,29 @@ private:
ar & BOOST_SERIALIZATION_NVP(pose_);
ar & BOOST_SERIALIZATION_NVP(K_);
}
/// @}
}
;}
};
// Define GTSAM traits
namespace traits {
template<typename Calibration>
struct is_manifold<PinholeCamera<Calibration> > : public std::true_type {
};
template<typename Calibration>
struct dimension<PinholeCamera<Calibration> > : public std::integral_constant<
int, dimension<Pose3>::value + dimension<Calibration>::value> {
};
template<typename Calibration>
struct zero<PinholeCamera<Calibration> > {
static PinholeCamera<Calibration> value() {
return PinholeCamera<Calibration>(zero<Pose3>::value(),
zero<Calibration>::value());
}
};
}
} // \ gtsam

26
gtsam/geometry/Point2.h
View File

@ -33,10 +33,9 @@ namespace gtsam {
* \nosubgrouping
*/
class GTSAM_EXPORT Point2 : public DerivedValue<Point2> {
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 2;
private:
double x_, y_;
public:
@ -153,10 +152,10 @@ public:
/// @{
/// dimension of the variable - used to autodetect sizes
inline static size_t Dim() { return dimension; }
inline static size_t Dim() { return 2; }
/// Dimensionality of tangent space = 2 DOF
inline size_t dim() const { return dimension; }
inline size_t dim() const { return 2; }
/// Updates a with tangent space delta
inline Point2 retract(const Vector& v) const { return *this + Point2(v); }
@ -251,5 +250,22 @@ private:
/// multiply with scalar
inline Point2 operator*(double s, const Point2& p) {return p*s;}
// Define GTSAM traits
namespace traits {
template<>
struct is_group<Point2> : public std::true_type {
};
template<>
struct is_manifold<Point2> : public std::true_type {
};
template<>
struct dimension<Point2> : public std::integral_constant<int, 2> {
};
}
}

24
gtsam/geometry/Point3.h
View File

@ -37,11 +37,9 @@ namespace gtsam {
* \nosubgrouping
*/
class GTSAM_EXPORT Point3 : public DerivedValue<Point3> {
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 3;
private:
double x_, y_, z_;
public:
@ -122,10 +120,10 @@ namespace gtsam {
/// @{
/// dimension of the variable - used to autodetect sizes
inline static size_t Dim() { return dimension; }
inline static size_t Dim() { return 3; }
/// return dimensionality of tangent space, DOF = 3
inline size_t dim() const { return dimension; }
inline size_t dim() const { return 3; }
/// Updates a with tangent space delta
inline Point3 retract(const Vector& v) const { return Point3(*this + v); }
@ -244,4 +242,20 @@ namespace gtsam {
/// Syntactic sugar for multiplying coordinates by a scalar s*p
inline Point3 operator*(double s, const Point3& p) { return p*s;}
// Define GTSAM traits
namespace traits {
template<>
struct is_group<Point3> : public std::true_type {
};
template<>
struct is_manifold<Point3> : public std::true_type {
};
template<>
struct dimension<Point3> : public std::integral_constant<int, 3> {
};
}
}

20
gtsam/geometry/Pose2.h
View File

@ -36,7 +36,6 @@ namespace gtsam {
class GTSAM_EXPORT Pose2 : public DerivedValue<Pose2> {
public:
static const size_t dimension = 3;
/** Pose Concept requirements */
typedef Rot2 Rotation;
@ -142,10 +141,10 @@ public:
/// @{
/// Dimensionality of tangent space = 3 DOF - used to autodetect sizes
inline static size_t Dim() { return dimension; }
inline static size_t Dim() { return 3; }
/// Dimensionality of tangent space = 3 DOF
inline size_t dim() const { return dimension; }
inline size_t dim() const { return 3; }
/// Retraction from R^3 \f$ [T_x,T_y,\theta] \f$ to Pose2 manifold neighborhood around current pose
Pose2 retract(const Vector& v) const;
@ -294,6 +293,8 @@ public:
*/
static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(2, 2); }
/// @}
private:
// Serialization function
@ -320,7 +321,18 @@ inline Matrix wedge<Pose2>(const Vector& xi) {
typedef std::pair<Point2,Point2> Point2Pair;
GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
/// @}
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<Pose2> : public std::true_type {
};
template<>
struct dimension<Pose2> : public std::integral_constant<int, 3> {
};
}
} // namespace gtsam

22
gtsam/geometry/Pose3.h
View File

@ -41,7 +41,6 @@ class Pose2;
*/
class GTSAM_EXPORT Pose3: public DerivedValue<Pose3> {
public:
static const size_t dimension = 6;
/** Pose Concept requirements */
typedef Rot3 Rotation;
@ -132,12 +131,12 @@ public:
/// Dimensionality of tangent space = 6 DOF - used to autodetect sizes
static size_t Dim() {
return dimension;
return 6;
}
/// Dimensionality of the tangent space = 6 DOF
size_t dim() const {
return dimension;
return 6;
}
/// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map
@ -355,4 +354,21 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
typedef std::pair<Point3, Point3> Point3Pair;
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
// Define GTSAM traits
namespace traits {
template<>
struct is_group<Pose3> : public std::true_type {
};
template<>
struct is_manifold<Pose3> : public std::true_type {
};
template<>
struct dimension<Pose3> : public std::integral_constant<int, 6> {
};
}
} // namespace gtsam

22
gtsam/geometry/Rot2.h
View File

@ -230,10 +230,6 @@ namespace gtsam {
/** return 2*2 transpose (inverse) rotation matrix */
Matrix transpose() const;
/// @}
/// @name Advanced Interface
/// @{
private:
/** Serialization function */
friend class boost::serialization::access;
@ -245,8 +241,22 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(s_);
}
/// @}
};
// Define GTSAM traits
namespace traits {
template<>
struct is_group<Rot2> : public std::true_type {
};
template<>
struct is_manifold<Rot2> : public std::true_type {
};
template<>
struct dimension<Rot2> : public std::integral_constant<int, 1> {
};
}
} // gtsam

28
gtsam/geometry/Rot3.h
View File

@ -59,10 +59,9 @@ namespace gtsam {
* \nosubgrouping
*/
class GTSAM_EXPORT Rot3 : public DerivedValue<Rot3> {
public:
static const size_t dimension = 3;
private:
#ifdef GTSAM_USE_QUATERNIONS
/** Internal Eigen Quaternion */
Quaternion quaternion_;
@ -260,10 +259,10 @@ namespace gtsam {
/// @{
/// dimension of the variable - used to autodetect sizes
static size_t Dim() { return dimension; }
static size_t Dim() { return 3; }
/// return dimensionality of tangent space, DOF = 3
size_t dim() const { return dimension; }
size_t dim() const { return 3; }
/**
* The method retract() is used to map from the tangent space back to the manifold.
@ -449,6 +448,8 @@ namespace gtsam {
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
/// @}
private:
/** Serialization function */
@ -478,8 +479,6 @@ namespace gtsam {
};
/// @}
/**
* [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R
* and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx'
@ -491,4 +490,21 @@ namespace gtsam {
* @return a vector [thetax, thetay, thetaz] in radians.
*/
GTSAM_EXPORT std::pair<Matrix3,Vector3> RQ(const Matrix3& A);
// Define GTSAM traits
namespace traits {
template<>
struct is_group<Rot3> : public std::true_type {
};
template<>
struct is_manifold<Rot3> : public std::true_type {
};
template<>
struct dimension<Rot3> : public std::integral_constant<int, 3> {
};
}
}

18
gtsam/geometry/StereoCamera.h
View File

@ -155,4 +155,22 @@ private:
};
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<StereoCamera> : public std::true_type {
};
template<>
struct dimension<StereoCamera> : public std::integral_constant<int, 6> {
};
template<>
struct zero<StereoCamera> {
static StereoCamera value() { return StereoCamera();}
};
}
}

16
gtsam/geometry/StereoPoint2.h
View File

@ -173,4 +173,20 @@ namespace gtsam {
};
// Define GTSAM traits
namespace traits {
template<>
struct is_group<StereoPoint2> : public std::true_type {
};
template<>
struct is_manifold<StereoPoint2> : public std::true_type {
};
template<>
struct dimension<StereoPoint2> : public std::integral_constant<int, 3> {
};
}
}

20
gtsam/geometry/Unit3.h
View File

@ -156,5 +156,25 @@ private:
};
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<Unit3> : public std::true_type {
};
template<>
struct dimension<Unit3> : public std::integral_constant<int, 2> {
};
template<>
struct zero<Unit3> {
static Unit3 value() {
return Unit3();
}
};
}
} // namespace gtsam

9
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -28,6 +28,13 @@ GTSAM_CONCEPT_MANIFOLD_INST(Cal3Bundler)
static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
static Point2 p(2,3);
/* ************************************************************************* */
TEST( Cal3Bundler, vector)
{
Cal3Bundler K;
CHECK(assert_equal((Vector(3)<<1,0,0),K.vector()));
}
/* ************************************************************************* */
TEST( Cal3Bundler, uncalibrate)
{
@ -36,7 +43,7 @@ TEST( Cal3Bundler, uncalibrate)
double g = v[0]*(1+v[1]*r+v[2]*r*r) ;
Point2 expected (1000+g*p.x(), 2000+g*p.y()) ;
Point2 actual = K.uncalibrate(p);
CHECK(assert_equal(actual,expected));
CHECK(assert_equal(expected,actual));
}
TEST( Cal3Bundler, calibrate )

8
gtsam/geometry/tests/testPoint2.cpp
View File

@ -84,8 +84,8 @@ namespace {
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
/* ************************************************************************* */
LieVector norm_proxy(const Point2& point) {
return LieVector(point.norm());
double norm_proxy(const Point2& point) {
return point.norm();
}
}
TEST( Point2, norm ) {
@ -112,8 +112,8 @@ TEST( Point2, norm ) {
/* ************************************************************************* */
namespace {
LieVector distance_proxy(const Point2& location, const Point2& point) {
return LieVector(location.distance(point));
double distance_proxy(const Point2& location, const Point2& point) {
return location.distance(point);
}
}
TEST( Point2, distance ) {

8
gtsam/geometry/tests/testPose2.cpp
View File

@ -574,8 +574,8 @@ TEST( Pose2, bearing_pose )
/* ************************************************************************* */
namespace {
LieVector range_proxy(const Pose2& pose, const Point2& point) {
return LieVector(pose.range(point));
double range_proxy(const Pose2& pose, const Point2& point) {
return pose.range(point);
}
}
TEST( Pose2, range )
@ -611,8 +611,8 @@ TEST( Pose2, range )
/* ************************************************************************* */
namespace {
LieVector range_pose_proxy(const Pose2& pose, const Pose2& point) {
return LieVector(pose.range(point));
double range_pose_proxy(const Pose2& pose, const Pose2& point) {
return pose.range(point);
}
}
TEST( Pose2, range_pose )

46
gtsam/geometry/tests/testPose3.cpp
View File

@ -547,8 +547,8 @@ Pose3
xl4(Rot3::ypr(0.0, 0.0, 1.0), Point3(1, 4,-4));
/* ************************************************************************* */
LieVector range_proxy(const Pose3& pose, const Point3& point) {
return LieVector(pose.range(point));
double range_proxy(const Pose3& pose, const Point3& point) {
return pose.range(point);
}
TEST( Pose3, range )
{
@ -582,8 +582,8 @@ TEST( Pose3, range )
}
/* ************************************************************************* */
LieVector range_pose_proxy(const Pose3& pose, const Pose3& point) {
return LieVector(pose.range(point));
double range_pose_proxy(const Pose3& pose, const Pose3& point) {
return pose.range(point);
}
TEST( Pose3, range_pose )
{
@ -674,30 +674,24 @@ TEST(Pose3, align_2) {
/* ************************************************************************* */
/// exp(xi) exp(y) = exp(xi + x)
/// Hence, y = log (exp(-xi)*exp(xi+x))
Vector xi = (Vector(6) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0);
Vector testDerivExpmapInv(const LieVector& dxi) {
Vector y = Pose3::Logmap(Pose3::Expmap(-xi)*Pose3::Expmap(xi+dxi));
return y;
Vector testDerivExpmapInv(const Vector6& dxi) {
return Pose3::Logmap(Pose3::Expmap(-xi) * Pose3::Expmap(xi + dxi));
}
TEST( Pose3, dExpInv_TLN) {
Matrix res = Pose3::dExpInv_exp(xi);
Matrix numericalDerivExpmapInv = numericalDerivative11(
boost::function<Vector(const LieVector&)>(
boost::bind(testDerivExpmapInv, _1)
),
LieVector(Vector::Zero(6)), 1e-5
);
Matrix numericalDerivExpmapInv = numericalDerivative11<Vector, Vector6>(
testDerivExpmapInv, Vector6::Zero(), 1e-5);
EXPECT(assert_equal(numericalDerivExpmapInv,res,3e-1));
}
/* ************************************************************************* */
Vector testDerivAdjoint(const LieVector& xi, const LieVector& v) {
return Pose3::adjointMap(xi)*v;
Vector testDerivAdjoint(const Vector6& xi, const Vector6& v) {
return Pose3::adjointMap(xi) * v;
}
TEST( Pose3, adjoint) {
@ -706,20 +700,16 @@ TEST( Pose3, adjoint) {
Matrix actualH;
Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH);
Matrix numericalH = numericalDerivative21(
boost::function<Vector(const LieVector&, const LieVector&)>(
boost::bind(testDerivAdjoint, _1, _2)
),
LieVector(screw::xi), LieVector(screw::xi), 1e-5
);
Matrix numericalH = numericalDerivative21<Vector, Vector6, Vector6>(
testDerivAdjoint, screw::xi, screw::xi, 1e-5);
EXPECT(assert_equal(expected,actual,1e-5));
EXPECT(assert_equal(numericalH,actualH,1e-5));
}
/* ************************************************************************* */
Vector testDerivAdjointTranspose(const LieVector& xi, const LieVector& v) {
return Pose3::adjointMap(xi).transpose()*v;
Vector testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) {
return Pose3::adjointMap(xi).transpose() * v;
}
TEST( Pose3, adjointTranspose) {
@ -730,12 +720,8 @@ TEST( Pose3, adjointTranspose) {
Matrix actualH;
Vector actual = Pose3::adjointTranspose(xi, v, actualH);
Matrix numericalH = numericalDerivative21(
boost::function<Vector(const LieVector&, const LieVector&)>(
boost::bind(testDerivAdjointTranspose, _1, _2)
),
LieVector(xi), LieVector(v), 1e-5
);
Matrix numericalH = numericalDerivative21<Vector, Vector6, Vector6>(
testDerivAdjointTranspose, xi, v, 1e-5);
EXPECT(assert_equal(expected,actual,1e-15));
EXPECT(assert_equal(numericalH,actualH,1e-5));

20
gtsam/geometry/tests/testRot3M.cpp
View File

@ -217,7 +217,7 @@ TEST( Rot3, rightJacobianExpMapSO3 )
// Linearization point
Vector3 thetahat; thetahat << 0.1, 0, 0;
Matrix expectedJacobian = numericalDerivative11<Rot3, LieVector>(boost::bind(&evaluateRotation, _1), LieVector(thetahat));
Matrix expectedJacobian = numericalDerivative11<Rot3, Vector3>(boost::bind(&evaluateRotation, _1), thetahat);
Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat);
EXPECT(assert_equal(expectedJacobian, actualJacobian));
}
@ -229,7 +229,8 @@ TEST( Rot3, rightJacobianExpMapSO3inverse )
Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias
Vector3 deltatheta; deltatheta << 0, 0, 0;
Matrix expectedJacobian = numericalDerivative11<LieVector>(boost::bind(&evaluateLogRotation, thetahat, _1), LieVector(deltatheta));
Matrix expectedJacobian = numericalDerivative11<Vector3,Vector3>(
boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta);
Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat);
EXPECT(assert_equal(expectedJacobian, actualJacobian));
}
@ -439,19 +440,18 @@ TEST( Rot3, between )
/* ************************************************************************* */
Vector w = (Vector(3) << 0.1, 0.27, -0.2);
// Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes?
// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
// Left trivialization Derivative of exp(w) wrpt w:
// How does exp(w) change when w changes?
// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
// => y = log (exp(-w) * exp(w+dw))
Vector testDexpL(const LieVector& dw) {
Vector y = Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw));
return y;
Vector3 testDexpL(const Vector3& dw) {
return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw));
}
TEST( Rot3, dexpL) {
Matrix actualDexpL = Rot3::dexpL(w);
Matrix expectedDexpL = numericalDerivative11(
boost::function<Vector(const LieVector&)>(
boost::bind(testDexpL, _1)), LieVector(zero(3)), 1e-2);
Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(testDexpL,
Vector3::Zero(), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
Matrix actualDexpInvL = Rot3::dexpInvL(w);

6
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -274,11 +274,7 @@ TEST( triangulation, TriangulationFactor ) {
Matrix HActual;
factor.evaluateError(landmark, HActual);
// Matrix expectedH1 = numericalDerivative11<Pose3>(
// boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2,
// boost::none, boost::none), pose1);
// The expected Jacobian
Matrix HExpected = numericalDerivative11<Point3>(
Matrix HExpected = numericalDerivative11<Vector,Point3>(
boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);
// Verify the Jacobians are correct

4
gtsam/geometry/tests/testUnit3.cpp
View File

@ -115,13 +115,13 @@ TEST(Unit3, error) {
Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Unit3>(
expected = numericalDerivative11<Vector,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), q);
p.error(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
}
{
expected = numericalDerivative11<Unit3>(
expected = numericalDerivative11<Vector,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), r);
p.error(r, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9));

14
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/assign/list_of.hpp>
@ -149,8 +148,9 @@ TEST( GaussianBayesNet, DeterminantTest )
}
/* ************************************************************************* */
typedef Eigen::Matrix<double,10,1> Vector10;
namespace {
double computeError(const GaussianBayesNet& gbn, const LieVector& values)
double computeError(const GaussianBayesNet& gbn, const Vector10& values)
{
pair<Matrix,Vector> Rd = GaussianFactorGraph(gbn).jacobian();
return 0.5 * (Rd.first * values - Rd.second).squaredNorm();
@ -180,14 +180,12 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0)));
// Compute the Hessian numerically
Matrix hessian = numericalHessian(
boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)),
LieVector(Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols())));
Matrix hessian = numericalHessian<Vector10>(
boost::bind(&computeError, gbn, _1), Vector10::Zero());
// Compute the gradient numerically
Vector gradient = numericalGradient(
boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)),
LieVector(Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols())));
Vector gradient = numericalGradient<Vector10>(
boost::bind(&computeError, gbn, _1), Vector10::Zero());
// Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();

23
gtsam/linear/tests/testGaussianBayesTree.cpp
View File

@ -24,7 +24,6 @@
using namespace boost::assign;
#include <gtsam/base/debug.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/linear/GaussianJunctionTree.h>
@ -175,13 +174,13 @@ TEST(GaussianBayesTree, complicatedMarginal) {
EXPECT(assert_equal(expectedCov, actualCov, 1e1));
}
/* ************************************************************************* */
typedef Eigen::Matrix<double, 10, 1> Vector10;
namespace {
/* ************************************************************************* */
double computeError(const GaussianBayesTree& gbt, const LieVector& values)
{
pair<Matrix,Vector> Rd = GaussianFactorGraph(gbt).jacobian();
return 0.5 * (Rd.first * values - Rd.second).squaredNorm();
}
double computeError(const GaussianBayesTree& gbt, const Vector10& values) {
pair<Matrix, Vector> Rd = GaussianFactorGraph(gbt).jacobian();
return 0.5 * (Rd.first * values - Rd.second).squaredNorm();
}
}
/* ************************************************************************* */
@ -243,14 +242,12 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
2, (Vector(4) << 1.0,2.0,15.0,16.0))))));
// Compute the Hessian numerically
Matrix hessian = numericalHessian(
boost::function<double(const LieVector&)>(boost::bind(&computeError, bt, _1)),
LieVector(Vector::Zero(GaussianFactorGraph(bt).jacobian().first.cols())));
Matrix hessian = numericalHessian<Vector10>(
boost::bind(&computeError, bt, _1), Vector10::Zero());
// Compute the gradient numerically
Vector gradient = numericalGradient(
boost::function<double(const LieVector&)>(boost::bind(&computeError, bt, _1)),
LieVector(Vector::Zero(GaussianFactorGraph(bt).jacobian().first.cols())));
Vector gradient = numericalGradient<Vector10>(
boost::bind(&computeError, bt, _1), Vector10::Zero());
// Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian();

17
gtsam/navigation/ImuBias.h
View File

@ -218,6 +218,23 @@ namespace imuBias {
} // namespace ImuBias
// Define GTSAM traits
namespace traits {
template<>
struct is_group<imuBias::ConstantBias> : public std::true_type {
};
template<>
struct is_manifold<imuBias::ConstantBias> : public std::true_type {
};
template<>
struct dimension<imuBias::ConstantBias> : public std::integral_constant<int, 6> {
};
}
} // namespace gtsam

4
gtsam/navigation/tests/testAttitudeFactor.cpp
View File

@ -45,7 +45,7 @@ TEST( Rot3AttitudeFactor, Constructor ) {
EXPECT(assert_equal(zero(2),factor.evaluateError(nRb),1e-5));
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Rot3>(
Matrix expectedH = numericalDerivative11<Vector,Rot3>(
boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none),
nRb);
@ -78,7 +78,7 @@ TEST( Pose3AttitudeFactor, Constructor ) {
EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5));
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Pose3>(
Matrix expectedH = numericalDerivative11<Vector,Pose3>(
boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1,
boost::none), T);

4
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -239,12 +239,12 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
// Compute numerical derivatives
Matrix expectedDelPdelBias = numericalDerivative11<imuBias::ConstantBias>(
Matrix expectedDelPdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
Matrix expectedDelVdelBias = numericalDerivative11<imuBias::ConstantBias>(
Matrix expectedDelVdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);

2
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -56,7 +56,7 @@ TEST( GPSFactor, Constructors ) {
EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5));
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Pose3>(
Matrix expectedH = numericalDerivative11<Vector,Pose3>(
boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T);
// Use the factor to calculate the derivative

67
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
@ -39,14 +38,14 @@ using symbol_shorthand::B;
/* ************************************************************************* */
namespace {
Vector callEvaluateError(const ImuFactor& factor,
const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias)
{
return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias);
}
Rot3 evaluateRotationError(const ImuFactor& factor,
const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias)
{
return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ;
@ -168,9 +167,9 @@ TEST( ImuFactor, Error )
// Linearization point
imuBias::ConstantBias bias; // Bias
Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
@ -192,15 +191,15 @@ TEST( ImuFactor, Error )
EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians
Matrix H1e = numericalDerivative11<Pose3>(
Matrix H1e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
Matrix H2e = numericalDerivative11<LieVector>(
Matrix H2e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
Matrix H3e = numericalDerivative11<Pose3>(
Matrix H3e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
Matrix H4e = numericalDerivative11<LieVector>(
Matrix H4e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
Matrix H5e = numericalDerivative11<imuBias::ConstantBias>(
Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
// Check rotation Jacobians
@ -240,16 +239,16 @@ TEST( ImuFactor, ErrorWithBiases )
// Linearization point
// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot)
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
// LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
// Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
// LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
@ -276,15 +275,15 @@ TEST( ImuFactor, ErrorWithBiases )
// EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians
Matrix H1e = numericalDerivative11<Pose3>(
Matrix H1e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
Matrix H2e = numericalDerivative11<LieVector>(
Matrix H2e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
Matrix H3e = numericalDerivative11<Pose3>(
Matrix H3e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
Matrix H4e = numericalDerivative11<LieVector>(
Matrix H4e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
Matrix H5e = numericalDerivative11<imuBias::ConstantBias>(
Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
// Check rotation Jacobians
@ -318,8 +317,8 @@ TEST( ImuFactor, PartialDerivativeExpmap )
// Compute numerical derivatives
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, LieVector>(boost::bind(
&evaluateRotation, measuredOmega, _1, deltaT), LieVector(biasOmega));
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(boost::bind(
&evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega));
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT);
@ -341,8 +340,8 @@ TEST( ImuFactor, PartialDerivativeLogmap )
// Compute numerical derivatives
Matrix expectedDelFdeltheta = numericalDerivative11<LieVector>(boost::bind(
&evaluateLogRotation, thetahat, _1), LieVector(deltatheta));
Matrix expectedDelFdeltheta = numericalDerivative11<Vector,Vector3>(boost::bind(
&evaluateLogRotation, thetahat, _1), Vector3(deltatheta));
const Vector3 x = thetahat; // parametrization of so(3)
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
@ -417,12 +416,12 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
// Compute numerical derivatives
Matrix expectedDelPdelBias = numericalDerivative11<imuBias::ConstantBias>(
Matrix expectedDelPdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
Matrix expectedDelVdelBias = numericalDerivative11<imuBias::ConstantBias>(
Matrix expectedDelVdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
@ -447,9 +446,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
//{
// // Linearization point
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
// LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
// Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
// LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
// imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012));
//
// // Pre-integrator
@ -503,9 +502,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
@ -531,15 +530,15 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
// Expected Jacobians
Matrix H1e = numericalDerivative11<Pose3>(
Matrix H1e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
Matrix H2e = numericalDerivative11<LieVector>(
Matrix H2e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
Matrix H3e = numericalDerivative11<Pose3>(
Matrix H3e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
Matrix H4e = numericalDerivative11<LieVector>(
Matrix H4e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
Matrix H5e = numericalDerivative11<imuBias::ConstantBias>(
Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
// Check rotation Jacobians

14
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -72,35 +72,35 @@ TEST( MagFactor, Factors ) {
// MagFactor
MagFactor f(1, measured, s, dir, bias, model);
EXPECT( assert_equal(zero(3),f.evaluateError(theta,H1),1e-5));
EXPECT( assert_equal(numericalDerivative11<Rot2> //
EXPECT( assert_equal(numericalDerivative11<Vector,Rot2> //
(boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7));
// MagFactor1
MagFactor1 f1(1, measured, s, dir, bias, model);
EXPECT( assert_equal(zero(3),f1.evaluateError(nRb,H1),1e-5));
EXPECT( assert_equal(numericalDerivative11<Rot3> //
EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> //
(boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7));
// MagFactor2
MagFactor2 f2(1, 2, measured, nRb, model);
EXPECT( assert_equal(zero(3),f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT( assert_equal(numericalDerivative11<Point3> //
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
(boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),//
H1, 1e-7));
EXPECT( assert_equal(numericalDerivative11<Point3> //
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
(boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),//
H2, 1e-7));
// MagFactor2
MagFactor3 f3(1, 2, 3, measured, nRb, model);
EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
EXPECT(assert_equal(numericalDerivative11<LieScalar> //
EXPECT(assert_equal(numericalDerivative11<Vector,LieScalar> //
(boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),//
H1, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Unit3> //
EXPECT(assert_equal(numericalDerivative11<Vector,Unit3> //
(boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),//
H2, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Point3> //
EXPECT(assert_equal(numericalDerivative11<Vector,Point3> //
(boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),//
H3, 1e-7));
}

4
gtsam/slam/tests/testEssentialMatrixConstraint.cpp
View File

@ -50,10 +50,10 @@ TEST( EssentialMatrixConstraint, test ) {
CHECK(assert_equal(expected, actual, 1e-8));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Pose3>(
Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2,
boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Pose3>(
Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1,
boost::none, boost::none), pose2);

18
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -96,7 +96,7 @@ TEST (EssentialMatrixFactor, factor) {
// Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected;
Hexpected = numericalDerivative11<EssentialMatrix>(
Hexpected = numericalDerivative11<Vector,EssentialMatrix>(
boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1,
boost::none), trueE);
@ -173,8 +173,8 @@ TEST (EssentialMatrixFactor2, factor) {
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2,
boost::none, boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, trueE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, trueE, d);
Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, trueE, d);
Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, trueE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
@ -247,8 +247,8 @@ TEST (EssentialMatrixFactor3, factor) {
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2,
boost::none, boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, bodyE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, bodyE, d);
Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, bodyE, d);
Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, bodyE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
@ -389,8 +389,8 @@ TEST (EssentialMatrixFactor2, extraTest) {
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2,
boost::none, boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, trueE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, trueE, d);
Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, trueE, d);
Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, trueE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));
@ -458,8 +458,8 @@ TEST (EssentialMatrixFactor3, extraTest) {
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2,
boost::none, boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, bodyE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, bodyE, d);
Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, bodyE, d);
Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, bodyE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));

20
gtsam/slam/tests/testPoseRotationPrior.cpp
View File

@ -37,13 +37,13 @@ const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);
const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2);
/* ************************************************************************* */
LieVector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) {
return LieVector(factor.evaluateError(x));
Vector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) {
return factor.evaluateError(x);
}
/* ************************************************************************* */
LieVector evalFactorError2(const Pose2RotationPrior& factor, const Pose2& x) {
return LieVector(factor.evaluateError(x));
Vector evalFactorError2(const Pose2RotationPrior& factor, const Pose2& x) {
return factor.evaluateError(x);
}
/* ************************************************************************* */
@ -52,8 +52,7 @@ TEST( testPoseRotationFactor, level3_zero_error ) {
Pose3RotationPrior factor(poseKey, rot3A, model3);
Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -63,8 +62,7 @@ TEST( testPoseRotationFactor, level3_error ) {
Pose3RotationPrior factor(poseKey, rot3C, model3);
Matrix actH1;
EXPECT(assert_equal((Vector(3) << -0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -74,8 +72,7 @@ TEST( testPoseRotationFactor, level2_zero_error ) {
Pose2RotationPrior factor(poseKey, rot2A, model1);
Matrix actH1;
EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>(
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -85,8 +82,7 @@ TEST( testPoseRotationFactor, level2_error ) {
Pose2RotationPrior factor(poseKey, rot2B, model1);
Matrix actH1;
EXPECT(assert_equal((Vector(1) << -M_PI_2), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>(
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}

32
gtsam/slam/tests/testPoseTranslationPrior.cpp
View File

@ -34,13 +34,13 @@ const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);
const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2);
/* ************************************************************************* */
LieVector evalFactorError3(const Pose3TranslationPrior& factor, const Pose3& x) {
return LieVector(factor.evaluateError(x));
Vector evalFactorError3(const Pose3TranslationPrior& factor, const Pose3& x) {
return factor.evaluateError(x);
}
/* ************************************************************************* */
LieVector evalFactorError2(const Pose2TranslationPrior& factor, const Pose2& x) {
return LieVector(factor.evaluateError(x));
Vector evalFactorError2(const Pose2TranslationPrior& factor, const Pose2& x) {
return factor.evaluateError(x);
}
/* ************************************************************************* */
@ -49,8 +49,7 @@ TEST( testPoseTranslationFactor, level3_zero_error ) {
Pose3TranslationPrior factor(poseKey, point3A, model3);
Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -60,8 +59,7 @@ TEST( testPoseTranslationFactor, level3_error ) {
Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1;
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -71,8 +69,7 @@ TEST( testPoseTranslationFactor, pitched3_zero_error ) {
Pose3TranslationPrior factor(poseKey, point3A, model3);
Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -82,8 +79,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) {
Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1;
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -93,8 +89,7 @@ TEST( testPoseTranslationFactor, smallrot3_zero_error ) {
Pose3TranslationPrior factor(poseKey, point3A, model3);
Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -104,8 +99,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) {
Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1;
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -115,8 +109,7 @@ TEST( testPoseTranslationFactor, level2_zero_error ) {
Pose2TranslationPrior factor(poseKey, point2A, model2);
Matrix actH1;
EXPECT(assert_equal(zero(2), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>(
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -126,8 +119,7 @@ TEST( testPoseTranslationFactor, level2_error ) {
Pose2TranslationPrior factor(poseKey, point2B, model2);
Matrix actH1;
EXPECT(assert_equal((Vector(2) << -3.0,-4.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>(
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}

21
gtsam/slam/tests/testRangeFactor.cpp
View File

@ -23,7 +23,6 @@
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/bind.hpp>
@ -37,12 +36,12 @@ typedef RangeFactor<Pose2, Point2> RangeFactor2D;
typedef RangeFactor<Pose3, Point3> RangeFactor3D;
/* ************************************************************************* */
LieVector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) {
Vector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) {
return factor.evaluateError(pose, point);
}
/* ************************************************************************* */
LieVector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) {
Vector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) {
return factor.evaluateError(pose, point);
}
@ -214,8 +213,8 @@ TEST( RangeFactor, Jacobian2D ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
H1Expected = numericalDerivative11<Vector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -243,8 +242,8 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
H1Expected = numericalDerivative11<Vector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -269,8 +268,8 @@ TEST( RangeFactor, Jacobian3D ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
H1Expected = numericalDerivative11<Vector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -298,8 +297,8 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
H1Expected = numericalDerivative11<Vector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));

16
gtsam/slam/tests/testReferenceFrameFactor.cpp
View File

@ -53,9 +53,9 @@ TEST( ReferenceFrameFactor, equals ) {
}
/* ************************************************************************* */
LieVector evaluateError_(const PointReferenceFrameFactor& c,
Vector evaluateError_(const PointReferenceFrameFactor& c,
const Point2& global, const Pose2& trans, const Point2& local) {
return LieVector(c.evaluateError(global, trans, local));
return Vector(c.evaluateError(global, trans, local));
}
TEST( ReferenceFrameFactor, jacobians ) {
@ -68,13 +68,13 @@ TEST( ReferenceFrameFactor, jacobians ) {
tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
Matrix numericalDT, numericalDL, numericalDF;
numericalDF = numericalDerivative31<LieVector,Point2,Pose2,Point2>(
numericalDF = numericalDerivative31<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5);
numericalDT = numericalDerivative32<LieVector,Point2,Pose2,Point2>(
numericalDT = numericalDerivative32<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5);
numericalDL = numericalDerivative33<LieVector,Point2,Pose2,Point2>(
numericalDL = numericalDerivative33<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5);
@ -100,13 +100,13 @@ TEST( ReferenceFrameFactor, jacobians_zero ) {
tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
Matrix numericalDT, numericalDL, numericalDF;
numericalDF = numericalDerivative31<LieVector,Point2,Pose2,Point2>(
numericalDF = numericalDerivative31<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5);
numericalDT = numericalDerivative32<LieVector,Point2,Pose2,Point2>(
numericalDT = numericalDerivative32<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5);
numericalDL = numericalDerivative33<LieVector,Point2,Pose2,Point2>(
numericalDL = numericalDerivative33<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5);

8
gtsam/slam/tests/testRotateFactor.cpp
View File

@ -69,13 +69,13 @@ TEST (RotateFactor, test) {
Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Rot3>(
expected = numericalDerivative11<Vector,Rot3>(
boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc);
f.evaluateError(iRc, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
}
{
expected = numericalDerivative11<Rot3>(
expected = numericalDerivative11<Vector,Rot3>(
boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R);
f.evaluateError(R, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
@ -141,14 +141,14 @@ TEST (RotateDirectionsFactor, test) {
Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Rot3>(
expected = numericalDerivative11<Vector,Rot3>(
boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
boost::none), iRc);
f.evaluateError(iRc, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
}
{
expected = numericalDerivative11<Rot3>(
expected = numericalDerivative11<Vector,Rot3>(
boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
boost::none), R);
f.evaluateError(R, actual);

19
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -183,4 +183,23 @@ private:
}
};
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<PoseRTV> : public std::true_type {
};
template<>
struct dimension<PoseRTV> : public std::integral_constant<int, 9> {
};
template<>
struct zero<PoseRTV> {
static PoseRTV value() {
return PoseRTV();
}
};
}
} // \namespace gtsam

6
gtsam_unstable/dynamics/VelocityConstraint.h
View File

@ -83,9 +83,9 @@ public:
virtual gtsam::Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
if (H1) *H1 = gtsam::numericalDerivative21<gtsam::LieVector,PoseRTV,PoseRTV>(
if (H1) *H1 = gtsam::numericalDerivative21<gtsam::Vector,PoseRTV,PoseRTV>(
boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5);
if (H2) *H2 = gtsam::numericalDerivative22<gtsam::LieVector,PoseRTV,PoseRTV>(
if (H2) *H2 = gtsam::numericalDerivative22<gtsam::Vector,PoseRTV,PoseRTV>(
boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5);
return evaluateError_(x1, x2, dt_, integration_mode_);
}
@ -103,7 +103,7 @@ public:
}
private:
static gtsam::LieVector evaluateError_(const PoseRTV& x1, const PoseRTV& x2,
static gtsam::Vector evaluateError_(const PoseRTV& x1, const PoseRTV& x2,
double dt, const dynamics::IntegrationMode& mode) {
const Velocity3& v1 = x1.v(), v2 = x2.v(), p1 = x1.t(), p2 = x2.t();

28
gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
View File

@ -18,11 +18,11 @@ const double h = 0.01;
//const double deg2rad = M_PI/180.0;
//Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0));
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0));
//LieVector v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0));
LieVector V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0));
LieVector V1_g1 = g1.inverse().Adjoint(V1_w);
//Vector6 v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0));
Vector6 V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0));
Vector6 V1_g1 = g1.inverse().Adjoint(V1_w);
Pose3 g2(g1.retract(h*V1_g1, Pose3::EXPMAP));
//LieVector v2 = Pose3::Logmap(g1.between(g2));
//Vector6 v2 = Pose3::Logmap(g1.between(g2));
double mass = 100.0;
Vector gamma2 = (Vector(2) << 0.0, 0.0); // no shape
@ -46,16 +46,16 @@ Vector computeFu(const Vector& gamma, const Vector& control) {
}
/* ************************************************************************* */
Vector testExpmapDeriv(const LieVector& v) {
Vector testExpmapDeriv(const Vector6& v) {
return Pose3::Logmap(Pose3::Expmap(-h*V1_g1)*Pose3::Expmap(h*V1_g1+v));
}
TEST(Reconstruction, ExpmapInvDeriv) {
Matrix numericalExpmap = numericalDerivative11(
boost::function<Vector(const LieVector&)>(
boost::function<Vector(const Vector6&)>(
boost::bind(testExpmapDeriv, _1)
),
LieVector(Vector::Zero(6)), 1e-5
Vector6(Vector::Zero(6)), 1e-5
);
Matrix dExpInv = Pose3::dExpInv_exp(h*V1_g1);
EXPECT(assert_equal(numericalExpmap, dExpInv, 1e-2));
@ -72,21 +72,21 @@ TEST( Reconstruction, evaluateError) {
Matrix numericalH1 = numericalDerivative31(
boost::function<Vector(const Pose3&, const Pose3&, const LieVector&)>(
boost::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
),
g2, g1, V1_g1, 1e-5
);
Matrix numericalH2 = numericalDerivative32(
boost::function<Vector(const Pose3&, const Pose3&, const LieVector&)>(
boost::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
),
g2, g1, V1_g1, 1e-5
);
Matrix numericalH3 = numericalDerivative33(
boost::function<Vector(const Pose3&, const Pose3&, const LieVector&)>(
boost::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
),
g2, g1, V1_g1, 1e-5
@ -119,7 +119,7 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
Pose3 g21 = g2.between(g1);
Vector V2_g2 = g21.Adjoint(V2_g1); // convert the new velocity to g2's frame
LieVector expectedv2(V2_g2);
Vector6 expectedv2(V2_g2);
// hard constraints don't need a noise model
DiscreteEulerPoincareHelicopter constraint(V(2), V(1), G(2), h,
@ -130,21 +130,21 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
EXPECT(assert_equal(zero(6), constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0));
Matrix numericalH1 = numericalDerivative31(
boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>(
boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
),
expectedv2, V1_g1, g2, 1e-5
);
Matrix numericalH2 = numericalDerivative32(
boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>(
boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
),
expectedv2, V1_g1, g2, 1e-5
);
Matrix numericalH3 = numericalDerivative33(
boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>(
boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
),
expectedv2, V1_g1, g2, 1e-5

20
gtsam_unstable/geometry/Pose3Upright.h
View File

@ -126,11 +126,9 @@ public:
/// Log map at identity - return the canonical coordinates of this rotation
static Vector Logmap(const Pose3Upright& p);
private:
/// @}
/// @name Advanced Interface
/// @{
private:
// Serialization function
friend class boost::serialization::access;
@ -142,4 +140,18 @@ private:
}; // \class Pose3Upright
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<Pose3Upright> : public std::true_type {
};
template<>
struct dimension<Pose3Upright> : public std::integral_constant<int, 4> {
};
}
} // \namespace gtsam

30
gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
View File

@ -29,7 +29,7 @@ TEST( InvDepthFactor, Project1) {
Point2 expected_uv = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector inv_landmark((Vector(5) << 1., 0., 1., 0., 0.));
Vector5 inv_landmark((Vector(5) << 1., 0., 1., 0., 0.));
LieScalar inv_depth(1./4);
Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth);
EXPECT(assert_equal(expected_uv, actual_uv));
@ -45,7 +45,7 @@ TEST( InvDepthFactor, Project2) {
Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0))));
Vector5 diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0))));
LieScalar inv_depth(1/sqrt(3.0));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
@ -60,7 +60,7 @@ TEST( InvDepthFactor, Project3) {
Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0))));
Vector5 diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0))));
LieScalar inv_depth( 1./sqrt(1.0+1+4));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
@ -75,7 +75,7 @@ TEST( InvDepthFactor, Project4) {
Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.))));
Vector5 diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.))));
LieScalar inv_depth(1./sqrt(1.+16.+4.));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
@ -83,14 +83,14 @@ TEST( InvDepthFactor, Project4) {
/* ************************************************************************* */
Point2 project_(const Pose3& pose, const LieVector& landmark, const LieScalar& inv_depth) {
Point2 project_(const Pose3& pose, const Vector5& landmark, const LieScalar& inv_depth) {
return InvDepthCamera3<Cal3_S2>(pose,K).project(landmark, inv_depth); }
TEST( InvDepthFactor, Dproject_pose)
{
LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative31<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Matrix actual;
Point2 uv = inv_camera.project(landmark, inv_depth, actual, boost::none, boost::none);
@ -100,9 +100,9 @@ TEST( InvDepthFactor, Dproject_pose)
/* ************************************************************************* */
TEST( InvDepthFactor, Dproject_landmark)
{
LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative32<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Matrix actual;
Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, actual, boost::none);
@ -112,9 +112,9 @@ TEST( InvDepthFactor, Dproject_landmark)
/* ************************************************************************* */
TEST( InvDepthFactor, Dproject_inv_depth)
{
LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative33<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Matrix actual;
Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, boost::none, actual);
@ -124,12 +124,12 @@ TEST( InvDepthFactor, Dproject_inv_depth)
/* ************************************************************************* */
TEST(InvDepthFactor, backproject)
{
LieVector expected((Vector(5) << 0.,0.,1., 0.1,0.2));
Vector expected((Vector(5) << 0.,0.,1., 0.1,0.2));
LieScalar inv_depth(1./4);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Point2 z = inv_camera.project(expected, inv_depth);
LieVector actual_vec;
Vector5 actual_vec;
LieScalar actual_inv;
boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 4);
EXPECT(assert_equal(expected,actual_vec,1e-7));
@ -140,12 +140,12 @@ TEST(InvDepthFactor, backproject)
TEST(InvDepthFactor, backproject2)
{
// backwards facing camera
LieVector expected((Vector(5) << -5.,-5.,2., 3., -0.1));
Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1));
LieScalar inv_depth(1./10);
InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K);
Point2 z = inv_camera.project(expected, inv_depth);
LieVector actual_vec;
Vector5 actual_vec;
LieScalar actual_inv;
boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 10);
EXPECT(assert_equal(expected,actual_vec,1e-7));

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

@ -22,6 +22,8 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
@ -38,7 +40,6 @@
namespace MPL = boost::mpl::placeholders;
#include <new> // for placement new
class ExpressionFactorBinaryTest;
// Forward declare for testing
@ -75,14 +76,15 @@ struct CallRecord {
//-----------------------------------------------------------------------------
/// 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,
void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA,
JacobianMap& jacobians, Key key) {
JacobianMap::iterator it = jacobians.find(key);
it->second.block<ROWS,COLS>(0,0) += dTdA; // block makes HUGE difference
it->second.block<ROWS, COLS>(0, 0) += dTdA; // block makes HUGE difference
}
/// Handle Leaf Case for Dynamic Matrix type (slower)
template<>
void handleLeafCase(const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>& dTdA,
void handleLeafCase(
const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& dTdA,
JacobianMap& jacobians, Key key) {
JacobianMap::iterator it = jacobians.find(key);
it->second += dTdA;
@ -90,20 +92,35 @@ void handleLeafCase(const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>& d
//-----------------------------------------------------------------------------
/**
* The ExecutionTrace class records a tree-structured expression's execution
* 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::dimension<T>::value;
enum {
Constant, Leaf, Function
} kind;
union {
Key key;
CallRecord<T::dimension>* ptr;
CallRecord<Dim>* ptr;
} content;
public:
/// Pointer always starts out as a Constant
@ -116,7 +133,7 @@ public:
content.key = key;
}
/// Take ownership of pointer to a Function Record
void setFunction(CallRecord<T::dimension>* record) {
void setFunction(CallRecord<Dim>* record) {
kind = Function;
content.ptr = record;
}
@ -145,7 +162,7 @@ public:
* *** This is the main entry point for reverseAD, called from Expression ***
* Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function)
*/
typedef Eigen::Matrix<double, T::dimension, T::dimension> JacobianTT;
typedef Eigen::Matrix<double, Dim, Dim> JacobianTT;
void startReverseAD(JacobianMap& jacobians) const {
if (kind == Leaf) {
// This branch will only be called on trivial Leaf expressions, i.e. Priors
@ -164,7 +181,7 @@ public:
content.ptr->reverseAD(dTdA, jacobians);
}
// Either add to Jacobians (Leaf) or propagate (Function)
typedef Eigen::Matrix<double, 2, T::dimension> Jacobian2T;
typedef Eigen::Matrix<double, 2, Dim> Jacobian2T;
void reverseAD2(const Jacobian2T& dTdA, JacobianMap& jacobians) const {
if (kind == Leaf)
handleLeafCase(dTdA, jacobians, content.key);
@ -179,7 +196,7 @@ public:
/// Primary template calls the generic Matrix reverseAD pipeline
template<size_t ROWS, class A>
struct Select {
typedef Eigen::Matrix<double, ROWS, A::dimension> Jacobian;
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);
@ -189,7 +206,7 @@ struct Select {
/// Partially specialized template calls the 2-dimensional output version
template<class A>
struct Select<2, A> {
typedef Eigen::Matrix<double, 2, A::dimension> Jacobian;
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);
@ -300,7 +317,7 @@ public:
/// Return dimensions for each argument
virtual void dims(std::map<Key, size_t>& map) const {
map[key_] = T::dimension;
map[key_] = traits::dimension<T>::value;
}
/// Return value
@ -351,13 +368,15 @@ public:
/// meta-function to generate fixed-size JacobianTA type
template<class T, class A>
struct Jacobian {
typedef Eigen::Matrix<double, T::dimension, A::dimension> type;
typedef Eigen::Matrix<double, traits::dimension<T>::value,
traits::dimension<A>::value> type;
};
/// meta-function to generate JacobianTA optional reference
template<class T, class A>
struct Optional {
typedef Eigen::Matrix<double, T::dimension, A::dimension> Jacobian;
typedef Eigen::Matrix<double, traits::dimension<T>::value,
traits::dimension<A>::value> Jacobian;
typedef boost::optional<Jacobian&> type;
};
@ -368,7 +387,7 @@ template<class T>
struct FunctionalBase: ExpressionNode<T> {
static size_t const N = 0; // number of arguments
typedef CallRecord<T::dimension> Record;
typedef CallRecord<traits::dimension<T>::value> Record;
/// Construct an execution trace for reverse AD
void trace(const Values& values, Record* record, char*& raw) const {
@ -437,7 +456,8 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
/// Start the reverse AD process
virtual void startReverseAD(JacobianMap& jacobians) const {
Base::Record::startReverseAD(jacobians);
Select<T::dimension, A>::reverseAD(This::trace, This::dTdA, jacobians);
Select<traits::dimension<T>::value, A>::reverseAD(This::trace, This::dTdA,
jacobians);
}
/// Given df/dT, multiply in dT/dA and continue reverse AD process
@ -447,7 +467,7 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
}
/// Version specialized to 2-dimensional output
typedef Eigen::Matrix<double, 2, T::dimension> Jacobian2T;
typedef Eigen::Matrix<double, 2, traits::dimension<T>::value> Jacobian2T;
virtual void reverseAD2(const Jacobian2T& dFdT,
JacobianMap& jacobians) const {
Base::Record::reverseAD2(dFdT, jacobians);

8
gtsam_unstable/nonlinear/Expression.h
View File

@ -124,6 +124,11 @@ public:
/// Return value and derivatives, reverse AD version
T reverse(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();
char raw[size];
ExecutionTrace<T> trace;
@ -154,7 +159,8 @@ public:
template<class T>
struct apply_compose {
typedef T result_type;
typedef Eigen::Matrix<double, T::dimension, T::dimension> Jacobian;
static const int Dim = traits::dimension<T>::value;
typedef Eigen::Matrix<double, Dim, Dim> Jacobian;
T operator()(const T& x, const T& y, boost::optional<Jacobian&> H1,
boost::optional<Jacobian&> H2) const {
return x.compose(y, H1, H2);

27
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -37,6 +37,8 @@ class ExpressionFactor: public NoiseModelFactor {
std::vector<size_t> dimensions_; ///< dimensions of the Jacobian matrices
size_t augmentedCols_; ///< total number of columns + 1 (for RHS)
static const int Dim = traits::dimension<T>::value;
public:
/// Constructor
@ -45,7 +47,7 @@ public:
measurement_(measurement), expression_(expression) {
if (!noiseModel)
throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
if (noiseModel->dim() != T::dimension)
if (noiseModel->dim() != Dim)
throw std::invalid_argument(
"ExpressionFactor was created with a NoiseModel of incorrect dimension.");
noiseModel_ = noiseModel;
@ -68,7 +70,7 @@ public:
#ifdef DEBUG_ExpressionFactor
BOOST_FOREACH(size_t d, dimensions_)
std::cout << d << " ";
std::cout << " -> " << T::dimension << "x" << augmentedCols_ << std::endl;
std::cout << " -> " << Dim << "x" << augmentedCols_ << std::endl;
#endif
}
@ -87,10 +89,9 @@ public:
JacobianMap blocks;
for (DenseIndex i = 0; i < size(); i++) {
Matrix& Hi = H->at(i);
Hi.resize(T::dimension, dimensions_[i]);
Hi.resize(Dim, dimensions_[i]);
Hi.setZero(); // zero out
Eigen::Block<Matrix> block = Hi.block(0, 0, T::dimension,
dimensions_[i]);
Eigen::Block<Matrix> block = Hi.block(0, 0, Dim, dimensions_[i]);
blocks.insert(std::make_pair(keys_[i], block));
}
@ -104,10 +105,15 @@ public:
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
// Allocate memory on stack and create a view on it (saves a malloc)
double memory[T::dimension * augmentedCols_];
Eigen::Map<Eigen::Matrix<double, T::dimension, Eigen::Dynamic> > //
matrix(memory, T::dimension, augmentedCols_);
// This method has been heavily optimized for maximum performance.
// We allocate a VerticalBlockMatrix on the stack first, and then create
// Eigen::Block<Matrix> views on this piece of memory which is then passed
// to [expression_.value] below, which writes directly into Ab_.
// Another malloc saved by creating a Matrix on the stack
double memory[Dim * augmentedCols_];
Eigen::Map<Eigen::Matrix<double, Dim, Eigen::Dynamic> > //
matrix(memory, Dim, augmentedCols_);
matrix.setZero(); // zero out
// Construct block matrix, is of right size but un-initialized
@ -117,8 +123,9 @@ public:
JacobianMap blocks;
for (DenseIndex i = 0; i < size(); i++)
blocks.insert(std::make_pair(keys_[i], Ab(i)));
// Evaluate error to get Jacobians and RHS vector b
T value = expression_.value(x, blocks);
T value = expression_.value(x, blocks); // <<< Reverse AD happens here !
Ab(size()).col(0) = -measurement_.localCoordinates(value);
// Whiten the corresponding system now

314
gtsam_unstable/nonlinear/ceres_autodiff.h Normal file
View File

@ -0,0 +1,314 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: keir@google.com (Keir Mierle)
//
// Computation of the Jacobian matrix for vector-valued functions of multiple
// variables, using automatic differentiation based on the implementation of
// dual numbers in jet.h. Before reading the rest of this file, it is adivsable
// to read jet.h's header comment in detail.
//
// The helper wrapper AutoDiff::Differentiate() computes the jacobian of
// functors with templated operator() taking this form:
//
// struct F {
// template<typename T>
// bool operator()(const T *x, const T *y, ..., T *z) {
// // Compute z[] based on x[], y[], ...
// // return true if computation succeeded, false otherwise.
// }
// };
//
// All inputs and outputs may be vector-valued.
//
// To understand how jets are used to compute the jacobian, a
// picture may help. Consider a vector-valued function, F, returning 3
// dimensions and taking a vector-valued parameter of 4 dimensions:
//
// y x
// [ * ] F [ * ]
// [ * ] <--- [ * ]
// [ * ] [ * ]
// [ * ]
//
// Similar to the 2-parameter example for f described in jet.h, computing the
// jacobian dy/dx is done by substutiting a suitable jet object for x and all
// intermediate steps of the computation of F. Since x is has 4 dimensions, use
// a Jet<double, 4>.
//
// Before substituting a jet object for x, the dual components are set
// appropriately for each dimension of x:
//
// y x
// [ * | * * * * ] f [ * | 1 0 0 0 ] x0
// [ * | * * * * ] <--- [ * | 0 1 0 0 ] x1
// [ * | * * * * ] [ * | 0 0 1 0 ] x2
// ---+--- [ * | 0 0 0 1 ] x3
// | ^ ^ ^ ^
// dy/dx | | | +----- infinitesimal for x3
// | | +------- infinitesimal for x2
// | +--------- infinitesimal for x1
// +----------- infinitesimal for x0
//
// The reason to set the internal 4x4 submatrix to the identity is that we wish
// to take the derivative of y separately with respect to each dimension of x.
// Each column of the 4x4 identity is therefore for a single component of the
// independent variable x.
//
// Then the jacobian of the mapping, dy/dx, is the 3x4 sub-matrix of the
// extended y vector, indicated in the above diagram.
//
// Functors with multiple parameters
// ---------------------------------
// In practice, it is often convenient to use a function f of two or more
// vector-valued parameters, for example, x[3] and z[6]. Unfortunately, the jet
// framework is designed for a single-parameter vector-valued input. The wrapper
// in this file addresses this issue adding support for functions with one or
// more parameter vectors.
//
// To support multiple parameters, all the parameter vectors are concatenated
// into one and treated as a single parameter vector, except that since the
// functor expects different inputs, we need to construct the jets as if they
// were part of a single parameter vector. The extended jets are passed
// separately for each parameter.
//
// For example, consider a functor F taking two vector parameters, p[2] and
// q[3], and producing an output y[4]:
//
// struct F {
// template<typename T>
// bool operator()(const T *p, const T *q, T *z) {
// // ...
// }
// };
//
// In this case, the necessary jet type is Jet<double, 5>. Here is a
// visualization of the jet objects in this case:
//
// Dual components for p ----+
// |
// -+-
// y [ * | 1 0 | 0 0 0 ] --- p[0]
// [ * | 0 1 | 0 0 0 ] --- p[1]
// [ * | . . | + + + ] |
// [ * | . . | + + + ] v
// [ * | . . | + + + ] <--- F(p, q)
// [ * | . . | + + + ] ^
// ^^^ ^^^^^ |
// dy/dp dy/dq [ * | 0 0 | 1 0 0 ] --- q[0]
// [ * | 0 0 | 0 1 0 ] --- q[1]
// [ * | 0 0 | 0 0 1 ] --- q[2]
// --+--
// |
// Dual components for q --------------+
//
// where the 4x2 submatrix (marked with ".") and 4x3 submatrix (marked with "+"
// of y in the above diagram are the derivatives of y with respect to p and q
// respectively. This is how autodiff works for functors taking multiple vector
// valued arguments (up to 6).
//
// Jacobian NULL pointers
// ----------------------
// In general, the functions below will accept NULL pointers for all or some of
// the Jacobian parameters, meaning that those Jacobians will not be computed.
#ifndef CERES_PUBLIC_INTERNAL_AUTODIFF_H_
#define CERES_PUBLIC_INTERNAL_AUTODIFF_H_
#include <stddef.h>
#include <gtsam_unstable/nonlinear/ceres_jet.h>
#include <gtsam_unstable/nonlinear/ceres_eigen.h>
#include <gtsam_unstable/nonlinear/ceres_fixed_array.h>
#include <gtsam_unstable/nonlinear/ceres_variadic_evaluate.h>
#define DCHECK assert
#define DCHECK_GT(a,b) assert((a)>(b))
namespace ceres {
namespace internal {
// Extends src by a 1st order pertubation for every dimension and puts it in
// dst. The size of src is N. Since this is also used for perturbations in
// blocked arrays, offset is used to shift which part of the jet the
// perturbation occurs. This is used to set up the extended x augmented by an
// identity matrix. The JetT type should be a Jet type, and T should be a
// numeric type (e.g. double). For example,
//
// 0 1 2 3 4 5 6 7 8
// dst[0] [ * | . . | 1 0 0 | . . . ]
// dst[1] [ * | . . | 0 1 0 | . . . ]
// dst[2] [ * | . . | 0 0 1 | . . . ]
//
// is what would get put in dst if N was 3, offset was 3, and the jet type JetT
// was 8-dimensional.
template <typename JetT, typename T, int N>
inline void Make1stOrderPerturbation(int offset, const T* src, JetT* dst) {
DCHECK(src);
DCHECK(dst);
for (int j = 0; j < N; ++j) {
dst[j].a = src[j];
dst[j].v.setZero();
dst[j].v[offset + j] = T(1.0);
}
}
// Takes the 0th order part of src, assumed to be a Jet type, and puts it in
// dst. This is used to pick out the "vector" part of the extended y.
template <typename JetT, typename T>
inline void Take0thOrderPart(int M, const JetT *src, T dst) {
DCHECK(src);
for (int i = 0; i < M; ++i) {
dst[i] = src[i].a;
}
}
// Takes N 1st order parts, starting at index N0, and puts them in the M x N
// matrix 'dst'. This is used to pick out the "matrix" parts of the extended y.
template <typename JetT, typename T, int N0, int N>
inline void Take1stOrderPart(const int M, const JetT *src, T *dst) {
DCHECK(src);
DCHECK(dst);
for (int i = 0; i < M; ++i) {
Eigen::Map<Eigen::Matrix<T, N, 1> >(dst + N * i, N) =
src[i].v.template segment<N>(N0);
}
}
// This is in a struct because default template parameters on a
// function are not supported in C++03 (though it is available in
// C++0x). N0 through N5 are the dimension of the input arguments to
// the user supplied functor.
template <typename Functor, typename T,
int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0,
int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0>
struct AutoDiff {
static bool Differentiate(const Functor& functor,
T const *const *parameters,
int num_outputs,
T *function_value,
T **jacobians) {
// This block breaks the 80 column rule to keep it somewhat readable.
DCHECK_GT(num_outputs, 0);
DCHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) ||
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) ||
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) ||
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) ||
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) ||
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0)));
typedef Jet<T, N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9> JetT;
FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(
N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9 + num_outputs);
// These are the positions of the respective jets in the fixed array x.
const int jet0 = 0;
const int jet1 = N0;
const int jet2 = N0 + N1;
const int jet3 = N0 + N1 + N2;
const int jet4 = N0 + N1 + N2 + N3;
const int jet5 = N0 + N1 + N2 + N3 + N4;
const int jet6 = N0 + N1 + N2 + N3 + N4 + N5;
const int jet7 = N0 + N1 + N2 + N3 + N4 + N5 + N6;
const int jet8 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7;
const int jet9 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8;
const JetT *unpacked_parameters[10] = {
x.get() + jet0,
x.get() + jet1,
x.get() + jet2,
x.get() + jet3,
x.get() + jet4,
x.get() + jet5,
x.get() + jet6,
x.get() + jet7,
x.get() + jet8,
x.get() + jet9,
};
JetT* output = x.get() + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9;
#define CERES_MAKE_1ST_ORDER_PERTURBATION(i) \
if (N ## i) { \
internal::Make1stOrderPerturbation<JetT, T, N ## i>( \
jet ## i, \
parameters[i], \
x.get() + jet ## i); \
}
CERES_MAKE_1ST_ORDER_PERTURBATION(0);
CERES_MAKE_1ST_ORDER_PERTURBATION(1);
CERES_MAKE_1ST_ORDER_PERTURBATION(2);
CERES_MAKE_1ST_ORDER_PERTURBATION(3);
CERES_MAKE_1ST_ORDER_PERTURBATION(4);
CERES_MAKE_1ST_ORDER_PERTURBATION(5);
CERES_MAKE_1ST_ORDER_PERTURBATION(6);
CERES_MAKE_1ST_ORDER_PERTURBATION(7);
CERES_MAKE_1ST_ORDER_PERTURBATION(8);
CERES_MAKE_1ST_ORDER_PERTURBATION(9);
#undef CERES_MAKE_1ST_ORDER_PERTURBATION
if (!VariadicEvaluate<Functor, JetT,
N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call(
functor, unpacked_parameters, output)) {
return false;
}
internal::Take0thOrderPart(num_outputs, output, function_value);
#define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \
if (N ## i) { \
if (jacobians[i]) { \
internal::Take1stOrderPart<JetT, T, \
jet ## i, \
N ## i>(num_outputs, \
output, \
jacobians[i]); \
} \
}
CERES_TAKE_1ST_ORDER_PERTURBATION(0);
CERES_TAKE_1ST_ORDER_PERTURBATION(1);
CERES_TAKE_1ST_ORDER_PERTURBATION(2);
CERES_TAKE_1ST_ORDER_PERTURBATION(3);
CERES_TAKE_1ST_ORDER_PERTURBATION(4);
CERES_TAKE_1ST_ORDER_PERTURBATION(5);
CERES_TAKE_1ST_ORDER_PERTURBATION(6);
CERES_TAKE_1ST_ORDER_PERTURBATION(7);
CERES_TAKE_1ST_ORDER_PERTURBATION(8);
CERES_TAKE_1ST_ORDER_PERTURBATION(9);
#undef CERES_TAKE_1ST_ORDER_PERTURBATION
return true;
}
};
} // namespace internal
} // namespace ceres
#endif // CERES_PUBLIC_INTERNAL_AUTODIFF_H_

93
gtsam_unstable/nonlinear/ceres_eigen.h Normal file
View File

@ -0,0 +1,93 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
#ifndef CERES_INTERNAL_EIGEN_H_
#define CERES_INTERNAL_EIGEN_H_
#include <gtsam/3rdparty/gtsam_eigen_includes.h>
namespace ceres {
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;
typedef Eigen::Matrix<double,
Eigen::Dynamic,
Eigen::Dynamic,
Eigen::RowMajor> Matrix;
typedef Eigen::Map<Vector> VectorRef;
typedef Eigen::Map<Matrix> MatrixRef;
typedef Eigen::Map<const Vector> ConstVectorRef;
typedef Eigen::Map<const Matrix> ConstMatrixRef;
// Column major matrices for DenseSparseMatrix/DenseQRSolver
typedef Eigen::Matrix<double,
Eigen::Dynamic,
Eigen::Dynamic,
Eigen::ColMajor> ColMajorMatrix;
typedef Eigen::Map<ColMajorMatrix, 0,
Eigen::Stride<Eigen::Dynamic, 1> > ColMajorMatrixRef;
typedef Eigen::Map<const ColMajorMatrix,
0,
Eigen::Stride<Eigen::Dynamic, 1> > ConstColMajorMatrixRef;
// C++ does not support templated typdefs, thus the need for this
// struct so that we can support statically sized Matrix and Maps.
template <int num_rows = Eigen::Dynamic, int num_cols = Eigen::Dynamic>
struct EigenTypes {
typedef Eigen::Matrix <double, num_rows, num_cols, Eigen::RowMajor>
Matrix;
typedef Eigen::Map<
Eigen::Matrix<double, num_rows, num_cols, Eigen::RowMajor> >
MatrixRef;
typedef Eigen::Matrix <double, num_rows, 1>
Vector;
typedef Eigen::Map <
Eigen::Matrix<double, num_rows, 1> >
VectorRef;
typedef Eigen::Map<
const Eigen::Matrix<double, num_rows, num_cols, Eigen::RowMajor> >
ConstMatrixRef;
typedef Eigen::Map <
const Eigen::Matrix<double, num_rows, 1> >
ConstVectorRef;
};
} // namespace ceres
#endif // CERES_INTERNAL_EIGEN_H_

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

@ -0,0 +1,190 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: rennie@google.com (Jeffrey Rennie)
// Author: sanjay@google.com (Sanjay Ghemawat) -- renamed to FixedArray
#ifndef CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_
#define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_
#include <cstddef>
#include <gtsam/3rdparty/gtsam_eigen_includes.h>
#include <gtsam_unstable/nonlinear/ceres_macros.h>
#include <gtsam_unstable/nonlinear/ceres_manual_constructor.h>
namespace ceres {
namespace internal {
// A FixedArray<T> represents a non-resizable array of T where the
// length of the array does not need to be a compile time constant.
//
// FixedArray allocates small arrays inline, and large arrays on
// the heap. It is a good replacement for non-standard and deprecated
// uses of alloca() and variable length arrays (a GCC extension).
//
// FixedArray keeps performance fast for small arrays, because it
// avoids heap operations. It also helps reduce the chances of
// accidentally overflowing your stack if large input is passed to
// your function.
//
// Also, FixedArray is useful for writing portable code. Not all
// compilers support arrays of dynamic size.
// Most users should not specify an inline_elements argument and let
// FixedArray<> automatically determine the number of elements
// to store inline based on sizeof(T).
//
// If inline_elements is specified, the FixedArray<> implementation
// will store arrays of length <= inline_elements inline.
//
// Finally note that unlike vector<T> FixedArray<T> will not zero-initialize
// simple types like int, double, bool, etc.
//
// Non-POD types will be default-initialized just like regular vectors or
// arrays.
#if defined(_WIN64)
typedef __int64 ssize_t;
#elif defined(_WIN32)
typedef __int32 ssize_t;
#endif
template <typename T, ssize_t inline_elements = -1>
class FixedArray {
public:
// For playing nicely with stl:
typedef T value_type;
typedef T* iterator;
typedef T const* const_iterator;
typedef T& reference;
typedef T const& const_reference;
typedef T* pointer;
typedef std::ptrdiff_t difference_type;
typedef size_t size_type;
// REQUIRES: n >= 0
// Creates an array object that can store "n" elements.
//
// FixedArray<T> will not zero-initialiaze POD (simple) types like int,
// double, bool, etc.
// Non-POD types will be default-initialized just like regular vectors or
// arrays.
explicit FixedArray(size_type n);
// Releases any resources.
~FixedArray();
// Returns the length of the array.
inline size_type size() const { return size_; }
// Returns the memory size of the array in bytes.
inline size_t memsize() const { return size_ * sizeof(T); }
// Returns a pointer to the underlying element array.
inline const T* get() const { return &array_[0].element; }
inline T* get() { return &array_[0].element; }
// REQUIRES: 0 <= i < size()
// Returns a reference to the "i"th element.
inline T& operator[](size_type i) {
DCHECK_LT(i, size_);
return array_[i].element;
}
// REQUIRES: 0 <= i < size()
// Returns a reference to the "i"th element.
inline const T& operator[](size_type i) const {
DCHECK_LT(i, size_);
return array_[i].element;
}
inline iterator begin() { return &array_[0].element; }
inline iterator end() { return &array_[size_].element; }
inline const_iterator begin() const { return &array_[0].element; }
inline const_iterator end() const { return &array_[size_].element; }
private:
// Container to hold elements of type T. This is necessary to handle
// the case where T is a a (C-style) array. The size of InnerContainer
// and T must be the same, otherwise callers' assumptions about use
// of this code will be broken.
struct InnerContainer {
T element;
};
// How many elements should we store inline?
// a. If not specified, use a default of 256 bytes (256 bytes
// seems small enough to not cause stack overflow or unnecessary
// stack pollution, while still allowing stack allocation for
// reasonably long character arrays.
// b. Never use 0 length arrays (not ISO C++)
static const size_type S1 = ((inline_elements < 0)
? (256/sizeof(T)) : inline_elements);
static const size_type S2 = (S1 <= 0) ? 1 : S1;
static const size_type kInlineElements = S2;
size_type const size_;
InnerContainer* const array_;
// Allocate some space, not an array of elements of type T, so that we can
// skip calling the T constructors and destructors for space we never use.
ManualConstructor<InnerContainer> inline_space_[kInlineElements];
};
// Implementation details follow
template <class T, ssize_t S>
inline FixedArray<T, S>::FixedArray(typename FixedArray<T, S>::size_type n)
: size_(n),
array_((n <= kInlineElements
? reinterpret_cast<InnerContainer*>(inline_space_)
: new InnerContainer[n])) {
// Construct only the elements actually used.
if (array_ == reinterpret_cast<InnerContainer*>(inline_space_)) {
for (size_t i = 0; i != size_; ++i) {
inline_space_[i].Init();
}
}
}
template <class T, ssize_t S>
inline FixedArray<T, S>::~FixedArray() {
if (array_ != reinterpret_cast<InnerContainer*>(inline_space_)) {
delete[] array_;
} else {
for (size_t i = 0; i != size_; ++i) {
inline_space_[i].Destroy();
}
}
}
} // namespace internal
} // namespace ceres
#endif // CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_

87
gtsam_unstable/nonlinear/ceres_fpclassify.h Normal file
View File

@ -0,0 +1,87 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: keir@google.com (Keir Mierle)
//
// Portable floating point classification. The names are picked such that they
// do not collide with macros. For example, "isnan" in C99 is a macro and hence
// does not respect namespaces.
//
// TODO(keir): Finish porting!
#ifndef CERES_PUBLIC_FPCLASSIFY_H_
#define CERES_PUBLIC_FPCLASSIFY_H_
#if defined(_MSC_VER)
#include <float.h>
#endif
#include <limits>
namespace ceres {
#if defined(_MSC_VER)
inline bool IsFinite (double x) { return _finite(x) != 0; }
inline bool IsInfinite(double x) { return _finite(x) == 0 && _isnan(x) == 0; }
inline bool IsNaN (double x) { return _isnan(x) != 0; }
inline bool IsNormal (double x) {
int classification = _fpclass(x);
return classification == _FPCLASS_NN ||
classification == _FPCLASS_PN;
}
#elif defined(ANDROID) && defined(_STLPORT_VERSION)
// On Android, when using the STLPort, the C++ isnan and isnormal functions
// are defined as macros.
inline bool IsNaN (double x) { return isnan(x); }
inline bool IsNormal (double x) { return isnormal(x); }
// On Android NDK r6, when using STLPort, the isinf and isfinite functions are
// not available, so reimplement them.
inline bool IsInfinite(double x) {
return x == std::numeric_limits<double>::infinity() ||
x == -std::numeric_limits<double>::infinity();
}
inline bool IsFinite(double x) {
return !isnan(x) && !IsInfinite(x);
}
# else
// These definitions are for the normal Unix suspects.
inline bool IsFinite (double x) { return std::isfinite(x); }
inline bool IsInfinite(double x) { return std::isinf(x); }
inline bool IsNaN (double x) { return std::isnan(x); }
inline bool IsNormal (double x) { return std::isnormal(x); }
#endif
} // namespace ceres
#endif // CERES_PUBLIC_FPCLASSIFY_H_

670
gtsam_unstable/nonlinear/ceres_jet.h Normal file
View File

@ -0,0 +1,670 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: keir@google.com (Keir Mierle)
//
// A simple implementation of N-dimensional dual numbers, for automatically
// computing exact derivatives of functions.
//
// While a complete treatment of the mechanics of automatic differentation is
// beyond the scope of this header (see
// http://en.wikipedia.org/wiki/Automatic_differentiation for details), the
// basic idea is to extend normal arithmetic with an extra element, "e," often
// denoted with the greek symbol epsilon, such that e != 0 but e^2 = 0. Dual
// numbers are extensions of the real numbers analogous to complex numbers:
// whereas complex numbers augment the reals by introducing an imaginary unit i
// such that i^2 = -1, dual numbers introduce an "infinitesimal" unit e such
// that e^2 = 0. Dual numbers have two components: the "real" component and the
// "infinitesimal" component, generally written as x + y*e. Surprisingly, this
// leads to a convenient method for computing exact derivatives without needing
// to manipulate complicated symbolic expressions.
//
// For example, consider the function
//
// f(x) = x^2 ,
//
// evaluated at 10. Using normal arithmetic, f(10) = 100, and df/dx(10) = 20.
// Next, augument 10 with an infinitesimal to get:
//
// f(10 + e) = (10 + e)^2
// = 100 + 2 * 10 * e + e^2
// = 100 + 20 * e -+-
// -- |
// | +--- This is zero, since e^2 = 0
// |
// +----------------- This is df/dx!
//
// Note that the derivative of f with respect to x is simply the infinitesimal
// component of the value of f(x + e). So, in order to take the derivative of
// any function, it is only necessary to replace the numeric "object" used in
// the function with one extended with infinitesimals. The class Jet, defined in
// this header, is one such example of this, where substitution is done with
// templates.
//
// To handle derivatives of functions taking multiple arguments, different
// infinitesimals are used, one for each variable to take the derivative of. For
// example, consider a scalar function of two scalar parameters x and y:
//
// f(x, y) = x^2 + x * y
//
// Following the technique above, to compute the derivatives df/dx and df/dy for
// f(1, 3) involves doing two evaluations of f, the first time replacing x with
// x + e, the second time replacing y with y + e.
//
// For df/dx:
//
// f(1 + e, y) = (1 + e)^2 + (1 + e) * 3
// = 1 + 2 * e + 3 + 3 * e
// = 4 + 5 * e
//
// --> df/dx = 5
//
// For df/dy:
//
// f(1, 3 + e) = 1^2 + 1 * (3 + e)
// = 1 + 3 + e
// = 4 + e
//
// --> df/dy = 1
//
// To take the gradient of f with the implementation of dual numbers ("jets") in
// this file, it is necessary to create a single jet type which has components
// for the derivative in x and y, and passing them to a templated version of f:
//
// template<typename T>
// T f(const T &x, const T &y) {
// return x * x + x * y;
// }
//
// // The "2" means there should be 2 dual number components.
// Jet<double, 2> x(0); // Pick the 0th dual number for x.
// Jet<double, 2> y(1); // Pick the 1st dual number for y.
// Jet<double, 2> z = f(x, y);
//
// LOG(INFO) << "df/dx = " << z.a[0]
// << "df/dy = " << z.a[1];
//
// Most users should not use Jet objects directly; a wrapper around Jet objects,
// which makes computing the derivative, gradient, or jacobian of templated
// functors simple, is in autodiff.h. Even autodiff.h should not be used
// directly; instead autodiff_cost_function.h is typically the file of interest.
//
// For the more mathematically inclined, this file implements first-order
// "jets". A 1st order jet is an element of the ring
//
// T[N] = T[t_1, ..., t_N] / (t_1, ..., t_N)^2
//
// which essentially means that each jet consists of a "scalar" value 'a' from T
// and a 1st order perturbation vector 'v' of length N:
//
// x = a + \sum_i v[i] t_i
//
// A shorthand is to write an element as x = a + u, where u is the pertubation.
// Then, the main point about the arithmetic of jets is that the product of
// perturbations is zero:
//
// (a + u) * (b + v) = ab + av + bu + uv
// = ab + (av + bu) + 0
//
// which is what operator* implements below. Addition is simpler:
//
// (a + u) + (b + v) = (a + b) + (u + v).
//
// The only remaining question is how to evaluate the function of a jet, for
// which we use the chain rule:
//
// f(a + u) = f(a) + f'(a) u
//
// where f'(a) is the (scalar) derivative of f at a.
//
// By pushing these things through sufficiently and suitably templated
// functions, we can do automatic differentiation. Just be sure to turn on
// function inlining and common-subexpression elimination, or it will be very
// slow!
//
// WARNING: Most Ceres users should not directly include this file or know the
// details of how jets work. Instead the suggested method for automatic
// derivatives is to use autodiff_cost_function.h, which is a wrapper around
// both jets.h and autodiff.h to make taking derivatives of cost functions for
// use in Ceres easier.
#ifndef CERES_PUBLIC_JET_H_
#define CERES_PUBLIC_JET_H_
#include <cmath>
#include <iosfwd>
#include <iostream> // NOLINT
#include <limits>
#include <string>
#include <gtsam/3rdparty/gtsam_eigen_includes.h>
#include <gtsam_unstable/nonlinear/ceres_fpclassify.h>
namespace ceres {
template <typename T, int N>
struct Jet {
enum { DIMENSION = N };
// Default-construct "a" because otherwise this can lead to false errors about
// uninitialized uses when other classes relying on default constructed T
// (where T is a Jet<T, N>). This usually only happens in opt mode. Note that
// the C++ standard mandates that e.g. default constructed doubles are
// initialized to 0.0; see sections 8.5 of the C++03 standard.
Jet() : a() {
v.setZero();
}
// Constructor from scalar: a + 0.
explicit Jet(const T& value) {
a = value;
v.setZero();
}
// Constructor from scalar plus variable: a + t_i.
Jet(const T& value, int k) {
a = value;
v.setZero();
v[k] = T(1.0);
}
// Constructor from scalar and vector part
// The use of Eigen::DenseBase allows Eigen expressions
// to be passed in without being fully evaluated until
// they are assigned to v
template<typename Derived>
EIGEN_STRONG_INLINE Jet(const T& a, const Eigen::DenseBase<Derived> &v)
: a(a), v(v) {
}
// Compound operators
Jet<T, N>& operator+=(const Jet<T, N> &y) {
*this = *this + y;
return *this;
}
Jet<T, N>& operator-=(const Jet<T, N> &y) {
*this = *this - y;
return *this;
}
Jet<T, N>& operator*=(const Jet<T, N> &y) {
*this = *this * y;
return *this;
}
Jet<T, N>& operator/=(const Jet<T, N> &y) {
*this = *this / y;
return *this;
}
// The scalar part.
T a;
// The infinitesimal part.
//
// Note the Eigen::DontAlign bit is needed here because this object
// gets allocated on the stack and as part of other arrays and
// structs. Forcing the right alignment there is the source of much
// pain and suffering. Even if that works, passing Jets around to
// functions by value has problems because the C++ ABI does not
// guarantee alignment for function arguments.
//
// Setting the DontAlign bit prevents Eigen from using SSE for the
// various operations on Jets. This is a small performance penalty
// since the AutoDiff code will still expose much of the code as
// statically sized loops to the compiler. But given the subtle
// issues that arise due to alignment, especially when dealing with
// multiple platforms, it seems to be a trade off worth making.
Eigen::Matrix<T, N, 1, Eigen::DontAlign> v;
};
// Unary +
template<typename T, int N> inline
Jet<T, N> const& operator+(const Jet<T, N>& f) {
return f;
}
// TODO(keir): Try adding __attribute__((always_inline)) to these functions to
// see if it causes a performance increase.
// Unary -
template<typename T, int N> inline
Jet<T, N> operator-(const Jet<T, N>&f) {
return Jet<T, N>(-f.a, -f.v);
}
// Binary +
template<typename T, int N> inline
Jet<T, N> operator+(const Jet<T, N>& f,
const Jet<T, N>& g) {
return Jet<T, N>(f.a + g.a, f.v + g.v);
}
// Binary + with a scalar: x + s
template<typename T, int N> inline
Jet<T, N> operator+(const Jet<T, N>& f, T s) {
return Jet<T, N>(f.a + s, f.v);
}
// Binary + with a scalar: s + x
template<typename T, int N> inline
Jet<T, N> operator+(T s, const Jet<T, N>& f) {
return Jet<T, N>(f.a + s, f.v);
}
// Binary -
template<typename T, int N> inline
Jet<T, N> operator-(const Jet<T, N>& f,
const Jet<T, N>& g) {
return Jet<T, N>(f.a - g.a, f.v - g.v);
}
// Binary - with a scalar: x - s
template<typename T, int N> inline
Jet<T, N> operator-(const Jet<T, N>& f, T s) {
return Jet<T, N>(f.a - s, f.v);
}
// Binary - with a scalar: s - x
template<typename T, int N> inline
Jet<T, N> operator-(T s, const Jet<T, N>& f) {
return Jet<T, N>(s - f.a, -f.v);
}
// Binary *
template<typename T, int N> inline
Jet<T, N> operator*(const Jet<T, N>& f,
const Jet<T, N>& g) {
return Jet<T, N>(f.a * g.a, f.a * g.v + f.v * g.a);
}
// Binary * with a scalar: x * s
template<typename T, int N> inline
Jet<T, N> operator*(const Jet<T, N>& f, T s) {
return Jet<T, N>(f.a * s, f.v * s);
}
// Binary * with a scalar: s * x
template<typename T, int N> inline
Jet<T, N> operator*(T s, const Jet<T, N>& f) {
return Jet<T, N>(f.a * s, f.v * s);
}
// Binary /
template<typename T, int N> inline
Jet<T, N> operator/(const Jet<T, N>& f,
const Jet<T, N>& g) {
// This uses:
//
// a + u (a + u)(b - v) (a + u)(b - v)
// ----- = -------------- = --------------
// b + v (b + v)(b - v) b^2
//
// which holds because v*v = 0.
const T g_a_inverse = T(1.0) / g.a;
const T f_a_by_g_a = f.a * g_a_inverse;
return Jet<T, N>(f.a * g_a_inverse, (f.v - f_a_by_g_a * g.v) * g_a_inverse);
}
// Binary / with a scalar: s / x
template<typename T, int N> inline
Jet<T, N> operator/(T s, const Jet<T, N>& g) {
const T minus_s_g_a_inverse2 = -s / (g.a * g.a);
return Jet<T, N>(s / g.a, g.v * minus_s_g_a_inverse2);
}
// Binary / with a scalar: x / s
template<typename T, int N> inline
Jet<T, N> operator/(const Jet<T, N>& f, T s) {
const T s_inverse = 1.0 / s;
return Jet<T, N>(f.a * s_inverse, f.v * s_inverse);
}
// Binary comparison operators for both scalars and jets.
#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \
template<typename T, int N> inline \
bool operator op(const Jet<T, N>& f, const Jet<T, N>& g) { \
return f.a op g.a; \
} \
template<typename T, int N> inline \
bool operator op(const T& s, const Jet<T, N>& g) { \
return s op g.a; \
} \
template<typename T, int N> inline \
bool operator op(const Jet<T, N>& f, const T& s) { \
return f.a op s; \
}
CERES_DEFINE_JET_COMPARISON_OPERATOR( < ) // NOLINT
CERES_DEFINE_JET_COMPARISON_OPERATOR( <= ) // NOLINT
CERES_DEFINE_JET_COMPARISON_OPERATOR( > ) // NOLINT
CERES_DEFINE_JET_COMPARISON_OPERATOR( >= ) // NOLINT
CERES_DEFINE_JET_COMPARISON_OPERATOR( == ) // NOLINT
CERES_DEFINE_JET_COMPARISON_OPERATOR( != ) // NOLINT
#undef CERES_DEFINE_JET_COMPARISON_OPERATOR
// Pull some functions from namespace std.
//
// This is necessary because we want to use the same name (e.g. 'sqrt') for
// double-valued and Jet-valued functions, but we are not allowed to put
// Jet-valued functions inside namespace std.
//
// TODO(keir): Switch to "using".
inline double abs (double x) { return std::abs(x); }
inline double log (double x) { return std::log(x); }
inline double exp (double x) { return std::exp(x); }
inline double sqrt (double x) { return std::sqrt(x); }
inline double cos (double x) { return std::cos(x); }
inline double acos (double x) { return std::acos(x); }
inline double sin (double x) { return std::sin(x); }
inline double asin (double x) { return std::asin(x); }
inline double tan (double x) { return std::tan(x); }
inline double atan (double x) { return std::atan(x); }
inline double sinh (double x) { return std::sinh(x); }
inline double cosh (double x) { return std::cosh(x); }
inline double tanh (double x) { return std::tanh(x); }
inline double pow (double x, double y) { return std::pow(x, y); }
inline double atan2(double y, double x) { return std::atan2(y, x); }
// In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule.
// abs(x + h) ~= x + h or -(x + h)
template <typename T, int N> inline
Jet<T, N> abs(const Jet<T, N>& f) {
return f.a < T(0.0) ? -f : f;
}
// log(a + h) ~= log(a) + h / a
template <typename T, int N> inline
Jet<T, N> log(const Jet<T, N>& f) {
const T a_inverse = T(1.0) / f.a;
return Jet<T, N>(log(f.a), f.v * a_inverse);
}
// exp(a + h) ~= exp(a) + exp(a) h
template <typename T, int N> inline
Jet<T, N> exp(const Jet<T, N>& f) {
const T tmp = exp(f.a);
return Jet<T, N>(tmp, tmp * f.v);
}
// sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a))
template <typename T, int N> inline
Jet<T, N> sqrt(const Jet<T, N>& f) {
const T tmp = sqrt(f.a);
const T two_a_inverse = T(1.0) / (T(2.0) * tmp);
return Jet<T, N>(tmp, f.v * two_a_inverse);
}
// cos(a + h) ~= cos(a) - sin(a) h
template <typename T, int N> inline
Jet<T, N> cos(const Jet<T, N>& f) {
return Jet<T, N>(cos(f.a), - sin(f.a) * f.v);
}
// acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h
template <typename T, int N> inline
Jet<T, N> acos(const Jet<T, N>& f) {
const T tmp = - T(1.0) / sqrt(T(1.0) - f.a * f.a);
return Jet<T, N>(acos(f.a), tmp * f.v);
}
// sin(a + h) ~= sin(a) + cos(a) h
template <typename T, int N> inline
Jet<T, N> sin(const Jet<T, N>& f) {
return Jet<T, N>(sin(f.a), cos(f.a) * f.v);
}
// asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h
template <typename T, int N> inline
Jet<T, N> asin(const Jet<T, N>& f) {
const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a);
return Jet<T, N>(asin(f.a), tmp * f.v);
}
// tan(a + h) ~= tan(a) + (1 + tan(a)^2) h
template <typename T, int N> inline
Jet<T, N> tan(const Jet<T, N>& f) {
const T tan_a = tan(f.a);
const T tmp = T(1.0) + tan_a * tan_a;
return Jet<T, N>(tan_a, tmp * f.v);
}
// atan(a + h) ~= atan(a) + 1 / (1 + a^2) h
template <typename T, int N> inline
Jet<T, N> atan(const Jet<T, N>& f) {
const T tmp = T(1.0) / (T(1.0) + f.a * f.a);
return Jet<T, N>(atan(f.a), tmp * f.v);
}
// sinh(a + h) ~= sinh(a) + cosh(a) h
template <typename T, int N> inline
Jet<T, N> sinh(const Jet<T, N>& f) {
return Jet<T, N>(sinh(f.a), cosh(f.a) * f.v);
}
// cosh(a + h) ~= cosh(a) + sinh(a) h
template <typename T, int N> inline
Jet<T, N> cosh(const Jet<T, N>& f) {
return Jet<T, N>(cosh(f.a), sinh(f.a) * f.v);
}
// tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h
template <typename T, int N> inline
Jet<T, N> tanh(const Jet<T, N>& f) {
const T tanh_a = tanh(f.a);
const T tmp = T(1.0) - tanh_a * tanh_a;
return Jet<T, N>(tanh_a, tmp * f.v);
}
// Jet Classification. It is not clear what the appropriate semantics are for
// these classifications. This picks that IsFinite and isnormal are "all"
// operations, i.e. all elements of the jet must be finite for the jet itself
// to be finite (or normal). For IsNaN and IsInfinite, the answer is less
// clear. This takes a "any" approach for IsNaN and IsInfinite such that if any
// part of a jet is nan or inf, then the entire jet is nan or inf. This leads
// to strange situations like a jet can be both IsInfinite and IsNaN, but in
// practice the "any" semantics are the most useful for e.g. checking that
// derivatives are sane.
// The jet is finite if all parts of the jet are finite.
template <typename T, int N> inline
bool IsFinite(const Jet<T, N>& f) {
if (!IsFinite(f.a)) {
return false;
}
for (int i = 0; i < N; ++i) {
if (!IsFinite(f.v[i])) {
return false;
}
}
return true;
}
// The jet is infinite if any part of the jet is infinite.
template <typename T, int N> inline
bool IsInfinite(const Jet<T, N>& f) {
if (IsInfinite(f.a)) {
return true;
}
for (int i = 0; i < N; i++) {
if (IsInfinite(f.v[i])) {
return true;
}
}
return false;
}
// The jet is NaN if any part of the jet is NaN.
template <typename T, int N> inline
bool IsNaN(const Jet<T, N>& f) {
if (IsNaN(f.a)) {
return true;
}
for (int i = 0; i < N; ++i) {
if (IsNaN(f.v[i])) {
return true;
}
}
return false;
}
// The jet is normal if all parts of the jet are normal.
template <typename T, int N> inline
bool IsNormal(const Jet<T, N>& f) {
if (!IsNormal(f.a)) {
return false;
}
for (int i = 0; i < N; ++i) {
if (!IsNormal(f.v[i])) {
return false;
}
}
return true;
}
// atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2)
//
// In words: the rate of change of theta is 1/r times the rate of
// change of (x, y) in the positive angular direction.
template <typename T, int N> inline
Jet<T, N> atan2(const Jet<T, N>& g, const Jet<T, N>& f) {
// Note order of arguments:
//
// f = a + da
// g = b + db
T const tmp = T(1.0) / (f.a * f.a + g.a * g.a);
return Jet<T, N>(atan2(g.a, f.a), tmp * (- g.a * f.v + f.a * g.v));
}
// pow -- base is a differentiable function, exponent is a constant.
// (a+da)^p ~= a^p + p*a^(p-1) da
template <typename T, int N> inline
Jet<T, N> pow(const Jet<T, N>& f, double g) {
T const tmp = g * pow(f.a, g - T(1.0));
return Jet<T, N>(pow(f.a, g), tmp * f.v);
}
// pow -- base is a constant, exponent is a differentiable function.
// (a)^(p+dp) ~= a^p + a^p log(a) dp
template <typename T, int N> inline
Jet<T, N> pow(double f, const Jet<T, N>& g) {
T const tmp = pow(f, g.a);
return Jet<T, N>(tmp, log(f) * tmp * g.v);
}
// pow -- both base and exponent are differentiable functions.
// (a+da)^(b+db) ~= a^b + b * a^(b-1) da + a^b log(a) * db
template <typename T, int N> inline
Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) {
T const tmp1 = pow(f.a, g.a);
T const tmp2 = g.a * pow(f.a, g.a - T(1.0));
T const tmp3 = tmp1 * log(f.a);
return Jet<T, N>(tmp1, tmp2 * f.v + tmp3 * g.v);
}
// Define the helper functions Eigen needs to embed Jet types.
//
// NOTE(keir): machine_epsilon() and precision() are missing, because they don't
// work with nested template types (e.g. where the scalar is itself templated).
// Among other things, this means that decompositions of Jet's does not work,
// for example
//
// Matrix<Jet<T, N> ... > A, x, b;
// ...
// A.solve(b, &x)
//
// does not work and will fail with a strange compiler error.
//
// TODO(keir): This is an Eigen 2.0 limitation that is lifted in 3.0. When we
// switch to 3.0, also add the rest of the specialization functionality.
template<typename T, int N> inline const Jet<T, N>& ei_conj(const Jet<T, N>& x) { return x; } // NOLINT
template<typename T, int N> inline const Jet<T, N>& ei_real(const Jet<T, N>& x) { return x; } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_imag(const Jet<T, N>& ) { return Jet<T, N>(0.0); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_abs (const Jet<T, N>& x) { return fabs(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_abs2(const Jet<T, N>& x) { return x * x; } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_sqrt(const Jet<T, N>& x) { return sqrt(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_exp (const Jet<T, N>& x) { return exp(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_log (const Jet<T, N>& x) { return log(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_sin (const Jet<T, N>& x) { return sin(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_cos (const Jet<T, N>& x) { return cos(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_tan (const Jet<T, N>& x) { return tan(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_atan(const Jet<T, N>& x) { return atan(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_sinh(const Jet<T, N>& x) { return sinh(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_cosh(const Jet<T, N>& x) { return cosh(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_tanh(const Jet<T, N>& x) { return tanh(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_pow (const Jet<T, N>& x, Jet<T, N> y) { return pow(x, y); } // NOLINT
// Note: This has to be in the ceres namespace for argument dependent lookup to
// function correctly. Otherwise statements like CHECK_LE(x, 2.0) fail with
// strange compile errors.
template <typename T, int N>
inline std::ostream &operator<<(std::ostream &s, const Jet<T, N>& z) {
return s << "[" << z.a << " ; " << z.v.transpose() << "]";
}
} // namespace ceres
namespace Eigen {
// Creating a specialization of NumTraits enables placing Jet objects inside
// Eigen arrays, getting all the goodness of Eigen combined with autodiff.
template<typename T, int N>
struct NumTraits<ceres::Jet<T, N> > {
typedef ceres::Jet<T, N> Real;
typedef ceres::Jet<T, N> NonInteger;
typedef ceres::Jet<T, N> Nested;
static typename ceres::Jet<T, N> dummy_precision() {
return ceres::Jet<T, N>(1e-12);
}
static inline Real epsilon() {
return Real(std::numeric_limits<T>::epsilon());
}
enum {
IsComplex = 0,
IsInteger = 0,
IsSigned,
ReadCost = 1,
AddCost = 1,
// For Jet types, multiplication is more expensive than addition.
MulCost = 3,
HasFloatingPoint = 1,
RequireInitialization = 1
};
};
} // namespace Eigen
#endif // CERES_PUBLIC_JET_H_

170
gtsam_unstable/nonlinear/ceres_macros.h Normal file
View File

@ -0,0 +1,170 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
//
// Various Google-specific macros.
//
// This code is compiled directly on many platforms, including client
// platforms like Windows, Mac, and embedded systems. Before making
// any changes here, make sure that you're not breaking any platforms.
#ifndef CERES_PUBLIC_INTERNAL_MACROS_H_
#define CERES_PUBLIC_INTERNAL_MACROS_H_
#include <cstddef> // For size_t.
// A macro to disallow the copy constructor and operator= functions
// This should be used in the private: declarations for a class
//
// For disallowing only assign or copy, write the code directly, but declare
// the intend in a comment, for example:
//
// void operator=(const TypeName&); // _DISALLOW_ASSIGN
// Note, that most uses of CERES_DISALLOW_ASSIGN and CERES_DISALLOW_COPY
// are broken semantically, one should either use disallow both or
// neither. Try to avoid these in new code.
#define CERES_DISALLOW_COPY_AND_ASSIGN(TypeName) \
TypeName(const TypeName&); \
void operator=(const TypeName&)
// A macro to disallow all the implicit constructors, namely the
// default constructor, copy constructor and operator= functions.
//
// This should be used in the private: declarations for a class
// that wants to prevent anyone from instantiating it. This is
// especially useful for classes containing only static methods.
#define CERES_DISALLOW_IMPLICIT_CONSTRUCTORS(TypeName) \
TypeName(); \
CERES_DISALLOW_COPY_AND_ASSIGN(TypeName)
// The arraysize(arr) macro returns the # of elements in an array arr.
// The expression is a compile-time constant, and therefore can be
// used in defining new arrays, for example. If you use arraysize on
// a pointer by mistake, you will get a compile-time error.
//
// One caveat is that arraysize() doesn't accept any array of an
// anonymous type or a type defined inside a function. In these rare
// cases, you have to use the unsafe ARRAYSIZE() macro below. This is
// due to a limitation in C++'s template system. The limitation might
// eventually be removed, but it hasn't happened yet.
// This template function declaration is used in defining arraysize.
// Note that the function doesn't need an implementation, as we only
// use its type.
template <typename T, size_t N>
char (&ArraySizeHelper(T (&array)[N]))[N];
// That gcc wants both of these prototypes seems mysterious. VC, for
// its part, can't decide which to use (another mystery). Matching of
// template overloads: the final frontier.
#ifndef _WIN32
template <typename T, size_t N>
char (&ArraySizeHelper(const T (&array)[N]))[N];
#endif
#define arraysize(array) (sizeof(ArraySizeHelper(array)))
// ARRAYSIZE performs essentially the same calculation as arraysize,
// but can be used on anonymous types or types defined inside
// functions. It's less safe than arraysize as it accepts some
// (although not all) pointers. Therefore, you should use arraysize
// whenever possible.
//
// The expression ARRAYSIZE(a) is a compile-time constant of type
// size_t.
//
// ARRAYSIZE catches a few type errors. If you see a compiler error
//
// "warning: division by zero in ..."
//
// when using ARRAYSIZE, you are (wrongfully) giving it a pointer.
// You should only use ARRAYSIZE on statically allocated arrays.
//
// The following comments are on the implementation details, and can
// be ignored by the users.
//
// ARRAYSIZE(arr) works by inspecting sizeof(arr) (the # of bytes in
// the array) and sizeof(*(arr)) (the # of bytes in one array
// element). If the former is divisible by the latter, perhaps arr is
// indeed an array, in which case the division result is the # of
// elements in the array. Otherwise, arr cannot possibly be an array,
// and we generate a compiler error to prevent the code from
// compiling.
//
// Since the size of bool is implementation-defined, we need to cast
// !(sizeof(a) & sizeof(*(a))) to size_t in order to ensure the final
// result has type size_t.
//
// This macro is not perfect as it wrongfully accepts certain
// pointers, namely where the pointer size is divisible by the pointee
// size. Since all our code has to go through a 32-bit compiler,
// where a pointer is 4 bytes, this means all pointers to a type whose
// size is 3 or greater than 4 will be (righteously) rejected.
//
// Kudos to Jorg Brown for this simple and elegant implementation.
//
// - wan 2005-11-16
//
// Starting with Visual C++ 2005, WinNT.h includes ARRAYSIZE. However,
// the definition comes from the over-broad windows.h header that
// introduces a macro, ERROR, that conflicts with the logging framework
// that Ceres uses. Instead, rename ARRAYSIZE to CERES_ARRAYSIZE.
#define CERES_ARRAYSIZE(a) \
((sizeof(a) / sizeof(*(a))) / \
static_cast<size_t>(!(sizeof(a) % sizeof(*(a)))))
// Tell the compiler to warn about unused return values for functions
// declared with this macro. The macro should be used on function
// declarations following the argument list:
//
// Sprocket* AllocateSprocket() MUST_USE_RESULT;
//
#if (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4)) \
&& !defined(COMPILER_ICC)
#define CERES_MUST_USE_RESULT __attribute__ ((warn_unused_result))
#else
#define CERES_MUST_USE_RESULT
#endif
// Platform independent macros to get aligned memory allocations.
// For example
//
// MyFoo my_foo CERES_ALIGN_ATTRIBUTE(16);
//
// Gives us an instance of MyFoo which is aligned at a 16 byte
// boundary.
#if defined(_MSC_VER)
#define CERES_ALIGN_ATTRIBUTE(n) __declspec(align(n))
#define CERES_ALIGN_OF(T) __alignof(T)
#elif defined(__GNUC__)
#define CERES_ALIGN_ATTRIBUTE(n) __attribute__((aligned(n)))
#define CERES_ALIGN_OF(T) __alignof(T)
#endif
#endif // CERES_PUBLIC_INTERNAL_MACROS_H_

208
gtsam_unstable/nonlinear/ceres_manual_constructor.h Normal file
View File

@ -0,0 +1,208 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: kenton@google.com (Kenton Varda)
//
// ManualConstructor statically-allocates space in which to store some
// object, but does not initialize it. You can then call the constructor
// and destructor for the object yourself as you see fit. This is useful
// for memory management optimizations, where you want to initialize and
// destroy an object multiple times but only allocate it once.
//
// (When I say ManualConstructor statically allocates space, I mean that
// the ManualConstructor object itself is forced to be the right size.)
#ifndef CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_
#define CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_
#include <new>
namespace ceres {
namespace internal {
// ------- Define CERES_ALIGNED_CHAR_ARRAY --------------------------------
#ifndef CERES_ALIGNED_CHAR_ARRAY
// Because MSVC and older GCCs require that the argument to their alignment
// construct to be a literal constant integer, we use a template instantiated
// at all the possible powers of two.
template<int alignment, int size> struct AlignType { };
template<int size> struct AlignType<0, size> { typedef char result[size]; };
#if !defined(CERES_ALIGN_ATTRIBUTE)
#define CERES_ALIGNED_CHAR_ARRAY you_must_define_CERES_ALIGNED_CHAR_ARRAY_for_your_compiler
#else // !defined(CERES_ALIGN_ATTRIBUTE)
#define CERES_ALIGN_TYPE_TEMPLATE(X) \
template<int size> struct AlignType<X, size> { \
typedef CERES_ALIGN_ATTRIBUTE(X) char result[size]; \
}
CERES_ALIGN_TYPE_TEMPLATE(1);
CERES_ALIGN_TYPE_TEMPLATE(2);
CERES_ALIGN_TYPE_TEMPLATE(4);
CERES_ALIGN_TYPE_TEMPLATE(8);
CERES_ALIGN_TYPE_TEMPLATE(16);
CERES_ALIGN_TYPE_TEMPLATE(32);
CERES_ALIGN_TYPE_TEMPLATE(64);
CERES_ALIGN_TYPE_TEMPLATE(128);
CERES_ALIGN_TYPE_TEMPLATE(256);
CERES_ALIGN_TYPE_TEMPLATE(512);
CERES_ALIGN_TYPE_TEMPLATE(1024);
CERES_ALIGN_TYPE_TEMPLATE(2048);
CERES_ALIGN_TYPE_TEMPLATE(4096);
CERES_ALIGN_TYPE_TEMPLATE(8192);
// Any larger and MSVC++ will complain.
#undef CERES_ALIGN_TYPE_TEMPLATE
#define CERES_ALIGNED_CHAR_ARRAY(T, Size) \
typename AlignType<CERES_ALIGN_OF(T), sizeof(T) * Size>::result
#endif // !defined(CERES_ALIGN_ATTRIBUTE)
#endif // CERES_ALIGNED_CHAR_ARRAY
template <typename Type>
class ManualConstructor {
public:
// No constructor or destructor because one of the most useful uses of
// this class is as part of a union, and members of a union cannot have
// constructors or destructors. And, anyway, the whole point of this
// class is to bypass these.
inline Type* get() {
return reinterpret_cast<Type*>(space_);
}
inline const Type* get() const {
return reinterpret_cast<const Type*>(space_);
}
inline Type* operator->() { return get(); }
inline const Type* operator->() const { return get(); }
inline Type& operator*() { return *get(); }
inline const Type& operator*() const { return *get(); }
// This is needed to get around the strict aliasing warning GCC generates.
inline void* space() {
return reinterpret_cast<void*>(space_);
}
// You can pass up to four constructor arguments as arguments of Init().
inline void Init() {
new(space()) Type;
}
template <typename T1>
inline void Init(const T1& p1) {
new(space()) Type(p1);
}
template <typename T1, typename T2>
inline void Init(const T1& p1, const T2& p2) {
new(space()) Type(p1, p2);
}
template <typename T1, typename T2, typename T3>
inline void Init(const T1& p1, const T2& p2, const T3& p3) {
new(space()) Type(p1, p2, p3);
}
template <typename T1, typename T2, typename T3, typename T4>
inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4) {
new(space()) Type(p1, p2, p3, p4);
}
template <typename T1, typename T2, typename T3, typename T4, typename T5>
inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
const T5& p5) {
new(space()) Type(p1, p2, p3, p4, p5);
}
template <typename T1, typename T2, typename T3, typename T4, typename T5,
typename T6>
inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
const T5& p5, const T6& p6) {
new(space()) Type(p1, p2, p3, p4, p5, p6);
}
template <typename T1, typename T2, typename T3, typename T4, typename T5,
typename T6, typename T7>
inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
const T5& p5, const T6& p6, const T7& p7) {
new(space()) Type(p1, p2, p3, p4, p5, p6, p7);
}
template <typename T1, typename T2, typename T3, typename T4, typename T5,
typename T6, typename T7, typename T8>
inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
const T5& p5, const T6& p6, const T7& p7, const T8& p8) {
new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8);
}
template <typename T1, typename T2, typename T3, typename T4, typename T5,
typename T6, typename T7, typename T8, typename T9>
inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
const T5& p5, const T6& p6, const T7& p7, const T8& p8,
const T9& p9) {
new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9);
}
template <typename T1, typename T2, typename T3, typename T4, typename T5,
typename T6, typename T7, typename T8, typename T9, typename T10>
inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
const T5& p5, const T6& p6, const T7& p7, const T8& p8,
const T9& p9, const T10& p10) {
new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10);
}
template <typename T1, typename T2, typename T3, typename T4, typename T5,
typename T6, typename T7, typename T8, typename T9, typename T10,
typename T11>
inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
const T5& p5, const T6& p6, const T7& p7, const T8& p8,
const T9& p9, const T10& p10, const T11& p11) {
new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, p11);
}
inline void Destroy() {
get()->~Type();
}
private:
CERES_ALIGNED_CHAR_ARRAY(Type, 1) space_;
};
#undef CERES_ALIGNED_CHAR_ARRAY
} // namespace internal
} // namespace ceres
#endif // CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_

644
gtsam_unstable/nonlinear/ceres_rotation.h Normal file
View File

@ -0,0 +1,644 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: keir@google.com (Keir Mierle)
// sameeragarwal@google.com (Sameer Agarwal)
//
// Templated functions for manipulating rotations. The templated
// functions are useful when implementing functors for automatic
// differentiation.
//
// In the following, the Quaternions are laid out as 4-vectors, thus:
//
// q[0] scalar part.
// q[1] coefficient of i.
// q[2] coefficient of j.
// q[3] coefficient of k.
//
// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j.
#ifndef CERES_PUBLIC_ROTATION_H_
#define CERES_PUBLIC_ROTATION_H_
#include <algorithm>
#include <cmath>
namespace ceres {
// Trivial wrapper to index linear arrays as matrices, given a fixed
// column and row stride. When an array "T* array" is wrapped by a
//
// (const) MatrixAdapter<T, row_stride, col_stride> M"
//
// the expression M(i, j) is equivalent to
//
// arrary[i * row_stride + j * col_stride]
//
// Conversion functions to and from rotation matrices accept
// MatrixAdapters to permit using row-major and column-major layouts,
// and rotation matrices embedded in larger matrices (such as a 3x4
// projection matrix).
template <typename T, int row_stride, int col_stride>
struct MatrixAdapter;
// Convenience functions to create a MatrixAdapter that treats the
// array pointed to by "pointer" as a 3x3 (contiguous) column-major or
// row-major matrix.
template <typename T>
MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer);
template <typename T>
MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer);
// Convert a value in combined axis-angle representation to a quaternion.
// The value angle_axis is a triple whose norm is an angle in radians,
// and whose direction is aligned with the axis of rotation,
// and quaternion is a 4-tuple that will contain the resulting quaternion.
// The implementation may be used with auto-differentiation up to the first
// derivative, higher derivatives may have unexpected results near the origin.
template<typename T>
void AngleAxisToQuaternion(const T* angle_axis, T* quaternion);
// Convert a quaternion to the equivalent combined axis-angle representation.
// The value quaternion must be a unit quaternion - it is not normalized first,
// and angle_axis will be filled with a value whose norm is the angle of
// rotation in radians, and whose direction is the axis of rotation.
// The implemention may be used with auto-differentiation up to the first
// derivative, higher derivatives may have unexpected results near the origin.
template<typename T>
void QuaternionToAngleAxis(const T* quaternion, T* angle_axis);
// Conversions between 3x3 rotation matrix (in column major order) and
// axis-angle rotation representations. Templated for use with
// autodifferentiation.
template <typename T>
void RotationMatrixToAngleAxis(const T* R, T* angle_axis);
template <typename T, int row_stride, int col_stride>
void RotationMatrixToAngleAxis(
const MatrixAdapter<const T, row_stride, col_stride>& R,
T* angle_axis);
template <typename T>
void AngleAxisToRotationMatrix(const T* angle_axis, T* R);
template <typename T, int row_stride, int col_stride>
void AngleAxisToRotationMatrix(
const T* angle_axis,
const MatrixAdapter<T, row_stride, col_stride>& R);
// Conversions between 3x3 rotation matrix (in row major order) and
// Euler angle (in degrees) rotation representations.
//
// The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z}
// axes, respectively. They are applied in that same order, so the
// total rotation R is Rz * Ry * Rx.
template <typename T>
void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R);
template <typename T, int row_stride, int col_stride>
void EulerAnglesToRotationMatrix(
const T* euler,
const MatrixAdapter<T, row_stride, col_stride>& R);
// Convert a 4-vector to a 3x3 scaled rotation matrix.
//
// The choice of rotation is such that the quaternion [1 0 0 0] goes to an
// identity matrix and for small a, b, c the quaternion [1 a b c] goes to
// the matrix
//
// [ 0 -c b ]
// I + 2 [ c 0 -a ] + higher order terms
// [ -b a 0 ]
//
// which corresponds to a Rodrigues approximation, the last matrix being
// the cross-product matrix of [a b c]. Together with the property that
// R(q1 * q2) = R(q1) * R(q2) this uniquely defines the mapping from q to R.
//
// The rotation matrix is row-major.
//
// No normalization of the quaternion is performed, i.e.
// R = ||q||^2 * Q, where Q is an orthonormal matrix
// such that det(Q) = 1 and Q*Q' = I
template <typename T> inline
void QuaternionToScaledRotation(const T q[4], T R[3 * 3]);
template <typename T, int row_stride, int col_stride> inline
void QuaternionToScaledRotation(
const T q[4],
const MatrixAdapter<T, row_stride, col_stride>& R);
// Same as above except that the rotation matrix is normalized by the
// Frobenius norm, so that R * R' = I (and det(R) = 1).
template <typename T> inline
void QuaternionToRotation(const T q[4], T R[3 * 3]);
template <typename T, int row_stride, int col_stride> inline
void QuaternionToRotation(
const T q[4],
const MatrixAdapter<T, row_stride, col_stride>& R);
// Rotates a point pt by a quaternion q:
//
// result = R(q) * pt
//
// Assumes the quaternion is unit norm. This assumption allows us to
// write the transform as (something)*pt + pt, as is clear from the
// formula below. If you pass in a quaternion with |q|^2 = 2 then you
// WILL NOT get back 2 times the result you get for a unit quaternion.
template <typename T> inline
void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]);
// With this function you do not need to assume that q has unit norm.
// It does assume that the norm is non-zero.
template <typename T> inline
void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]);
// zw = z * w, where * is the Quaternion product between 4 vectors.
template<typename T> inline
void QuaternionProduct(const T z[4], const T w[4], T zw[4]);
// xy = x cross y;
template<typename T> inline
void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]);
template<typename T> inline
T DotProduct(const T x[3], const T y[3]);
// y = R(angle_axis) * x;
template<typename T> inline
void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]);
// --- IMPLEMENTATION
template<typename T, int row_stride, int col_stride>
struct MatrixAdapter {
T* pointer_;
explicit MatrixAdapter(T* pointer)
: pointer_(pointer)
{}
T& operator()(int r, int c) const {
return pointer_[r * row_stride + c * col_stride];
}
};
template <typename T>
MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer) {
return MatrixAdapter<T, 1, 3>(pointer);
}
template <typename T>
MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer) {
return MatrixAdapter<T, 3, 1>(pointer);
}
template<typename T>
inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) {
const T& a0 = angle_axis[0];
const T& a1 = angle_axis[1];
const T& a2 = angle_axis[2];
const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;
// For points not at the origin, the full conversion is numerically stable.
if (theta_squared > T(0.0)) {
const T theta = sqrt(theta_squared);
const T half_theta = theta * T(0.5);
const T k = sin(half_theta) / theta;
quaternion[0] = cos(half_theta);
quaternion[1] = a0 * k;
quaternion[2] = a1 * k;
quaternion[3] = a2 * k;
} else {
// At the origin, sqrt() will produce NaN in the derivative since
// the argument is zero. By approximating with a Taylor series,
// and truncating at one term, the value and first derivatives will be
// computed correctly when Jets are used.
const T k(0.5);
quaternion[0] = T(1.0);
quaternion[1] = a0 * k;
quaternion[2] = a1 * k;
quaternion[3] = a2 * k;
}
}
template<typename T>
inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) {
const T& q1 = quaternion[1];
const T& q2 = quaternion[2];
const T& q3 = quaternion[3];
const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3;
// For quaternions representing non-zero rotation, the conversion
// is numerically stable.
if (sin_squared_theta > T(0.0)) {
const T sin_theta = sqrt(sin_squared_theta);
const T& cos_theta = quaternion[0];
// If cos_theta is negative, theta is greater than pi/2, which
// means that angle for the angle_axis vector which is 2 * theta
// would be greater than pi.
//
// While this will result in the correct rotation, it does not
// result in a normalized angle-axis vector.
//
// In that case we observe that 2 * theta ~ 2 * theta - 2 * pi,
// which is equivalent saying
//
// theta - pi = atan(sin(theta - pi), cos(theta - pi))
// = atan(-sin(theta), -cos(theta))
//
const T two_theta =
T(2.0) * ((cos_theta < 0.0)
? atan2(-sin_theta, -cos_theta)
: atan2(sin_theta, cos_theta));
const T k = two_theta / sin_theta;
angle_axis[0] = q1 * k;
angle_axis[1] = q2 * k;
angle_axis[2] = q3 * k;
} else {
// For zero rotation, sqrt() will produce NaN in the derivative since
// the argument is zero. By approximating with a Taylor series,
// and truncating at one term, the value and first derivatives will be
// computed correctly when Jets are used.
const T k(2.0);
angle_axis[0] = q1 * k;
angle_axis[1] = q2 * k;
angle_axis[2] = q3 * k;
}
}
// The conversion of a rotation matrix to the angle-axis form is
// numerically problematic when then rotation angle is close to zero
// or to Pi. The following implementation detects when these two cases
// occurs and deals with them by taking code paths that are guaranteed
// to not perform division by a small number.
template <typename T>
inline void RotationMatrixToAngleAxis(const T* R, T* angle_axis) {
RotationMatrixToAngleAxis(ColumnMajorAdapter3x3(R), angle_axis);
}
template <typename T, int row_stride, int col_stride>
void RotationMatrixToAngleAxis(
const MatrixAdapter<const T, row_stride, col_stride>& R,
T* angle_axis) {
// x = k * 2 * sin(theta), where k is the axis of rotation.
angle_axis[0] = R(2, 1) - R(1, 2);
angle_axis[1] = R(0, 2) - R(2, 0);
angle_axis[2] = R(1, 0) - R(0, 1);
static const T kOne = T(1.0);
static const T kTwo = T(2.0);
// Since the right hand side may give numbers just above 1.0 or
// below -1.0 leading to atan misbehaving, we threshold.
T costheta = std::min(std::max((R(0, 0) + R(1, 1) + R(2, 2) - kOne) / kTwo,
T(-1.0)),
kOne);
// sqrt is guaranteed to give non-negative results, so we only
// threshold above.
T sintheta = std::min(sqrt(angle_axis[0] * angle_axis[0] +
angle_axis[1] * angle_axis[1] +
angle_axis[2] * angle_axis[2]) / kTwo,
kOne);
// Use the arctan2 to get the right sign on theta
const T theta = atan2(sintheta, costheta);
// Case 1: sin(theta) is large enough, so dividing by it is not a
// problem. We do not use abs here, because while jets.h imports
// std::abs into the namespace, here in this file, abs resolves to
// the int version of the function, which returns zero always.
//
// We use a threshold much larger then the machine epsilon, because
// if sin(theta) is small, not only do we risk overflow but even if
// that does not occur, just dividing by a small number will result
// in numerical garbage. So we play it safe.
static const double kThreshold = 1e-12;
if ((sintheta > kThreshold) || (sintheta < -kThreshold)) {
const T r = theta / (kTwo * sintheta);
for (int i = 0; i < 3; ++i) {
angle_axis[i] *= r;
}
return;
}
// Case 2: theta ~ 0, means sin(theta) ~ theta to a good
// approximation.
if (costheta > 0.0) {
const T kHalf = T(0.5);
for (int i = 0; i < 3; ++i) {
angle_axis[i] *= kHalf;
}
return;
}
// Case 3: theta ~ pi, this is the hard case. Since theta is large,
// and sin(theta) is small. Dividing by theta by sin(theta) will
// either give an overflow or worse still numerically meaningless
// results. Thus we use an alternate more complicated formula
// here.
// Since cos(theta) is negative, division by (1-cos(theta)) cannot
// overflow.
const T inv_one_minus_costheta = kOne / (kOne - costheta);
// We now compute the absolute value of coordinates of the axis
// vector using the diagonal entries of R. To resolve the sign of
// these entries, we compare the sign of angle_axis[i]*sin(theta)
// with the sign of sin(theta). If they are the same, then
// angle_axis[i] should be positive, otherwise negative.
for (int i = 0; i < 3; ++i) {
angle_axis[i] = theta * sqrt((R(i, i) - costheta) * inv_one_minus_costheta);
if (((sintheta < 0.0) && (angle_axis[i] > 0.0)) ||
((sintheta > 0.0) && (angle_axis[i] < 0.0))) {
angle_axis[i] = -angle_axis[i];
}
}
}
template <typename T>
inline void AngleAxisToRotationMatrix(const T* angle_axis, T* R) {
AngleAxisToRotationMatrix(angle_axis, ColumnMajorAdapter3x3(R));
}
template <typename T, int row_stride, int col_stride>
void AngleAxisToRotationMatrix(
const T* angle_axis,
const MatrixAdapter<T, row_stride, col_stride>& R) {
static const T kOne = T(1.0);
const T theta2 = DotProduct(angle_axis, angle_axis);
if (theta2 > T(std::numeric_limits<double>::epsilon())) {
// We want to be careful to only evaluate the square root if the
// norm of the angle_axis vector is greater than zero. Otherwise
// we get a division by zero.
const T theta = sqrt(theta2);
const T wx = angle_axis[0] / theta;
const T wy = angle_axis[1] / theta;
const T wz = angle_axis[2] / theta;
const T costheta = cos(theta);
const T sintheta = sin(theta);
R(0, 0) = costheta + wx*wx*(kOne - costheta);
R(1, 0) = wz*sintheta + wx*wy*(kOne - costheta);
R(2, 0) = -wy*sintheta + wx*wz*(kOne - costheta);
R(0, 1) = wx*wy*(kOne - costheta) - wz*sintheta;
R(1, 1) = costheta + wy*wy*(kOne - costheta);
R(2, 1) = wx*sintheta + wy*wz*(kOne - costheta);
R(0, 2) = wy*sintheta + wx*wz*(kOne - costheta);
R(1, 2) = -wx*sintheta + wy*wz*(kOne - costheta);
R(2, 2) = costheta + wz*wz*(kOne - costheta);
} else {
// Near zero, we switch to using the first order Taylor expansion.
R(0, 0) = kOne;
R(1, 0) = angle_axis[2];
R(2, 0) = -angle_axis[1];
R(0, 1) = -angle_axis[2];
R(1, 1) = kOne;
R(2, 1) = angle_axis[0];
R(0, 2) = angle_axis[1];
R(1, 2) = -angle_axis[0];
R(2, 2) = kOne;
}
}
template <typename T>
inline void EulerAnglesToRotationMatrix(const T* euler,
const int row_stride_parameter,
T* R) {
DCHECK(row_stride_parameter==3);
EulerAnglesToRotationMatrix(euler, RowMajorAdapter3x3(R));
}
template <typename T, int row_stride, int col_stride>
void EulerAnglesToRotationMatrix(
const T* euler,
const MatrixAdapter<T, row_stride, col_stride>& R) {
const double kPi = 3.14159265358979323846;
const T degrees_to_radians(kPi / 180.0);
const T pitch(euler[0] * degrees_to_radians);
const T roll(euler[1] * degrees_to_radians);
const T yaw(euler[2] * degrees_to_radians);
const T c1 = cos(yaw);
const T s1 = sin(yaw);
const T c2 = cos(roll);
const T s2 = sin(roll);
const T c3 = cos(pitch);
const T s3 = sin(pitch);
R(0, 0) = c1*c2;
R(0, 1) = -s1*c3 + c1*s2*s3;
R(0, 2) = s1*s3 + c1*s2*c3;
R(1, 0) = s1*c2;
R(1, 1) = c1*c3 + s1*s2*s3;
R(1, 2) = -c1*s3 + s1*s2*c3;
R(2, 0) = -s2;
R(2, 1) = c2*s3;
R(2, 2) = c2*c3;
}
template <typename T> inline
void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) {
QuaternionToScaledRotation(q, RowMajorAdapter3x3(R));
}
template <typename T, int row_stride, int col_stride> inline
void QuaternionToScaledRotation(
const T q[4],
const MatrixAdapter<T, row_stride, col_stride>& R) {
// Make convenient names for elements of q.
T a = q[0];
T b = q[1];
T c = q[2];
T d = q[3];
// This is not to eliminate common sub-expression, but to
// make the lines shorter so that they fit in 80 columns!
T aa = a * a;
T ab = a * b;
T ac = a * c;
T ad = a * d;
T bb = b * b;
T bc = b * c;
T bd = b * d;
T cc = c * c;
T cd = c * d;
T dd = d * d;
R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); // NOLINT
R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); // NOLINT
R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; // NOLINT
}
template <typename T> inline
void QuaternionToRotation(const T q[4], T R[3 * 3]) {
QuaternionToRotation(q, RowMajorAdapter3x3(R));
}
template <typename T, int row_stride, int col_stride> inline
void QuaternionToRotation(const T q[4],
const MatrixAdapter<T, row_stride, col_stride>& R) {
QuaternionToScaledRotation(q, R);
T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
CHECK_NE(normalizer, T(0));
normalizer = T(1) / normalizer;
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
R(i, j) *= normalizer;
}
}
}
template <typename T> inline
void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
const T t2 = q[0] * q[1];
const T t3 = q[0] * q[2];
const T t4 = q[0] * q[3];
const T t5 = -q[1] * q[1];
const T t6 = q[1] * q[2];
const T t7 = q[1] * q[3];
const T t8 = -q[2] * q[2];
const T t9 = q[2] * q[3];
const T t1 = -q[3] * q[3];
result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT
result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT
result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT
}
template <typename T> inline
void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
// 'scale' is 1 / norm(q).
const T scale = T(1) / sqrt(q[0] * q[0] +
q[1] * q[1] +
q[2] * q[2] +
q[3] * q[3]);
// Make unit-norm version of q.
const T unit[4] = {
scale * q[0],
scale * q[1],
scale * q[2],
scale * q[3],
};
UnitQuaternionRotatePoint(unit, pt, result);
}
template<typename T> inline
void QuaternionProduct(const T z[4], const T w[4], T zw[4]) {
zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3];
zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2];
zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1];
zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0];
}
// xy = x cross y;
template<typename T> inline
void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) {
x_cross_y[0] = x[1] * y[2] - x[2] * y[1];
x_cross_y[1] = x[2] * y[0] - x[0] * y[2];
x_cross_y[2] = x[0] * y[1] - x[1] * y[0];
}
template<typename T> inline
T DotProduct(const T x[3], const T y[3]) {
return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]);
}
template<typename T> inline
void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) {
const T theta2 = DotProduct(angle_axis, angle_axis);
if (theta2 > T(std::numeric_limits<double>::epsilon())) {
// Away from zero, use the rodriguez formula
//
// result = pt costheta +
// (w x pt) * sintheta +
// w (w . pt) (1 - costheta)
//
// We want to be careful to only evaluate the square root if the
// norm of the angle_axis vector is greater than zero. Otherwise
// we get a division by zero.
//
const T theta = sqrt(theta2);
const T costheta = cos(theta);
const T sintheta = sin(theta);
const T theta_inverse = 1.0 / theta;
const T w[3] = { angle_axis[0] * theta_inverse,
angle_axis[1] * theta_inverse,
angle_axis[2] * theta_inverse };
// Explicitly inlined evaluation of the cross product for
// performance reasons.
const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1],
w[2] * pt[0] - w[0] * pt[2],
w[0] * pt[1] - w[1] * pt[0] };
const T tmp =
(w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta);
result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp;
result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp;
result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp;
} else {
// Near zero, the first order Taylor approximation of the rotation
// matrix R corresponding to a vector w and angle w is
//
// R = I + hat(w) * sin(theta)
//
// But sintheta ~ theta and theta * w = angle_axis, which gives us
//
// R = I + hat(w)
//
// and actually performing multiplication with the point pt, gives us
// R * pt = pt + w x pt.
//
// Switching to the Taylor expansion near zero provides meaningful
// derivatives when evaluated using Jets.
//
// Explicitly inlined evaluation of the cross product for
// performance reasons.
const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1],
angle_axis[2] * pt[0] - angle_axis[0] * pt[2],
angle_axis[0] * pt[1] - angle_axis[1] * pt[0] };
result[0] = pt[0] + w_cross_pt[0];
result[1] = pt[1] + w_cross_pt[1];
result[2] = pt[2] + w_cross_pt[2];
}
}
} // namespace ceres
#endif // CERES_PUBLIC_ROTATION_H_

181
gtsam_unstable/nonlinear/ceres_variadic_evaluate.h Normal file
View File

@ -0,0 +1,181 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2013 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
// mierle@gmail.com (Keir Mierle)
#ifndef CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_
#define CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_
#include <stddef.h>
#include <gtsam_unstable/nonlinear/ceres_jet.h>
#include <gtsam_unstable/nonlinear/ceres_eigen.h>
#include <gtsam_unstable/nonlinear/ceres_fixed_array.h>
namespace ceres {
namespace internal {
// This block of quasi-repeated code calls the user-supplied functor, which may
// take a variable number of arguments. This is accomplished by specializing the
// struct based on the size of the trailing parameters; parameters with 0 size
// are assumed missing.
template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
int N5, int N6, int N7, int N8, int N9>
struct VariadicEvaluate {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
input[1],
input[2],
input[3],
input[4],
input[5],
input[6],
input[7],
input[8],
input[9],
output);
}
};
template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
int N5, int N6, int N7, int N8>
struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, N8, 0> {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
input[1],
input[2],
input[3],
input[4],
input[5],
input[6],
input[7],
input[8],
output);
}
};
template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
int N5, int N6, int N7>
struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, 0, 0> {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
input[1],
input[2],
input[3],
input[4],
input[5],
input[6],
input[7],
output);
}
};
template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
int N5, int N6>
struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, 0, 0, 0> {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
input[1],
input[2],
input[3],
input[4],
input[5],
input[6],
output);
}
};
template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
int N5>
struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, 0, 0, 0, 0> {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
input[1],
input[2],
input[3],
input[4],
input[5],
output);
}
};
template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4>
struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, 0, 0, 0, 0, 0> {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
input[1],
input[2],
input[3],
input[4],
output);
}
};
template<typename Functor, typename T, int N0, int N1, int N2, int N3>
struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, 0, 0, 0, 0, 0, 0> {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
input[1],
input[2],
input[3],
output);
}
};
template<typename Functor, typename T, int N0, int N1, int N2>
struct VariadicEvaluate<Functor, T, N0, N1, N2, 0, 0, 0, 0, 0, 0, 0> {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
input[1],
input[2],
output);
}
};
template<typename Functor, typename T, int N0, int N1>
struct VariadicEvaluate<Functor, T, N0, N1, 0, 0, 0, 0, 0, 0, 0, 0> {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
input[1],
output);
}
};
template<typename Functor, typename T, int N0>
struct VariadicEvaluate<Functor, T, N0, 0, 0, 0, 0, 0, 0, 0, 0, 0> {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
output);
}
};
} // namespace internal
} // namespace ceres
#endif // CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_

323
gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp Normal file
View File

@ -0,0 +1,323 @@
/* ----------------------------------------------------------------------------
* 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 testExpression.cpp
* @date September 18, 2014
* @author Frank Dellaert
* @author Paul Furgale
* @brief unit tests for Block Automatic Differentiation
*/
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam_unstable/nonlinear/Expression.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/LieScalar.h>
#include <gtsam_unstable/nonlinear/ceres_autodiff.h>
#include <gtsam_unstable/nonlinear/ceres_rotation.h>
#undef CHECK
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/list_of.hpp>
using boost::assign::list_of;
using boost::assign::map_list_of;
using namespace std;
using namespace gtsam;
// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector
typedef PinholeCamera<Cal3Bundler> Camera;
/* ************************************************************************* */
// Some Ceres Snippets copied for testing
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
template<typename T> inline T &RowMajorAccess(T *base, int rows, int cols,
int i, int j) {
return base[cols * i + j];
}
inline double RandDouble() {
double r = static_cast<double>(rand());
return r / RAND_MAX;
}
// A structure for projecting a 3x4 camera matrix and a
// homogeneous 3D point, to a 2D inhomogeneous point.
struct Projective {
// Function that takes P and X as separate vectors:
// P, X -> x
template<typename A>
bool operator()(A const P[12], A const X[4], A x[2]) const {
A PX[3];
for (int i = 0; i < 3; ++i) {
PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0]
+ RowMajorAccess(P, 3, 4, i, 1) * X[1]
+ RowMajorAccess(P, 3, 4, i, 2) * X[2]
+ RowMajorAccess(P, 3, 4, i, 3) * X[3];
}
if (PX[2] != 0.0) {
x[0] = PX[0] / PX[2];
x[1] = PX[1] / PX[2];
return true;
}
return false;
}
// Adapt to eigen types
Vector2 operator()(const MatrixRowMajor& P, const Vector4& X) const {
Vector2 x;
if (operator()(P.data(), X.data(), x.data()))
return x;
else
throw std::runtime_error("Projective fail");
}
};
// Templated pinhole camera model for used with Ceres. The camera is
// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for
// focal length and 2 for radial distortion. The principal point is not modeled
// (i.e. it is assumed be located at the image center).
struct SnavelyProjection {
template<typename T>
bool operator()(const T* const camera, const T* const point,
T* predicted) const {
// camera[0,1,2] are the angle-axis rotation.
T p[3];
ceres::AngleAxisRotatePoint(camera, point, p);
// camera[3,4,5] are the translation.
p[0] += camera[3];
p[1] += camera[4];
p[2] += camera[5];
// Compute the center of distortion. The sign change comes from
// the camera model that Noah Snavely's Bundler assumes, whereby
// the camera coordinate system has a negative z axis.
T xp = -p[0] / p[2];
T yp = -p[1] / p[2];
// Apply second and fourth order radial distortion.
const T& l1 = camera[7];
const T& l2 = camera[8];
T r2 = xp * xp + yp * yp;
T distortion = T(1.0) + r2 * (l1 + l2 * r2);
// Compute final projected point position.
const T& focal = camera[6];
predicted[0] = focal * distortion * xp;
predicted[1] = focal * distortion * yp;
return true;
}
// Adapt to GTSAM types
Vector2 operator()(const Vector9& P, const Vector3& X) const {
Vector2 x;
if (operator()(P.data(), X.data(), x.data()))
return x;
else
throw std::runtime_error("Snavely fail");
}
};
/* ************************************************************************* */
// Test Ceres AutoDiff
TEST(Expression, AutoDiff) {
using ceres::internal::AutoDiff;
// Instantiate function
Projective projective;
// Make arguments
typedef Eigen::Matrix<double, 3, 4, Eigen::RowMajor> M;
M P;
P << 1, 0, 0, 0, 0, 1, 0, 5, 0, 0, 1, 0;
Vector4 X(10, 0, 5, 1);
// Apply the mapping, to get image point b_x.
Vector expected = Vector2(2, 1);
Vector2 actual = projective(P, X);
EXPECT(assert_equal(expected,actual,1e-9));
// Get expected derivatives
Matrix E1 = numericalDerivative21<Vector2, M, Vector4>(Projective(), P, X);
Matrix E2 = numericalDerivative22<Vector2, M, Vector4>(Projective(), P, X);
// Get derivatives with AutoDiff
Vector2 actual2;
MatrixRowMajor H1(2, 12), H2(2, 4);
double *parameters[] = { P.data(), X.data() };
double *jacobians[] = { H1.data(), H2.data() };
CHECK(
(AutoDiff<Projective, double, 12, 4>::Differentiate( projective, parameters, 2, actual2.data(), jacobians)));
EXPECT(assert_equal(E1,H1,1e-8));
EXPECT(assert_equal(E2,H2,1e-8));
}
/* ************************************************************************* */
// Test Ceres AutoDiff on Snavely
TEST(Expression, AutoDiff2) {
using ceres::internal::AutoDiff;
// Instantiate function
SnavelyProjection snavely;
// Make arguments
Vector9 P; // zero rotation, (0,5,0) translation, focal length 1
P << 0, 0, 0, 0, 5, 0, 1, 0, 0;
Vector3 X(10, 0, -5); // negative Z-axis convention of Snavely!
// Apply the mapping, to get image point b_x.
Vector expected = Vector2(2, 1);
Vector2 actual = snavely(P, X);
EXPECT(assert_equal(expected,actual,1e-9));
// Get expected derivatives
Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(
SnavelyProjection(), P, X);
Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(
SnavelyProjection(), P, X);
// Get derivatives with AutoDiff
Vector2 actual2;
MatrixRowMajor H1(2, 9), H2(2, 3);
double *parameters[] = { P.data(), X.data() };
double *jacobians[] = { H1.data(), H2.data() };
CHECK(
(AutoDiff<SnavelyProjection, double, 9, 3>::Differentiate( snavely, parameters, 2, actual2.data(), jacobians)));
EXPECT(assert_equal(E1,H1,1e-8));
EXPECT(assert_equal(E2,H2,1e-8));
}
/* ************************************************************************* */
// Adapt ceres-style autodiff
template<typename F, typename T, typename A1, typename A2>
class AdaptAutoDiff {
static const int N = traits::dimension<T>::value;
static const int M1 = traits::dimension<A1>::value;
static const int M2 = traits::dimension<A2>::value;
typedef Eigen::Matrix<double, N, M1, Eigen::RowMajor> RowMajor1;
typedef Eigen::Matrix<double, N, M2, Eigen::RowMajor> RowMajor2;
typedef Canonical<T> CanonicalT;
typedef Canonical<A1> Canonical1;
typedef Canonical<A2> Canonical2;
typedef typename CanonicalT::vector VectorT;
typedef typename Canonical1::vector Vector1;
typedef typename Canonical2::vector Vector2;
// Instantiate function and charts
CanonicalT chartT;
Canonical1 chart1;
Canonical2 chart2;
F f;
public:
typedef Eigen::Matrix<double, N, M1> JacobianTA1;
typedef Eigen::Matrix<double, N, M2> JacobianTA2;
T operator()(const A1& a1, const A2& a2, boost::optional<JacobianTA1&> H1 =
boost::none, boost::optional<JacobianTA2&> H2 = boost::none) {
using ceres::internal::AutoDiff;
// Make arguments
Vector1 v1 = chart1.apply(a1);
Vector2 v2 = chart2.apply(a2);
bool success;
VectorT result;
if (H1 || H2) {
// Get derivatives with AutoDiff
double *parameters[] = { v1.data(), v2.data() };
double rowMajor1[N * M1], rowMajor2[N * M2]; // om the stack
double *jacobians[] = { rowMajor1, rowMajor2 };
success = AutoDiff<F, double, 9, 3>::Differentiate(f, parameters, 2,
result.data(), jacobians);
// Convert from row-major to columnn-major
// TODO: if this is a bottleneck (probably not!) fix Autodiff to be Column-Major
*H1 = Eigen::Map<RowMajor1>(rowMajor1);
*H2 = Eigen::Map<RowMajor2>(rowMajor2);
} else {
// Apply the mapping, to get result
success = f(v1.data(), v2.data(), result.data());
}
if (!success)
throw std::runtime_error(
"AdaptAutoDiff: function call resulted in failure");
return chartT.retract(result);
}
};
/* ************************************************************************* */
// Test AutoDiff wrapper Snavely
TEST(Expression, AutoDiff3) {
// Make arguments
Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler(1, 0, 0));
Point3 X(10, 0, -5); // negative Z-axis convention of Snavely!
typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor;
Adaptor snavely;
// Apply the mapping, to get image point b_x.
Point2 expected(2, 1);
Point2 actual = snavely(P, X);
EXPECT(assert_equal(expected,actual,1e-9));
// // Get expected derivatives
Matrix E1 = numericalDerivative21<Point2, Camera, Point3>(Adaptor(), P, X);
Matrix E2 = numericalDerivative22<Point2, Camera, Point3>(Adaptor(), P, X);
// Get derivatives with AutoDiff, not gives RowMajor results!
Matrix29 H1;
Matrix23 H2;
Point2 actual2 = snavely(P, X, H1, H2);
EXPECT(assert_equal(expected,actual,1e-9));
EXPECT(assert_equal(E1,H1,1e-8));
EXPECT(assert_equal(E2,H2,1e-8));
}
/* ************************************************************************* */
// Test AutoDiff wrapper in an expression
TEST(Expression, Snavely) {
Expression<Camera> P(1);
Expression<Point3> X(2);
typedef AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> Adaptor;
Expression<Point2> expression(Adaptor(), P, X);
set<Key> expected = list_of(1)(2);
EXPECT(expected == expression.keys());
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

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

@ -18,7 +18,6 @@
*/
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam_unstable/nonlinear/Expression.h>
#include <gtsam/base/Testable.h>

10
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -315,8 +315,9 @@ public:
// Jacobian w.r.t. Vel1
if (H2){
Matrix H2_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
Matrix H2_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
Matrix H2_Pose = numericalDerivative11<POSE, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
Matrix H2_Vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
*H2 = stack(2, &H2_Pose, &H2_Vel);
}
@ -336,8 +337,9 @@ public:
// Jacobian w.r.t. Vel2
if (H5){
Matrix H5_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
Matrix H5_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
Matrix H5_Pose = numericalDerivative11<POSE, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
Matrix H5_Vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
*H5 = stack(2, &H5_Pose, &H5_Vel);
}

10
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -242,8 +242,9 @@ public:
// Jacobian w.r.t. Vel1
if (H2){
Matrix H2_Pose = gtsam::numericalDerivative11<POSE, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
Matrix H2_Vel = gtsam::numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
Matrix H2_Pose = gtsam::numericalDerivative11<POSE, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
Matrix H2_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
*H2 = stack(2, &H2_Pose, &H2_Vel);
}
@ -263,8 +264,9 @@ public:
// Jacobian w.r.t. Vel2
if (H5){
Matrix H5_Pose = gtsam::numericalDerivative11<POSE, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
Matrix H5_Vel = gtsam::numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
Matrix H5_Pose = gtsam::numericalDerivative11<POSE, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
Matrix H5_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
*H5 = stack(2, &H5_Pose, &H5_Vel);
}

12
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

@ -104,11 +104,15 @@ public:
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
if(H1) {
(*H1) = numericalDerivative11<Pose3>(boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, _1, landmark), pose);
if (H1) {
(*H1) = numericalDerivative11<Vector, Pose3>(
boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, _1,
landmark), pose);
}
if(H2) {
(*H2) = numericalDerivative11<LieVector>(boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, _1), landmark);
if (H2) {
(*H2) = numericalDerivative11<Vector, Vector6>(
boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose,
_1), landmark);
}
return inverseDepthError(pose, landmark);

12
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -107,11 +107,15 @@ public:
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
if(H1) {
(*H1) = numericalDerivative11<Pose3>(boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, _1, landmark), pose);
if (H1) {
(*H1) = numericalDerivative11<Vector, Pose3>(
boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, _1,
landmark), pose);
}
if(H2) {
(*H2) = numericalDerivative11<LieVector>(boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, _1), landmark);
if (H2) {
(*H2) = numericalDerivative11<Vector, Vector3>(
boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose,
_1), landmark);
}
return inverseDepthError(pose, landmark);

10
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -108,10 +108,10 @@ public:
boost::optional<gtsam::Matrix&> H2=boost::none) const {
if(H1) {
(*H1) = numericalDerivative11<Pose3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose);
(*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose);
}
if(H2) {
(*H2) = numericalDerivative11<LieVector>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark);
(*H2) = numericalDerivative11<Vector,Vector3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark);
}
return inverseDepthError(pose, landmark);
@ -228,13 +228,13 @@ public:
boost::optional<gtsam::Matrix&> H3=boost::none) const {
if(H1) {
(*H1) = numericalDerivative11<Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1);
(*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1);
}
if(H2) {
(*H2) = numericalDerivative11<Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2);
(*H2) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2);
}
if(H3) {
(*H3) = numericalDerivative11<LieVector>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark);
(*H3) = numericalDerivative11<Vector,Vector3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark);
}
return inverseDepthError(pose1, pose2, landmark);

308
gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp
View File

@ -29,31 +29,37 @@
using namespace std;
using namespace gtsam;
gtsam::Rot3 world_R_ECEF(
Rot3 world_R_ECEF(
0.31686, 0.51505, 0.79645,
0.85173, -0.52399, 0,
0.41733, 0.67835, -0.60471);
gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
/* ************************************************************************* */
gtsam::Pose3 predictionErrorPose(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias>& factor) {
Pose3 predictionErrorPose(const Pose3& p1, const LieVector& v1,
const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2,
const InertialNavFactor_GlobalVelocity<Pose3, LieVector,
imuBias::ConstantBias>& factor) {
return Pose3::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).head(6));
}
gtsam::LieVector predictionErrorVel(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias>& factor) {
return LieVector::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).tail(3));
Vector predictionErrorVel(const Pose3& p1, const LieVector& v1,
const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2,
const InertialNavFactor_GlobalVelocity<Pose3, LieVector,
imuBias::ConstantBias>& factor) {
return factor.evaluateError(p1, v1, b1, p2, v2).tail(3);
}
/* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, Constructor)
{
gtsam::Key Pose1(11);
gtsam::Key Pose2(12);
gtsam::Key Vel1(21);
gtsam::Key Vel2(22);
gtsam::Key Bias1(31);
Key Pose1(11);
Key Pose2(12);
Key Vel1(21);
Key Vel2(22);
Key Bias1(31);
Vector measurement_acc((Vector(3) << 0.1,0.2,0.4));
Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03));
@ -61,8 +67,8 @@ TEST( InertialNavFactor_GlobalVelocity, Constructor)
double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -72,11 +78,11 @@ TEST( InertialNavFactor_GlobalVelocity, Constructor)
/* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, Equals)
{
gtsam::Key Pose1(11);
gtsam::Key Pose2(12);
gtsam::Key Vel1(21);
gtsam::Key Vel2(22);
gtsam::Key Bias1(31);
Key Pose1(11);
Key Pose2(12);
Key Vel1(21);
Key Vel2(22);
Key Bias1(31);
Vector measurement_acc((Vector(3) << 0.1,0.2,0.4));
Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03));
@ -84,8 +90,8 @@ TEST( InertialNavFactor_GlobalVelocity, Equals)
double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -97,17 +103,17 @@ TEST( InertialNavFactor_GlobalVelocity, Equals)
/* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, Predict)
{
gtsam::Key PoseKey1(11);
gtsam::Key PoseKey2(12);
gtsam::Key VelKey1(21);
gtsam::Key VelKey2(22);
gtsam::Key BiasKey1(31);
Key PoseKey1(11);
Key PoseKey2(12);
Key VelKey1(21);
Key VelKey2(22);
Key BiasKey1(31);
double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -134,17 +140,17 @@ TEST( InertialNavFactor_GlobalVelocity, Predict)
/* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel)
{
gtsam::Key PoseKey1(11);
gtsam::Key PoseKey2(12);
gtsam::Key VelKey1(21);
gtsam::Key VelKey2(22);
gtsam::Key BiasKey1(31);
Key PoseKey1(11);
Key PoseKey2(12);
Key VelKey1(21);
Key VelKey2(22);
Key BiasKey1(31);
double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -170,17 +176,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel)
/* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, ErrorRot)
{
gtsam::Key PoseKey1(11);
gtsam::Key PoseKey2(12);
gtsam::Key VelKey1(21);
gtsam::Key VelKey2(22);
gtsam::Key BiasKey1(31);
Key PoseKey1(11);
Key PoseKey2(12);
Key VelKey1(21);
Key VelKey2(22);
Key BiasKey1(31);
double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -205,17 +211,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot)
/* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
{
gtsam::Key PoseKey1(11);
gtsam::Key PoseKey2(12);
gtsam::Key VelKey1(21);
gtsam::Key VelKey2(22);
gtsam::Key BiasKey1(31);
Key PoseKey1(11);
Key PoseKey2(12);
Key VelKey1(21);
Key VelKey2(22);
Key BiasKey1(31);
double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -262,33 +268,33 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
// -q[1], q[0],0.0);
// Matrix J_hyp( -(R1*qx).matrix() );
//
// gtsam::Matrix J_expected;
// Matrix J_expected;
//
// LieVector v(predictionRq(angles, q));
//
// J_expected = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionRq, _1, q), angles);
// J_expected = numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionRq, _1, q), angles);
//
// cout<<"J_hyp"<<J_hyp<<endl;
// cout<<"J_expected"<<J_expected<<endl;
//
// CHECK( gtsam::assert_equal(J_expected, J_hyp, 1e-6));
// CHECK( assert_equal(J_expected, J_hyp, 1e-6));
//}
///* VADIM - END ************************************************************************* */
/* ************************************************************************* */
TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
gtsam::Key PoseKey1(11);
gtsam::Key PoseKey2(12);
gtsam::Key VelKey1(21);
gtsam::Key VelKey2(22);
gtsam::Key BiasKey1(31);
Key PoseKey1(11);
Key PoseKey2(12);
Key VelKey1(21);
Key VelKey2(22);
Key BiasKey1(31);
double measurement_dt(0.01);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -324,19 +330,19 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols()));
// Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose;
H1_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
H2_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
H3_expectedPose = gtsam::numericalDerivative11<Pose3, imuBias::ConstantBias>(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
H4_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
H5_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose;
H1_expectedPose = numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
H2_expectedPose = numericalDerivative11<Pose3, Vector3>(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
H4_expectedPose = numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
H5_expectedPose = numericalDerivative11<Pose3, Vector3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
// Verify they are equal for this choice of state
CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-5));
CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-5));
CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 2e-3));
CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-5));
CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-5));
CHECK( assert_equal(H1_expectedPose, H1_actualPose, 1e-5));
CHECK( assert_equal(H2_expectedPose, H2_actualPose, 1e-5));
CHECK( assert_equal(H3_expectedPose, H3_actualPose, 2e-3));
CHECK( assert_equal(H4_expectedPose, H4_actualPose, 1e-5));
CHECK( assert_equal(H5_expectedPose, H5_actualPose, 1e-5));
// Checking for Vel part in the jacobians
// ******
@ -347,19 +353,19 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols()));
// Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel;
H1_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
H2_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
H3_expectedVel = gtsam::numericalDerivative11<LieVector, imuBias::ConstantBias>(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
H4_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
H5_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel;
H1_expectedVel = numericalDerivative11<Vector, Pose3>(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
H2_expectedVel = numericalDerivative11<Vector, Vector3>(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
H4_expectedVel = numericalDerivative11<Vector, Pose3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
H5_expectedVel = numericalDerivative11<Vector, Vector3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
// Verify they are equal for this choice of state
CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5));
CHECK( assert_equal(H1_expectedVel, H1_actualVel, 1e-5));
CHECK( assert_equal(H2_expectedVel, H2_actualVel, 1e-5));
CHECK( assert_equal(H3_expectedVel, H3_actualVel, 1e-5));
CHECK( assert_equal(H4_expectedVel, H4_actualVel, 1e-5));
CHECK( assert_equal(H5_expectedVel, H5_actualVel, 1e-5));
}
@ -368,11 +374,11 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
/* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform)
{
gtsam::Key Pose1(11);
gtsam::Key Pose2(12);
gtsam::Key Vel1(21);
gtsam::Key Vel2(22);
gtsam::Key Bias1(31);
Key Pose1(11);
Key Pose2(12);
Key Vel1(21);
Key Vel2(22);
Key Bias1(31);
Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4));
Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03));
@ -380,8 +386,8 @@ TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform)
double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -394,11 +400,11 @@ TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform)
/* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform)
{
gtsam::Key Pose1(11);
gtsam::Key Pose2(12);
gtsam::Key Vel1(21);
gtsam::Key Vel2(22);
gtsam::Key Bias1(31);
Key Pose1(11);
Key Pose2(12);
Key Vel1(21);
Key Vel2(22);
Key Bias1(31);
Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4));
Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03));
@ -406,8 +412,8 @@ TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform)
double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -422,17 +428,17 @@ TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform)
/* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform)
{
gtsam::Key PoseKey1(11);
gtsam::Key PoseKey2(12);
gtsam::Key VelKey1(21);
gtsam::Key VelKey2(22);
gtsam::Key BiasKey1(31);
Key PoseKey1(11);
Key PoseKey2(12);
Key VelKey1(21);
Key VelKey2(22);
Key BiasKey1(31);
double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -462,17 +468,17 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform)
/* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform)
{
gtsam::Key PoseKey1(11);
gtsam::Key PoseKey2(12);
gtsam::Key VelKey1(21);
gtsam::Key VelKey2(22);
gtsam::Key BiasKey1(31);
Key PoseKey1(11);
Key PoseKey2(12);
Key VelKey1(21);
Key VelKey2(22);
Key BiasKey1(31);
double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -501,17 +507,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform)
/* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform)
{
gtsam::Key PoseKey1(11);
gtsam::Key PoseKey2(12);
gtsam::Key VelKey1(21);
gtsam::Key VelKey2(22);
gtsam::Key BiasKey1(31);
Key PoseKey1(11);
Key PoseKey2(12);
Key VelKey1(21);
Key VelKey2(22);
Key BiasKey1(31);
double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -540,17 +546,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform)
/* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
{
gtsam::Key PoseKey1(11);
gtsam::Key PoseKey2(12);
gtsam::Key VelKey1(21);
gtsam::Key VelKey2(22);
gtsam::Key BiasKey1(31);
Key PoseKey1(11);
Key PoseKey2(12);
Key VelKey1(21);
Key VelKey2(22);
Key BiasKey1(31);
double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -589,17 +595,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
/* ************************************************************************* */
TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
gtsam::Key PoseKey1(11);
gtsam::Key PoseKey2(12);
gtsam::Key VelKey1(21);
gtsam::Key VelKey2(22);
gtsam::Key BiasKey1(31);
Key PoseKey1(11);
Key PoseKey2(12);
Key VelKey1(21);
Key VelKey2(22);
Key BiasKey1(31);
double measurement_dt(0.01);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -640,19 +646,19 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols()));
// Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose;
H1_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
H2_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
H3_expectedPose = gtsam::numericalDerivative11<Pose3, imuBias::ConstantBias>(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
H4_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
H5_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose;
H1_expectedPose = numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
H2_expectedPose = numericalDerivative11<Pose3, Vector3>(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
H4_expectedPose = numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
H5_expectedPose = numericalDerivative11<Pose3, Vector3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
// Verify they are equal for this choice of state
CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-5));
CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-5));
CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 2e-3));
CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-5));
CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-5));
CHECK( assert_equal(H1_expectedPose, H1_actualPose, 1e-5));
CHECK( assert_equal(H2_expectedPose, H2_actualPose, 1e-5));
CHECK( assert_equal(H3_expectedPose, H3_actualPose, 2e-3));
CHECK( assert_equal(H4_expectedPose, H4_actualPose, 1e-5));
CHECK( assert_equal(H5_expectedPose, H5_actualPose, 1e-5));
// Checking for Vel part in the jacobians
// ******
@ -663,19 +669,19 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols()));
// Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel;
H1_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
H2_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
H3_expectedVel = gtsam::numericalDerivative11<LieVector, imuBias::ConstantBias>(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
H4_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
H5_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel;
H1_expectedVel = numericalDerivative11<Vector, Pose3>(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
H2_expectedVel = numericalDerivative11<Vector, Vector3>(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
H4_expectedVel = numericalDerivative11<Vector, Pose3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
H5_expectedVel = numericalDerivative11<Vector, Vector3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
// Verify they are equal for this choice of state
CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5));
CHECK( assert_equal(H1_expectedVel, H1_actualVel, 1e-5));
CHECK( assert_equal(H2_expectedVel, H2_actualVel, 1e-5));
CHECK( assert_equal(H3_expectedVel, H3_actualVel, 1e-5));
CHECK( assert_equal(H4_expectedVel, H4_actualVel, 1e-5));
CHECK( assert_equal(H5_expectedVel, H5_actualVel, 1e-5));
}
/* ************************************************************************* */

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

@ -24,8 +24,8 @@ using namespace gtsam;
TEST( InvDepthFactorVariant1, optimize) {
// Create two poses looking in the x-direction
Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0));
Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5));
Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.0));
Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.5));
// Create a landmark 5 meters in front of pose1 (camera center at (0,0,1))
Point3 landmark(5, 0, 1);
@ -76,18 +76,18 @@ TEST( InvDepthFactorVariant1, optimize) {
LieVector actual = result.at<LieVector>(landmarkKey);
values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
result.at<Pose3>(poseKey1).print("Pose1 After:\n");
pose1.print("Pose1 Truth:\n");
std::cout << std::endl << std::endl;
values.at<Pose3>(poseKey2).print("Pose2 Before:\n");
result.at<Pose3>(poseKey2).print("Pose2 After:\n");
pose2.print("Pose2 Truth:\n");
std::cout << std::endl << std::endl;
values.at<LieVector>(landmarkKey).print("Landmark Before:\n");
result.at<LieVector>(landmarkKey).print("Landmark After:\n");
expected.print("Landmark Truth:\n");
std::cout << std::endl << std::endl;
// values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
// result.at<Pose3>(poseKey1).print("Pose1 After:\n");
// pose1.print("Pose1 Truth:\n");
// cout << endl << endl;
// values.at<Pose3>(poseKey2).print("Pose2 Before:\n");
// result.at<Pose3>(poseKey2).print("Pose2 After:\n");
// pose2.print("Pose2 Truth:\n");
// cout << endl << endl;
// values.at<LieVector>(landmarkKey).print("Landmark Before:\n");
// result.at<LieVector>(landmarkKey).print("Landmark After:\n");
// expected.print("Landmark Truth:\n");
// cout << endl << endl;
// Calculate world coordinates of landmark versions
Point3 world_landmarkBefore;
@ -105,10 +105,10 @@ TEST( InvDepthFactorVariant1, optimize) {
world_landmarkAfter = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
}
world_landmarkBefore.print("World Landmark Before:\n");
world_landmarkAfter.print("World Landmark After:\n");
landmark.print("World Landmark Truth:\n");
std::cout << std::endl << std::endl;
// world_landmarkBefore.print("World Landmark Before:\n");
// world_landmarkAfter.print("World Landmark After:\n");
// landmark.print("World Landmark Truth:\n");
// cout << endl << endl;

35
gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp
View File

@ -73,19 +73,18 @@ TEST( InvDepthFactorVariant2, optimize) {
Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
LieVector actual = result.at<LieVector>(landmarkKey);
values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
result.at<Pose3>(poseKey1).print("Pose1 After:\n");
pose1.print("Pose1 Truth:\n");
std::cout << std::endl << std::endl;
values.at<Pose3>(poseKey2).print("Pose2 Before:\n");
result.at<Pose3>(poseKey2).print("Pose2 After:\n");
pose2.print("Pose2 Truth:\n");
std::cout << std::endl << std::endl;
values.at<LieVector>(landmarkKey).print("Landmark Before:\n");
result.at<LieVector>(landmarkKey).print("Landmark After:\n");
expected.print("Landmark Truth:\n");
std::cout << std::endl << std::endl;
// values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
// result.at<Pose3>(poseKey1).print("Pose1 After:\n");
// pose1.print("Pose1 Truth:\n");
// std::cout << std::endl << std::endl;
// values.at<Pose3>(poseKey2).print("Pose2 Before:\n");
// result.at<Pose3>(poseKey2).print("Pose2 After:\n");
// pose2.print("Pose2 Truth:\n");
// std::cout << std::endl << std::endl;
// values.at<LieVector>(landmarkKey).print("Landmark Before:\n");
// result.at<LieVector>(landmarkKey).print("Landmark After:\n");
// expected.print("Landmark Truth:\n");
// std::cout << std::endl << std::endl;
// Calculate world coordinates of landmark versions
Point3 world_landmarkBefore;
@ -101,12 +100,10 @@ TEST( InvDepthFactorVariant2, optimize) {
world_landmarkAfter = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
}
world_landmarkBefore.print("World Landmark Before:\n");
world_landmarkAfter.print("World Landmark After:\n");
landmark.print("World Landmark Truth:\n");
std::cout << std::endl << std::endl;
// world_landmarkBefore.print("World Landmark Before:\n");
// world_landmarkAfter.print("World Landmark After:\n");
// landmark.print("World Landmark Truth:\n");
// std::cout << std::endl << std::endl;
// Test that the correct landmark parameters have been recovered
EXPECT(assert_equal(expected, actual, 1e-9));

2
gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp
View File

@ -39,7 +39,7 @@ TEST( InvDepthFactorVariant3, optimize) {
// Create expected landmark
Point3 landmark_p1 = pose1.transform_to(landmark);
landmark_p1.print("Landmark in Pose1 Frame:\n");
// landmark_p1.print("Landmark in Pose1 Frame:\n");
double theta = atan2(landmark_p1.x(), landmark_p1.z());
double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z()));
double rho = 1./landmark_p1.norm();

8
gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp
View File

@ -176,8 +176,8 @@ TEST( PoseBetweenFactor, Jacobian ) {
Point3(-3.37493895, 6.14660244, -8.93650986));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2);
Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2);
// Use the factor to calculate the derivative
Matrix actualH1;
@ -205,8 +205,8 @@ TEST( PoseBetweenFactor, JacobianWithTransform ) {
Point3(-3.5257579, 6.02637531, -8.98382384));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2);
Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2);
// Use the factor to calculate the derivative
Matrix actualH1;

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

@ -166,7 +166,7 @@ TEST( PosePriorFactor, Jacobian ) {
Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Pose3>(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose);
Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose);
// Use the factor to calculate the derivative
Matrix actualH1;
@ -190,7 +190,7 @@ TEST( PosePriorFactor, JacobianWithTransform ) {
Point3(-4.74767676, 7.67044942, -11.00985));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Pose3>(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose);
Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose);
// Use the factor to calculate the derivative
Matrix actualH1;

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

@ -170,7 +170,7 @@ TEST( ProjectionFactor, Jacobian ) {
CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
// Verify H2 with numerical derivative
Matrix H2Expected = numericalDerivative32<Pose3, Pose3, Point3>(
Matrix H2Expected = numericalDerivative32<Vector,Pose3, Pose3, Point3>(
boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
boost::none, boost::none, boost::none)), pose, Pose3(), point);
@ -205,7 +205,7 @@ TEST( ProjectionFactor, JacobianWithTransform ) {
CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
// Verify H2 with numerical derivative
Matrix H2Expected = numericalDerivative32<Pose3, Pose3, Point3>(
Matrix H2Expected = numericalDerivative32<Vector, Pose3, Pose3, Point3>(
boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
boost::none, boost::none, boost::none)), pose, body_P_sensor, point);

8
gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp
View File

@ -136,11 +136,11 @@ TEST( ProjectionFactor, Jacobian ) {
CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
// Verify H2 and H4 with numerical derivatives
Matrix H2Expected = numericalDerivative11<LieVector, Pose3>(
Matrix H2Expected = numericalDerivative11<Vector, Pose3>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point,
*K1, boost::none, boost::none, boost::none, boost::none), Pose3());
Matrix H4Expected = numericalDerivative11<LieVector, Cal3_S2>(
Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), point,
_1, boost::none, boost::none, boost::none, boost::none), *K1);
@ -172,11 +172,11 @@ TEST( ProjectionFactor, JacobianWithTransform ) {
CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
// Verify H2 and H4 with numerical derivatives
Matrix H2Expected = numericalDerivative11<LieVector, Pose3>(
Matrix H2Expected = numericalDerivative11<Vector, Pose3>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point,
*K1, boost::none, boost::none, boost::none, boost::none), body_P_sensor);
Matrix H4Expected = numericalDerivative11<LieVector, Cal3_S2>(
Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point,
_1, boost::none, boost::none, boost::none, boost::none), *K1);

32
gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp
View File

@ -24,8 +24,8 @@ const Point3 point1(3.0, 4.0, 2.0);
const gtsam::Key poseKey = 1, pointKey = 2;
/* ************************************************************************* */
LieVector evalFactorError(const RelativeElevationFactor& factor, const Pose3& x, const Point3& p) {
return LieVector(factor.evaluateError(x, p));
Vector evalFactorError(const RelativeElevationFactor& factor, const Pose3& x, const Point3& p) {
return factor.evaluateError(x, p);
}
/* ************************************************************************* */
@ -35,9 +35,9 @@ TEST( testRelativeElevationFactor, level_zero_error ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2;
EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
@ -50,9 +50,9 @@ TEST( testRelativeElevationFactor, level_positive ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 2.0), factor.evaluateError(pose1, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
@ -65,9 +65,9 @@ TEST( testRelativeElevationFactor, level_negative ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose1, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
@ -80,9 +80,9 @@ TEST( testRelativeElevationFactor, rotated_zero_error ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2;
EXPECT(assert_equal(zero(1), factor.evaluateError(pose2, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
@ -95,9 +95,9 @@ TEST( testRelativeElevationFactor, rotated_positive ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 2.0), factor.evaluateError(pose2, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
@ -110,9 +110,9 @@ TEST( testRelativeElevationFactor, rotated_negative1 ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose2, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
@ -125,9 +125,9 @@ TEST( testRelativeElevationFactor, rotated_negative2 ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose3, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));

20
gtsam_unstable/slam/tests/testTSAMFactors.cpp
View File

@ -44,10 +44,10 @@ TEST( DeltaFactor, all ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(
H1Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none,
boost::none), pose);
H2Expected = numericalDerivative11<LieVector, Point2>(
H2Expected = numericalDerivative11<Vector, Point2>(
boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none,
boost::none), point);
@ -78,16 +78,16 @@ TEST( DeltaFactorBase, all ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected, H3Expected, H4Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(
H1Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2,
point, boost::none, boost::none, boost::none, boost::none), base1);
H2Expected = numericalDerivative11<LieVector, Pose2>(
H2Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2,
point, boost::none, boost::none, boost::none, boost::none), pose);
H3Expected = numericalDerivative11<LieVector, Pose2>(
H3Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1,
point, boost::none, boost::none, boost::none, boost::none), base2);
H4Expected = numericalDerivative11<LieVector, Point2>(
H4Expected = numericalDerivative11<Vector, Point2>(
boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2,
_1, boost::none, boost::none, boost::none, boost::none), point);
@ -119,16 +119,16 @@ TEST( OdometryFactorBase, all ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected, H3Expected, H4Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(
H1Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2,
pose2, boost::none, boost::none, boost::none, boost::none), base1);
H2Expected = numericalDerivative11<LieVector, Pose2>(
H2Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2,
pose2, boost::none, boost::none, boost::none, boost::none), pose1);
H3Expected = numericalDerivative11<LieVector, Pose2>(
H3Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1,
pose2, boost::none, boost::none, boost::none, boost::none), base2);
H4Expected = numericalDerivative11<LieVector, Pose2>(
H4Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1,
base2, _1, boost::none, boost::none, boost::none, boost::none),
pose2);

167
tests/testManifold.cpp Normal file
View File

@ -0,0 +1,167 @@
/* ----------------------------------------------------------------------------
* 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 testExpression.cpp
* @date September 18, 2014
* @author Frank Dellaert
* @author Paul Furgale
* @brief unit tests for Block Automatic Differentiation
*/
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam_unstable/nonlinear/Expression.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/LieScalar.h>
#undef CHECK
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/list_of.hpp>
using boost::assign::list_of;
using boost::assign::map_list_of;
using namespace std;
using namespace gtsam;
// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector
typedef PinholeCamera<Cal3Bundler> Camera;
/* ************************************************************************* */
// is_manifold
TEST(Manifold, _is_manifold) {
using namespace traits;
EXPECT(!is_manifold<int>::value);
EXPECT(is_manifold<Point2>::value);
EXPECT(is_manifold<Matrix24>::value);
EXPECT(is_manifold<double>::value);
EXPECT(is_manifold<Vector>::value);
EXPECT(is_manifold<Matrix>::value);
}
/* ************************************************************************* */
// dimension
TEST(Manifold, _dimension) {
using namespace traits;
EXPECT_LONGS_EQUAL(2, dimension<Point2>::value);
EXPECT_LONGS_EQUAL(8, dimension<Matrix24>::value);
EXPECT_LONGS_EQUAL(1, dimension<double>::value);
EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension<Vector>::value);
EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension<Matrix>::value);
}
/* ************************************************************************* */
// charts
TEST(Manifold, DefaultChart) {
DefaultChart<Point2> chart1(Point2(0, 0));
EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0));
EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0));
Vector v2(2);
v2 << 1, 0;
DefaultChart<Vector2> chart2(Vector2(0, 0));
EXPECT(assert_equal(v2,chart2.apply(Vector2(1,0))));
EXPECT(chart2.retract(v2)==Vector2(1,0));
DefaultChart<double> chart3(0);
Vector v1(1);
v1 << 1;
EXPECT(assert_equal(v1,chart3.apply(1)));
EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9);
// Dynamic does not work yet !
Vector z = zero(2), v(2);
v << 1, 0;
DefaultChart<Vector> chart4(z);
EXPECT(assert_equal(chart4.apply(v),v));
EXPECT(assert_equal(chart4.retract(v),v));
Vector v3(3);
v3 << 1, 1, 1;
Rot3 I = Rot3::identity();
Rot3 R = I.retractCayley(v3);
DefaultChart<Rot3> chart5(I);
EXPECT(assert_equal(v3,chart5.apply(R)));
EXPECT(assert_equal(chart5.retract(v3),R));
// Check zero vector
DefaultChart<Rot3> chart6(R);
EXPECT(assert_equal(zero(3),chart6.apply(R)));
}
/* ************************************************************************* */
// zero
TEST(Manifold, _zero) {
EXPECT(assert_equal(Pose3(),traits::zero<Pose3>::value()));
Cal3Bundler cal(0, 0, 0);
EXPECT(assert_equal(cal,traits::zero<Cal3Bundler>::value()));
EXPECT(assert_equal(Camera(Pose3(),cal),traits::zero<Camera>::value()));
}
/* ************************************************************************* */
// charts
TEST(Manifold, Canonical) {
Canonical<Point2> chart1;
EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0));
EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0));
Vector v2(2);
v2 << 1, 0;
Canonical<Vector2> chart2;
EXPECT(assert_equal(v2,chart2.apply(Vector2(1,0))));
EXPECT(chart2.retract(v2)==Vector2(1,0));
Canonical<double> chart3;
Eigen::Matrix<double, 1, 1> v1;
v1 << 1;
EXPECT(chart3.apply(1)==v1);
EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9);
Canonical<Point3> chart4;
Point3 point(1, 2, 3);
Vector v3(3);
v3 << 1, 2, 3;
EXPECT(assert_equal(v3,chart4.apply(point)));
EXPECT(assert_equal(chart4.retract(v3),point));
Canonical<Pose3> chart5;
Pose3 pose(Rot3::identity(), point);
Vector v6(6);
v6 << 0, 0, 0, 1, 2, 3;
EXPECT(assert_equal(v6,chart5.apply(pose)));
EXPECT(assert_equal(chart5.retract(v6),pose));
Canonical<Camera> chart6;
Cal3Bundler cal0(0, 0, 0);
Camera camera(Pose3(), cal0);
Vector z9 = Vector9::Zero();
EXPECT(assert_equal(z9,chart6.apply(camera)));
EXPECT(assert_equal(chart6.retract(z9),camera));
Cal3Bundler cal; // Note !! Cal3Bundler() != zero<Cal3Bundler>::value()
Camera camera2(pose, cal);
Vector v9(9);
v9 << 0, 0, 0, 1, 2, 3, 1, 0, 0;
EXPECT(assert_equal(v9,chart6.apply(camera2)));
EXPECT(assert_equal(chart6.retract(v9),camera2));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */