diff --git a/.cproject b/.cproject index 7223156ff..895e2667a 100644 --- a/.cproject +++ b/.cproject @@ -600,6 +600,7 @@ make + tests/testBayesTree.run true false @@ -607,6 +608,7 @@ make + testBinaryBayesNet.run true false @@ -654,6 +656,7 @@ make + testSymbolicBayesNet.run true false @@ -661,6 +664,7 @@ make + tests/testSymbolicFactor.run true false @@ -668,6 +672,7 @@ make + testSymbolicFactorGraph.run true false @@ -683,6 +688,7 @@ make + tests/testBayesTree true false @@ -800,6 +806,54 @@ true true + + make + -j5 + testInertialNavFactor_GlobalVelocity.run + true + true + true + + + make + -j5 + testInvDepthFactorVariant3.run + true + true + true + + + make + -j5 + testInvDepthFactorVariant1.run + true + true + true + + + make + -j5 + testEquivInertialNavFactor_GlobalVel.run + true + true + true + + + make + -j5 + testInvDepthFactorVariant2.run + true + true + true + + + make + -j5 + testRelativeElevationFactor.run + true + true + true + make -j5 @@ -896,6 +950,14 @@ true true + + make + -j5 + testGaussianBayesTree.run + true + true + true + make -j5 @@ -1042,6 +1104,7 @@ make + testErrors.run true false @@ -1271,6 +1334,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1353,7 +1456,6 @@ make - testSimulated2DOriented.run true false @@ -1393,7 +1495,6 @@ make - testSimulated2D.run true false @@ -1401,7 +1502,6 @@ make - testSimulated3D.run true false @@ -1415,46 +1515,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1712,6 +1772,7 @@ cpack + -G DEB true false @@ -1719,6 +1780,7 @@ cpack + -G RPM true false @@ -1726,6 +1788,7 @@ cpack + -G TGZ true false @@ -1733,6 +1796,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2065,6 +2129,22 @@ true true + + make + -j5 + testRot2.run + true + true + true + + + make + -j5 + testRot3Q.run + true + true + true + make -j2 @@ -2161,6 +2241,14 @@ true true + + make + -j5 + testVelocityConstraint.run + true + true + true + make -j1 @@ -2321,6 +2409,14 @@ true true + + make + -j5 + testNumericalDerivative.run + true + true + true + make -j5 @@ -2459,6 +2555,7 @@ make + testGraph.run true false @@ -2466,6 +2563,7 @@ make + testJunctionTree.run true false @@ -2473,6 +2571,7 @@ make + testSymbolicBayesNetB.run true false @@ -2534,6 +2633,14 @@ true true + + make + -j5 + testManifold.run + true + true + true + make -j5 @@ -2566,6 +2673,14 @@ true true + + make + -j5 + testAdaptAutoDiff.run + true + true + true + make -j2 @@ -2968,7 +3083,6 @@ make - tests/testGaussianISAM2 true false diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 23e5fd023..2a8d4bc41 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -174,4 +174,18 @@ private: } }; + +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public Dynamic { +}; + +} + } // \namespace gtsam diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 21a2e10ad..750a8a21f 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -111,4 +111,22 @@ namespace gtsam { private: double d_; }; + + // Define GTSAM traits + namespace traits { + + template<> + struct is_group : public std::true_type { + }; + + template<> + struct is_manifold : public std::true_type { + }; + + template<> + struct dimension : public std::integral_constant { + }; + + } + } // \namespace gtsam diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index a8bfe3007..8286c95a6 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -128,6 +128,19 @@ private: ar & boost::serialization::make_nvp("Vector", boost::serialization::base_object(*this)); } - }; + +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public Dynamic { +}; + +} + } // \namespace gtsam diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index ceebf6bad..63390ec1f 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -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 #include +#include +#include +#include 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 +struct is_group: public std::false_type { +}; + +// identity, no default provided, by default given by default constructor +template +struct identity { + static T value() { + return T(); + } +}; + +// is manifold, by default this is false +template +struct is_manifold: public std::false_type { +}; + +// dimension, can return Eigen::Dynamic (-1) if not known at compile time +template +struct dimension; + +/** + * zero::value is intended to be the origin of a canonical coordinate system + * with canonical(t) == DefaultChart(zero::value).apply(t) + * Below we provide the group identity as zero *in case* it is a group + */ +template struct zero: public identity { + BOOST_STATIC_ASSERT(is_group::value); +}; + +// double + +template<> +struct is_group : public std::true_type { +}; + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +template<> +struct zero { + static double value() { + return 0; + } +}; + +// Fixed size Eigen::Matrix type + +template +struct is_group > : public std::true_type { +}; + +template +struct is_manifold > : public std::true_type { +}; + +// TODO: Could be more sophisticated using Eigen traits and SFINAE? + +typedef std::integral_constant Dynamic; + +template +struct dimension > : public Dynamic { +}; + +template +struct dimension > : public Dynamic { +}; + +template +struct dimension > : public Dynamic { +}; + +template +struct dimension > : public std::integral_constant< + int, M * N> { +}; + +template +struct zero > : 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 value() { + return Eigen::Matrix::Zero(); + } +}; + +} // \ namespace traits + +// Chart is a map from T -> vector, retract is its inverse +template +struct DefaultChart { + BOOST_STATIC_ASSERT(traits::is_manifold::value); + typedef Eigen::Matrix::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::value is a chart around zero::value + * An example is Canonical + */ +template struct Canonical { + typedef T type; + typedef typename DefaultChart::vector vector; + DefaultChart chart; + Canonical() : + chart(traits::zero::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 { + typedef Eigen::Matrix 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 +struct DefaultChart > { + typedef Eigen::Matrix T; + typedef Eigen::Matrix::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 map(diff.data()); + return vector(map); + } + T retract(const vector& d) { + Eigen::Map map(d.data()); + return t_ + map; + } +}; + +// Dynamically sized Vector +template<> +struct DefaultChart { + 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 +template 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 diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 036f23f68..87c912e57 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -16,7 +16,6 @@ */ // \callgraph - #pragma once #include @@ -29,597 +28,492 @@ #pragma GCC diagnostic pop #endif -#include #include +#include namespace gtsam { - /* - * Note that all of these functions have two versions, a boost.function version and a - * standard C++ function pointer version. This allows reformulating the arguments of - * a function to fit the correct structure, which is useful for situations like - * member functions and functions with arguments not involved in the derivative: - * - * Usage of the boost bind version to rearrange arguments: - * for a function with one relevant param and an optional derivative: - * Foo bar(const Obj& a, boost::optional H1) - * Use boost.bind to restructure: - * boost::bind(bar, _1, boost::none) - * This syntax will fix the optional argument to boost::none, while using the first argument provided - * - * For member functions, such as below, with an instantiated copy instanceOfSomeClass - * Foo SomeClass::bar(const Obj& a) - * Use boost bind as follows to create a function pointer that uses the member function: - * boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1) - * - * For additional details, see the documentation: - * http://www.boost.org/doc/libs/release/libs/bind/bind.html - */ +/* + * Note that all of these functions have two versions, a boost.function version and a + * standard C++ function pointer version. This allows reformulating the arguments of + * a function to fit the correct structure, which is useful for situations like + * member functions and functions with arguments not involved in the derivative: + * + * Usage of the boost bind version to rearrange arguments: + * for a function with one relevant param and an optional derivative: + * Foo bar(const Obj& a, boost::optional H1) + * Use boost.bind to restructure: + * boost::bind(bar, _1, boost::none) + * This syntax will fix the optional argument to boost::none, while using the first argument provided + * + * For member functions, such as below, with an instantiated copy instanceOfSomeClass + * Foo SomeClass::bar(const Obj& a) + * Use boost bind as follows to create a function pointer that uses the member function: + * boost::bind(&SomeClass::bar, ref(instanceOfSomeClass), _1) + * + * For additional details, see the documentation: + * http://www.boost.org/doc/libs/release/libs/bind/bind.html + */ +/** + * Numerically compute gradient of scalar function + * Class X is the input argument + * The class X needs to have dim, expmap, logmap + */ +template +Vector numericalGradient(boost::function h, const X& x, + double delta = 1e-5) { + double factor = 1.0 / (2.0 * delta); - /** global functions for converting to a LieVector for use with numericalDerivative */ - inline LieVector makeLieVector(const Vector& v) { return LieVector(v); } - inline LieVector makeLieVectorD(double d) { return LieVector((Vector) (Vector(1) << d)); } + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X must be a manifold type."); + static const int N = traits::dimension::value; + BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); + typedef DefaultChart ChartX; + typedef typename ChartX::vector TangentX; - /** - * Numerically compute gradient of scalar function - * Class X is the input argument - * The class X needs to have dim, expmap, logmap - */ - template - Vector numericalGradient(boost::function h, const X& x, double delta=1e-5) { - double factor = 1.0/(2.0*delta); - const size_t n = x.dim(); - Vector d = zero(n), g = zero(n); - for (size_t j=0;j - Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) { - return numericalGradient(boost::bind(h, _1), x, delta); - } - - /** - * Compute numerical derivative in argument 1 of unary function - * @param h unary function yielding m-vector - * @param x n-dimensional value at which to evaluate h - * @param delta increment for numerical derivative - * Class Y is the output argument - * Class X is the input argument - * @return m*n Jacobian computed via central differencing - * Both classes X,Y need dim, expmap, logmap - */ - template - Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { - Y hx = h(x); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(h, _1), x, delta); - } - -// /** remapping for double valued functions */ -// template -// Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { -// return numericalDerivative11(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta); -// } - - template - Matrix numericalDerivative11(double (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVectorD, boost::bind(h, _1)), x, delta); - } - - /** remapping for vector valued functions */ - template - Matrix numericalDerivative11(boost::function h, const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta); - } - - template - Matrix numericalDerivative11(Vector (*h)(const X&), const X& x, double delta=1e-5) { - return numericalDerivative11(boost::bind(makeLieVector, boost::bind(h, _1)), x, delta); - } - - /** - * Compute numerical derivative in argument 1 of binary function - * @param h binary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2 need dim, expmap, logmap - */ - template - Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - Y hx = h(x1,x2); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x1.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21(boost::bind(h, _1, _2), x1, x2, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } - - template - Matrix numericalDerivative21(double (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative21(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } - - template - inline Matrix numericalDerivative21(Vector (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative21( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } - - /** - * Compute numerical derivative in argument 2 of binary function - * @param h binary function yielding m-vector - * @param x1 first argument value - * @param x2 n-dimensional second argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2 need dim, expmap, logmap - */ - template - Matrix numericalDerivative22 - (boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - Y hx = h(x1,x2); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x2.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative22 - (Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22(boost::bind(h, _1, _2), x1, x2, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative22(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } - - template - inline Matrix numericalDerivative22(double (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2)), x1, x2, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative22(boost::function h, - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } - - template - inline Matrix numericalDerivative22(Vector (*h)(const X1&, const X2&), - const X1& x1, const X2& x2, double delta=1e-5) { - return numericalDerivative22( - boost::bind(makeLieVector, boost::bind(h, _1, _2)), x1, x2, delta); - } - - /** - * Compute numerical derivative in argument 1 of ternary function - * @param h ternary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param x3 third argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2,X3 need dim, expmap, logmap - */ - template - Matrix numericalDerivative31 - (boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) - { - Y hx = h(x1,x2,x3); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x1.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative31 - (Y (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative31(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative31(double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative31(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative31(Vector (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative31( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** - * Compute numerical derivative in argument 2 of ternary function - * @param h ternary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param x3 third argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2,X3 need dim, expmap, logmap - */ - template - Matrix numericalDerivative32 - (boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) - { - Y hx = h(x1,x2,x3); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x2.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative32 - (Y (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative32(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative32(double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative32(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative32(Vector (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative32( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** - * Compute numerical derivative in argument 3 of ternary function - * @param h ternary function yielding m-vector - * @param x1 n-dimensional first argument value - * @param x2 second argument value - * @param x3 third argument value - * @param delta increment for numerical derivative - * @return m*n Jacobian computed via central differencing - * All classes Y,X1,X2,X3 need dim, expmap, logmap - */ - template - Matrix numericalDerivative33 - (boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) - { - Y hx = h(x1,x2,x3); - double factor = 1.0/(2.0*delta); - const size_t m = hx.dim(), n = x3.dim(); - Vector d = zero(n); - Matrix H = zeros(m,n); - for (size_t j=0;j - inline Matrix numericalDerivative33 - (Y (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for double return values */ - template - Matrix numericalDerivative33(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative33(double (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVectorD, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** pseudo-partial template specialization for vector return values */ - template - Matrix numericalDerivative33(boost::function h, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - template - inline Matrix numericalDerivative33(Vector (*h)(const X1&, const X2&, const X3&), - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalDerivative33( - boost::bind(makeLieVector, boost::bind(h, _1, _2, _3)), x1, x2, x3, delta); - } - - /** - * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar - * function. This is implemented simply as the derivative of the gradient. - * @param f A function taking a Lie object as input and returning a scalar - * @param x The center point for computing the Hessian - * @param delta The numerical derivative step size - * @return n*n Hessian matrix computed via central differencing - */ - template - inline Matrix numericalHessian(boost::function f, const X& x, double delta=1e-5) { - return numericalDerivative11(boost::function(boost::bind( - static_cast,const X&, double)>(&numericalGradient), - f, _1, delta)), x, delta); - } - - template - inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta=1e-5) { - return numericalHessian(boost::function(f), x, delta); - } - - - /** Helper class that computes the derivative of f w.r.t. x1, centered about - * x1_, as a function of x2 - */ - template - class G_x1 { - const boost::function& f_; - const X1& x1_; - double delta_; - public: - G_x1(const boost::function& f, const X1& x1, double delta) : f_(f), x1_(x1), delta_(delta) {} - Vector operator()(const X2& x2) { - return numericalGradient(boost::function(boost::bind(f_, _1, x2)), x1_, delta_); - } - }; - - template - inline Matrix numericalHessian212(boost::function f, const X1& x1, const X2& x2, double delta=1e-5) { - G_x1 g_x1(f, x1, delta); - return numericalDerivative11(boost::function(boost::bind(boost::ref(g_x1), _1)), x2, delta); - } - - - template - inline Matrix numericalHessian212(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalHessian212(boost::function(f), x1, x2, delta); - } - - - template - inline Matrix numericalHessian211(boost::function f, const X1& x1, const X2& x2, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X1&, double) = &numericalGradient; - boost::function f2(boost::bind(f, _1, x2)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x1, delta); - } - - - template - inline Matrix numericalHessian211(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalHessian211(boost::function(f), x1, x2, delta); - } - - - template - inline Matrix numericalHessian222(boost::function f, const X1& x1, const X2& x2, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X2&, double) = &numericalGradient; - boost::function f2(boost::bind(f, x1, _1)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x2, delta); - } - - - template - inline Matrix numericalHessian222(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta=1e-5) { - return numericalHessian222(boost::function(f), x1, x2, delta); - } - - /** - * Numerical Hessian for tenary functions - */ - /* **************************************************************** */ - template - inline Matrix numericalHessian311(boost::function f, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X1&, double) = &numericalGradient; - boost::function f2(boost::bind(f, _1, x2, x3)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x1, delta); - } - - template - inline Matrix numericalHessian311(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian311(boost::function(f), x1, x2, x3, delta); - } - - /* **************************************************************** */ - template - inline Matrix numericalHessian322(boost::function f, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X2&, double) = &numericalGradient; - boost::function f2(boost::bind(f, x1, _1, x3)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x2, delta); - } - - template - inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian322(boost::function(f), x1, x2, x3, delta); - } - - /* **************************************************************** */ - template - inline Matrix numericalHessian333(boost::function f, - const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - - Vector (*numGrad)(boost::function, const X3&, double) = &numericalGradient; - boost::function f2(boost::bind(f, x1, x2, _1)); - - return numericalDerivative11(boost::function(boost::bind(numGrad, f2, _1, delta)), x3, delta); - } - - template - inline Matrix numericalHessian333(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian333(boost::function(f), x1, x2, x3, delta); - } - - /* **************************************************************** */ - template - inline Matrix numericalHessian312(boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian212(boost::function(boost::bind(f, _1, _2, x3)), x1, x2, delta ); - } - - template - inline Matrix numericalHessian313(boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian212(boost::function(boost::bind(f, _1, x2, _2)), x1, x3, delta ); - } - - template - inline Matrix numericalHessian323(boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian212(boost::function(boost::bind(f, x1, _1, _2)), x2, x3, delta ); - } - - /* **************************************************************** */ - template - inline Matrix numericalHessian312(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian312(boost::function(f), x1, x2, x3, delta); - } - - template - inline Matrix numericalHessian313(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian313(boost::function(f), x1, x2, x3, delta); - } - - template - inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta=1e-5) { - return numericalHessian323(boost::function(f), x1, x2, x3, delta); + // get chart at x + ChartX chartX(x); + + // Prepare a tangent vector to perturb x with, only works for fixed size + TangentX d; + d.setZero(); + + Vector g = zero(N); // Can be fixed size + for (int j = 0; j < N; j++) { + d(j) = delta; + double hxplus = h(chartX.retract(d)); + d(j) = -delta; + double hxmin = h(chartX.retract(d)); + d(j) = 0; + g(j) = (hxplus - hxmin) * factor; } + return g; +} + +/** + * @brief New-style numerical derivatives using manifold_traits + * @brief Computes numerical derivative in argument 1 of unary function + * @param h unary function yielding m-vector + * @param x n-dimensional value at which to evaluate h + * @param delta increment for numerical derivative + * Class Y is the output argument + * Class X is the input argument + * @return m*n Jacobian computed via central differencing + */ +template +// TODO Should compute fixed-size matrix +Matrix numericalDerivative11(boost::function h, const X& x, + double delta = 1e-5) { + using namespace traits; + + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + typedef DefaultChart ChartY; + typedef typename ChartY::vector TangentY; + + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X must be a manifold type."); + static const int N = traits::dimension::value; + BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); + typedef DefaultChart ChartX; + typedef typename ChartX::vector TangentX; + + // get value at x, and corresponding chart + Y hx = h(x); + ChartY chartY(hx); + + // Bit of a hack for now to find number of rows + TangentY zeroY = chartY.apply(hx); + size_t m = zeroY.size(); + + // get chart at x + ChartX chartX(x); + + // Prepare a tangent vector to perturb x with, only works for fixed size + TangentX dx; + dx.setZero(); + + // Fill in Jacobian H + Matrix H = zeros(m, N); + double factor = 1.0 / (2.0 * delta); + for (int j = 0; j < N; j++) { + dx(j) = delta; + TangentY dy1 = chartY.apply(h(chartX.retract(dx))); + dx(j) = -delta; + TangentY dy2 = chartY.apply(h(chartX.retract(dx))); + dx(j) = 0; + H.col(j) << (dy1 - dy2) * factor; + } + return H; +} + +/** use a raw C++ function pointer */ +template +Matrix numericalDerivative11(Y (*h)(const X&), const X& x, + double delta = 1e-5) { + return numericalDerivative11(boost::bind(h, _1), x, delta); +} + +/** + * Compute numerical derivative in argument 1 of binary function + * @param h binary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +Matrix numericalDerivative21(const boost::function& h, + const X1& x1, const X2& x2, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, _1, x2), x1, delta); +} + +/** use a raw C++ function pointer */ +template +inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1, + const X2& x2, double delta = 1e-5) { + return numericalDerivative21(boost::bind(h, _1, _2), x1, x2, delta); +} + +/** + * Compute numerical derivative in argument 2 of binary function + * @param h binary function yielding m-vector + * @param x1 first argument value + * @param x2 n-dimensional second argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +Matrix numericalDerivative22(boost::function h, + const X1& x1, const X2& x2, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X2 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, _1), x2, delta); +} + +/** use a raw C++ function pointer */ +template +inline Matrix numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1, + const X2& x2, double delta = 1e-5) { + return numericalDerivative22(boost::bind(h, _1, _2), x1, x2, delta); +} + +/** + * Compute numerical derivative in argument 1 of ternary function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + * All classes Y,X1,X2,X3 need dim, expmap, logmap + */ +template +Matrix numericalDerivative31( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, _1, x2, x3), x1, delta); +} + +template +inline Matrix numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalDerivative31(boost::bind(h, _1, _2, _3), x1, + x2, x3, delta); +} + +/** + * Compute numerical derivative in argument 2 of ternary function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + * All classes Y,X1,X2,X3 need dim, expmap, logmap + */ +template +Matrix numericalDerivative32( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X2 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, _1, x3), x2, delta); +} + +template +inline Matrix numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalDerivative32(boost::bind(h, _1, _2, _3), x1, + x2, x3, delta); +} + +/** + * Compute numerical derivative in argument 3 of ternary function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + * All classes Y,X1,X2,X3 need dim, expmap, logmap + */ +template +Matrix numericalDerivative33( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X3 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, x2, _1), x3, delta); +} + +template +inline Matrix numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalDerivative33(boost::bind(h, _1, _2, _3), x1, + x2, x3, delta); +} + +/** + * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar + * function. This is implemented simply as the derivative of the gradient. + * @param f A function taking a Lie object as input and returning a scalar + * @param x The center point for computing the Hessian + * @param delta The numerical derivative step size + * @return n*n Hessian matrix computed via central differencing + */ +template +inline Matrix numericalHessian(boost::function f, const X& x, + double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + "Template argument X must be a manifold type."); + typedef boost::function F; + typedef boost::function G; + G ng = static_cast(numericalGradient ); + return numericalDerivative11(boost::bind(ng, f, _1, delta), x, + delta); +} + +template +inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta = + 1e-5) { + return numericalHessian(boost::function(f), x, delta); +} + +/** Helper class that computes the derivative of f w.r.t. x1, centered about + * x1_, as a function of x2 + */ +template +class G_x1 { + const boost::function& f_; + X1 x1_; + double delta_; +public: + G_x1(const boost::function& f, const X1& x1, + double delta) : + f_(f), x1_(x1), delta_(delta) { + } + Vector operator()(const X2& x2) { + return numericalGradient(boost::bind(f_, _1, x2), x1_, delta_); + } +}; + +template +inline Matrix numericalHessian212( + boost::function f, const X1& x1, const X2& x2, + double delta = 1e-5) { + G_x1 g_x1(f, x1, delta); + return numericalDerivative11( + boost::function( + boost::bind(boost::ref(g_x1), _1)), x2, delta); +} + +template +inline Matrix numericalHessian212(double (*f)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta = 1e-5) { + return numericalHessian212(boost::function(f), + x1, x2, delta); +} + +template +inline Matrix numericalHessian211( + boost::function f, const X1& x1, const X2& x2, + double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X1&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, _1, x2)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x1, delta); +} + +template +inline Matrix numericalHessian211(double (*f)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta = 1e-5) { + return numericalHessian211(boost::function(f), + x1, x2, delta); +} + +template +inline Matrix numericalHessian222( + boost::function f, const X1& x1, const X2& x2, + double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X2&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, x1, _1)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x2, delta); +} + +template +inline Matrix numericalHessian222(double (*f)(const X1&, const X2&), + const X1& x1, const X2& x2, double delta = 1e-5) { + return numericalHessian222(boost::function(f), + x1, x2, delta); +} + +/** + * Numerical Hessian for tenary functions + */ +/* **************************************************************** */ +template +inline Matrix numericalHessian311( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X1&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, _1, x2, x3)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x1, delta); +} + +template +inline Matrix numericalHessian311(double (*f)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian311( + boost::function(f), x1, x2, x3, + delta); +} + +/* **************************************************************** */ +template +inline Matrix numericalHessian322( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X2&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, x1, _1, x3)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x2, delta); +} + +template +inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian322( + boost::function(f), x1, x2, x3, + delta); +} + +/* **************************************************************** */ +template +inline Matrix numericalHessian333( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + + Vector (*numGrad)(boost::function, const X3&, + double) = &numericalGradient; + boost::function f2(boost::bind(f, x1, x2, _1)); + + return numericalDerivative11( + boost::function(boost::bind(numGrad, f2, _1, delta)), + x3, delta); +} + +template +inline Matrix numericalHessian333(double (*f)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian333( + boost::function(f), x1, x2, x3, + delta); +} + +/* **************************************************************** */ +template +inline Matrix numericalHessian312( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian212( + boost::function(boost::bind(f, _1, _2, x3)), + x1, x2, delta); +} + +template +inline Matrix numericalHessian313( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian212( + boost::function(boost::bind(f, _1, x2, _2)), + x1, x3, delta); +} + +template +inline Matrix numericalHessian323( + boost::function f, const X1& x1, + const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian212( + boost::function(boost::bind(f, x1, _1, _2)), + x2, x3, delta); +} + +/* **************************************************************** */ +template +inline Matrix numericalHessian312(double (*f)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian312( + boost::function(f), x1, x2, x3, + delta); +} + +template +inline Matrix numericalHessian313(double (*f)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian313( + boost::function(f), x1, x2, x3, + delta); +} + +template +inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&), + const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { + return numericalHessian323( + boost::function(f), x1, x2, x3, + delta); +} } diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index f7e4d3baa..b4a9b91d6 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -15,115 +15,123 @@ * @date Apr 8, 2011 */ +#include #include -#include - +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(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(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(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(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(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(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(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(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(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(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(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); +} /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index e508710cd..2de5a808d 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -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 : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +template<> +struct zero { + static Cal3Bundler value() { + return Cal3Bundler(0, 0, 0); + } +}; + +} + +} // namespace gtsam diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 82bfa4c5f..d716d398e 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -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 : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { }; } +} + diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index eacbf7053..ad8e7b904 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -126,10 +126,6 @@ public: private: - /// @} - /// @name Advanced Interface - /// @{ - /** Serialization function */ friend class boost::serialization::access; template @@ -140,9 +136,25 @@ private: ar & BOOST_SERIALIZATION_NVP(xi_); } - /// @} +}; +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +template<> +struct zero { + static Cal3Unified value() { return Cal3Unified();} }; } +} + diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 4e17c64f4..a87a30e36 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -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 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 : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +template<> +struct zero { + static Cal3_S2 value() { return Cal3_S2();} +}; + +} + } // \ namespace gtsam diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 32b966261..a973f9cec 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -196,5 +196,23 @@ private: }; +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +template<> +struct zero { + static EssentialMatrix value() { return EssentialMatrix();} +}; + +} + } // gtsam diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 86e6a9097..4b93ca70c 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -303,7 +303,7 @@ public: return K_.uncalibrate(pn); } - typedef Eigen::Matrix Matrix2K; + typedef Eigen::Matrix::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 @@ -614,6 +610,29 @@ private: ar & BOOST_SERIALIZATION_NVP(pose_); ar & BOOST_SERIALIZATION_NVP(K_); } - /// @} - } - ;} + +}; + +// Define GTSAM traits +namespace traits { + +template +struct is_manifold > : public std::true_type { +}; + +template +struct dimension > : public std::integral_constant< + int, dimension::value + dimension::value> { +}; + +template +struct zero > { + static PinholeCamera value() { + return PinholeCamera(zero::value(), + zero::value()); + } +}; + +} + +} // \ gtsam diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index ccab84233..7d1fab133 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -33,10 +33,9 @@ namespace gtsam { * \nosubgrouping */ class GTSAM_EXPORT Point2 : public DerivedValue { -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 : public std::true_type { +}; + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +} + } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 6e5b1ea8a..d69ceb861 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -37,11 +37,9 @@ namespace gtsam { * \nosubgrouping */ class GTSAM_EXPORT Point3 : public DerivedValue { - 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 : public std::true_type { + }; + + template<> + struct is_manifold : public std::true_type { + }; + + template<> + struct dimension : public std::integral_constant { + }; + + } } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 13773ddb4..b6a2314ff 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -36,7 +36,6 @@ namespace gtsam { class GTSAM_EXPORT Pose2 : public DerivedValue { 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 rotationInterval() { return std::make_pair(2, 2); } + /// @} + private: // Serialization function @@ -320,7 +321,18 @@ inline Matrix wedge(const Vector& xi) { typedef std::pair Point2Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); -/// @} +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +} } // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 55cda05a8..5f99b25ac 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -41,7 +41,6 @@ class Pose2; */ class GTSAM_EXPORT Pose3: public DerivedValue { 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(const Vector& xi) { typedef std::pair Point3Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); +// Define GTSAM traits +namespace traits { + +template<> +struct is_group : public std::true_type { +}; + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +} + } // namespace gtsam diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index d121beb12..4142d4473 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -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 : public std::true_type { + }; + + template<> + struct is_manifold : public std::true_type { + }; + + template<> + struct dimension : public std::integral_constant { + }; + + } } // gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 59a09ba39..62ac9f3f9 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -59,10 +59,9 @@ namespace gtsam { * \nosubgrouping */ class GTSAM_EXPORT Rot3 : public DerivedValue { - 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 RQ(const Matrix3& A); + + // Define GTSAM traits + namespace traits { + + template<> + struct is_group : public std::true_type { + }; + + template<> + struct is_manifold : public std::true_type { + }; + + template<> + struct dimension : public std::integral_constant { + }; + + } } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 70917b2c4..82914f6ab 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -155,4 +155,22 @@ private: }; +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +template<> +struct zero { + static StereoCamera value() { return StereoCamera();} +}; + +} + } diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 8ce2e49bf..000f7d16f 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -173,4 +173,20 @@ namespace gtsam { }; + // Define GTSAM traits + namespace traits { + + template<> + struct is_group : public std::true_type { + }; + + template<> + struct is_manifold : public std::true_type { + }; + + template<> + struct dimension : public std::integral_constant { + }; + + } } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 8d2c024c0..bb2ee318a 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -156,5 +156,25 @@ private: }; +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +template<> +struct zero { + static Unit3 value() { + return Unit3(); + } +}; + +} + } // namespace gtsam diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 6e62d4be0..905605b55 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -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 ) diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 66ee5a387..215f376f6 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -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 ) { diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 42b548f5a..12266c16f 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -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 ) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 175a11ff1..2a775379d 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -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( - boost::bind(testDerivExpmapInv, _1) - ), - LieVector(Vector::Zero(6)), 1e-5 - ); + Matrix numericalDerivExpmapInv = numericalDerivative11( + 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( - boost::bind(testDerivAdjoint, _1, _2) - ), - LieVector(screw::xi), LieVector(screw::xi), 1e-5 - ); + Matrix numericalH = numericalDerivative21( + 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( - boost::bind(testDerivAdjointTranspose, _1, _2) - ), - LieVector(xi), LieVector(v), 1e-5 - ); + Matrix numericalH = numericalDerivative21( + testDerivAdjointTranspose, xi, v, 1e-5); EXPECT(assert_equal(expected,actual,1e-15)); EXPECT(assert_equal(numericalH,actualH,1e-5)); diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index b0890057e..08f5a42e9 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -217,7 +217,7 @@ TEST( Rot3, rightJacobianExpMapSO3 ) // Linearization point Vector3 thetahat; thetahat << 0.1, 0, 0; - Matrix expectedJacobian = numericalDerivative11(boost::bind(&evaluateRotation, _1), LieVector(thetahat)); + Matrix expectedJacobian = numericalDerivative11(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(boost::bind(&evaluateLogRotation, thetahat, _1), LieVector(deltatheta)); + Matrix expectedJacobian = numericalDerivative11( + 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( - boost::bind(testDexpL, _1)), LieVector(zero(3)), 1e-2); + Matrix expectedDexpL = numericalDerivative11(testDexpL, + Vector3::Zero(), 1e-2); EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); Matrix actualDexpInvL = Rot3::dexpInvL(w); diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 51c195d32..0bd553a40 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -274,11 +274,7 @@ TEST( triangulation, TriangulationFactor ) { Matrix HActual; factor.evaluateError(landmark, HActual); -// Matrix expectedH1 = numericalDerivative11( -// boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, -// boost::none, boost::none), pose1); - // The expected Jacobian - Matrix HExpected = numericalDerivative11( + Matrix HExpected = numericalDerivative11( boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); // Verify the Jacobians are correct diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 4aa553df2..53ae2fc58 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -115,13 +115,13 @@ TEST(Unit3, error) { Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), q); p.error(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); } { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), r); p.error(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index d65f96f7f..cd5231e49 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include @@ -149,8 +148,9 @@ TEST( GaussianBayesNet, DeterminantTest ) } /* ************************************************************************* */ +typedef Eigen::Matrix Vector10; namespace { - double computeError(const GaussianBayesNet& gbn, const LieVector& values) + double computeError(const GaussianBayesNet& gbn, const Vector10& values) { pair 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(boost::bind(&computeError, gbn, _1)), - LieVector(Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols()))); + Matrix hessian = numericalHessian( + boost::bind(&computeError, gbn, _1), Vector10::Zero()); // Compute the gradient numerically - Vector gradient = numericalGradient( - boost::function(boost::bind(&computeError, gbn, _1)), - LieVector(Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols()))); + Vector gradient = numericalGradient( + boost::bind(&computeError, gbn, _1), Vector10::Zero()); // Compute the gradient using dense matrices Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian(); diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 8f37cac0c..217fab543 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -24,7 +24,6 @@ using namespace boost::assign; #include -#include #include #include #include @@ -175,13 +174,13 @@ TEST(GaussianBayesTree, complicatedMarginal) { EXPECT(assert_equal(expectedCov, actualCov, 1e1)); } +/* ************************************************************************* */ +typedef Eigen::Matrix Vector10; namespace { - /* ************************************************************************* */ - double computeError(const GaussianBayesTree& gbt, const LieVector& values) - { - pair Rd = GaussianFactorGraph(gbt).jacobian(); - return 0.5 * (Rd.first * values - Rd.second).squaredNorm(); - } +double computeError(const GaussianBayesTree& gbt, const Vector10& values) { + pair 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(boost::bind(&computeError, bt, _1)), - LieVector(Vector::Zero(GaussianFactorGraph(bt).jacobian().first.cols()))); + Matrix hessian = numericalHessian( + boost::bind(&computeError, bt, _1), Vector10::Zero()); // Compute the gradient numerically - Vector gradient = numericalGradient( - boost::function(boost::bind(&computeError, bt, _1)), - LieVector(Vector::Zero(GaussianFactorGraph(bt).jacobian().first.cols()))); + Vector gradient = numericalGradient( + boost::bind(&computeError, bt, _1), Vector10::Zero()); // Compute the gradient using dense matrices Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian(); diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 32911e589..8301a0a6b 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -218,6 +218,23 @@ namespace imuBias { } // namespace ImuBias +// Define GTSAM traits +namespace traits { + +template<> +struct is_group : public std::true_type { +}; + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +} + } // namespace gtsam diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index c4155ea16..790080556 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -45,7 +45,7 @@ TEST( Rot3AttitudeFactor, Constructor ) { EXPECT(assert_equal(zero(2),factor.evaluateError(nRb),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( 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( + Matrix expectedH = numericalDerivative11( boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1, boost::none), T); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 1a8b6160d..279c20e69 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -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( + Matrix expectedDelPdelBias = numericalDerivative11( 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( + Matrix expectedDelVdelBias = numericalDerivative11( 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); diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index f02047aa9..8c93020c9 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -56,7 +56,7 @@ TEST( GPSFactor, Constructors ) { EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( + Matrix expectedH = numericalDerivative11( boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); // Use the factor to calculate the derivative diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index a6894898b..823ce8306 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include #include #include @@ -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( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( 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( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( 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(boost::bind( - &evaluateRotation, measuredOmega, _1, deltaT), LieVector(biasOmega)); + Matrix expectedDelRdelBiasOmega = numericalDerivative11(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(boost::bind( - &evaluateLogRotation, thetahat, _1), LieVector(deltatheta)); + Matrix expectedDelFdeltheta = numericalDerivative11(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( + Matrix expectedDelPdelBias = numericalDerivative11( 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( + Matrix expectedDelVdelBias = numericalDerivative11( 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( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); // Check rotation Jacobians diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 1875e4f1c..5599c93d6 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -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 // + EXPECT( assert_equal(numericalDerivative11 // (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 // + EXPECT( assert_equal(numericalDerivative11 // (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 // + EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// H1, 1e-7)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT( assert_equal(numericalDerivative11 // (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 // + EXPECT(assert_equal(numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// H1, 1e-7)); - EXPECT(assert_equal(numericalDerivative11 // + EXPECT(assert_equal(numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// H2, 1e-7)); - EXPECT(assert_equal(numericalDerivative11 // + EXPECT(assert_equal(numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// H3, 1e-7)); } diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 5184393ac..4c0edf5c9 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -50,10 +50,10 @@ TEST( EssentialMatrixConstraint, test ) { CHECK(assert_equal(expected, actual, 1e-8)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11( + Matrix expectedH1 = numericalDerivative11( boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11( + Matrix expectedH2 = numericalDerivative11( boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 1e5674599..c889fb1e9 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -96,7 +96,7 @@ TEST (EssentialMatrixFactor, factor) { // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected; - Hexpected = numericalDerivative11( + Hexpected = numericalDerivative11( boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, boost::none), trueE); @@ -173,8 +173,8 @@ TEST (EssentialMatrixFactor2, factor) { boost::function f = boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); + Hexpected1 = numericalDerivative21(f, trueE, d); + Hexpected2 = numericalDerivative22(f, trueE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); @@ -247,8 +247,8 @@ TEST (EssentialMatrixFactor3, factor) { boost::function f = boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); + Hexpected1 = numericalDerivative21(f, bodyE, d); + Hexpected2 = numericalDerivative22(f, bodyE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); @@ -389,8 +389,8 @@ TEST (EssentialMatrixFactor2, extraTest) { boost::function f = boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); + Hexpected1 = numericalDerivative21(f, trueE, d); + Hexpected2 = numericalDerivative22(f, trueE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); @@ -458,8 +458,8 @@ TEST (EssentialMatrixFactor3, extraTest) { boost::function f = boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); + Hexpected1 = numericalDerivative21(f, bodyE, d); + Hexpected2 = numericalDerivative22(f, bodyE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index b40da2e2a..3cac377eb 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam/slam/tests/testPoseRotationPrior.cpp @@ -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( - 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( - 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( - 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( - boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testPoseTranslationPrior.cpp b/gtsam/slam/tests/testPoseTranslationPrior.cpp index 36d94c9a3..47ff708d9 100644 --- a/gtsam/slam/tests/testPoseTranslationPrior.cpp +++ b/gtsam/slam/tests/testPoseTranslationPrior.cpp @@ -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( - 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( - 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( - 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( - 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( - 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( - 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( - 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( - boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testRangeFactor.cpp b/gtsam/slam/tests/testRangeFactor.cpp index 4fa6164a1..cba9300f5 100644 --- a/gtsam/slam/tests/testRangeFactor.cpp +++ b/gtsam/slam/tests/testRangeFactor.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include @@ -37,12 +36,12 @@ typedef RangeFactor RangeFactor2D; typedef RangeFactor 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(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); + H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); + H2Expected = numericalDerivative11(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(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); + H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); + H2Expected = numericalDerivative11(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(boost::bind(&factorError3D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); + H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); + H2Expected = numericalDerivative11(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(boost::bind(&factorError3D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); + H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); + H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index 078bf85cd..309801ccb 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -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( + numericalDF = numericalDerivative31( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDT = numericalDerivative32( + numericalDT = numericalDerivative32( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDL = numericalDerivative33( + numericalDL = numericalDerivative33( 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( + numericalDF = numericalDerivative31( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDT = numericalDerivative32( + numericalDT = numericalDerivative32( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDL = numericalDerivative33( + numericalDL = numericalDerivative33( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index f36405318..15e8cf984 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -69,13 +69,13 @@ TEST (RotateFactor, test) { Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc); f.evaluateError(iRc, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } { - expected = numericalDerivative11( + expected = numericalDerivative11( 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( + expected = numericalDerivative11( boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, boost::none), iRc); f.evaluateError(iRc, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, boost::none), R); f.evaluateError(R, actual); diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index 51e09ca5f..80729e8a2 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -183,4 +183,23 @@ private: } }; +// Define GTSAM traits +namespace traits { + +template<> +struct is_manifold : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +template<> +struct zero { + static PoseRTV value() { + return PoseRTV(); + } +}; + +} } // \namespace gtsam diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 252f9dbfe..c9db449f9 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -83,9 +83,9 @@ public: virtual gtsam::Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, boost::optional H1=boost::none, boost::optional H2=boost::none) const { - if (H1) *H1 = gtsam::numericalDerivative21( + if (H1) *H1 = gtsam::numericalDerivative21( boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5); - if (H2) *H2 = gtsam::numericalDerivative22( + if (H2) *H2 = gtsam::numericalDerivative22( 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(); diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 82197302b..475d5285c 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -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( + boost::function( 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( + boost::function( 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( + boost::function( 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( + boost::function( 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( + boost::function( 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( + boost::function( 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( + boost::function( boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index 2a814b915..c1e12b4a7 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -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 : public std::true_type { +}; + +template<> +struct dimension : public std::integral_constant { +}; + +} + + } // \namespace gtsam diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index b477d3e44..3385998b0 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -29,7 +29,7 @@ TEST( InvDepthFactor, Project1) { Point2 expected_uv = level_camera.project(landmark); InvDepthCamera3 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 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 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 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(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(project_,level_pose, landmark, inv_depth); + Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); InvDepthCamera3 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(project_,level_pose, landmark, inv_depth); + Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); InvDepthCamera3 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(project_,level_pose, landmark, inv_depth); + Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); InvDepthCamera3 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 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 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)); diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 6a5502fd1..d5c5f1279 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -22,6 +22,8 @@ #include #include #include +#include + #include #include @@ -38,7 +40,6 @@ namespace MPL = boost::mpl::placeholders; #include // 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 -void handleLeafCase(const Eigen::Matrix& dTdA, +void handleLeafCase(const Eigen::Matrix& dTdA, JacobianMap& jacobians, Key key) { JacobianMap::iterator it = jacobians.find(key); - it->second.block(0,0) += dTdA; // block makes HUGE difference + it->second.block(0, 0) += dTdA; // block makes HUGE difference } /// Handle Leaf Case for Dynamic Matrix type (slower) template<> -void handleLeafCase(const Eigen::Matrix& dTdA, +void handleLeafCase( + const Eigen::Matrix& dTdA, JacobianMap& jacobians, Key key) { JacobianMap::iterator it = jacobians.find(key); it->second += dTdA; @@ -90,20 +92,35 @@ void handleLeafCase(const Eigen::Matrix& 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 ExecutionTrace { + static const int Dim = traits::dimension::value; enum { Constant, Leaf, Function } kind; union { Key key; - CallRecord* ptr; + CallRecord* 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* record) { + void setFunction(CallRecord* 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 JacobianTT; + typedef Eigen::Matrix 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 Jacobian2T; + typedef Eigen::Matrix 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 struct Select { - typedef Eigen::Matrix Jacobian; + typedef Eigen::Matrix::value> Jacobian; static void reverseAD(const ExecutionTrace& 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 struct Select<2, A> { - typedef Eigen::Matrix Jacobian; + typedef Eigen::Matrix::value> Jacobian; static void reverseAD(const ExecutionTrace& 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& map) const { - map[key_] = T::dimension; + map[key_] = traits::dimension::value; } /// Return value @@ -351,13 +368,15 @@ public: /// meta-function to generate fixed-size JacobianTA type template struct Jacobian { - typedef Eigen::Matrix type; + typedef Eigen::Matrix::value, + traits::dimension::value> type; }; /// meta-function to generate JacobianTA optional reference template struct Optional { - typedef Eigen::Matrix Jacobian; + typedef Eigen::Matrix::value, + traits::dimension::value> Jacobian; typedef boost::optional type; }; @@ -368,7 +387,7 @@ template struct FunctionalBase: ExpressionNode { static size_t const N = 0; // number of arguments - typedef CallRecord Record; + typedef CallRecord::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, Base { /// Start the reverse AD process virtual void startReverseAD(JacobianMap& jacobians) const { Base::Record::startReverseAD(jacobians); - Select::reverseAD(This::trace, This::dTdA, jacobians); + Select::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, Base { } /// Version specialized to 2-dimensional output - typedef Eigen::Matrix Jacobian2T; + typedef Eigen::Matrix::value> Jacobian2T; virtual void reverseAD2(const Jacobian2T& dFdT, JacobianMap& jacobians) const { Base::Record::reverseAD2(dFdT, jacobians); diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 1ab69880e..6ac7d9ce8 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -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 trace; @@ -154,7 +159,8 @@ public: template struct apply_compose { typedef T result_type; - typedef Eigen::Matrix Jacobian; + static const int Dim = traits::dimension::value; + typedef Eigen::Matrix Jacobian; T operator()(const T& x, const T& y, boost::optional H1, boost::optional H2) const { return x.compose(y, H1, H2); diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 8030165b9..84456c934 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -37,6 +37,8 @@ class ExpressionFactor: public NoiseModelFactor { std::vector dimensions_; ///< dimensions of the Jacobian matrices size_t augmentedCols_; ///< total number of columns + 1 (for RHS) + static const int Dim = traits::dimension::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 block = Hi.block(0, 0, T::dimension, - dimensions_[i]); + Eigen::Block 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 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 > // - 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 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 > // + 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 diff --git a/gtsam_unstable/nonlinear/ceres_autodiff.h b/gtsam_unstable/nonlinear/ceres_autodiff.h new file mode 100644 index 000000000..2a0ac8987 --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_autodiff.h @@ -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 +// 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. +// +// 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 +// bool operator()(const T *p, const T *q, T *z) { +// // ... +// } +// }; +// +// In this case, the necessary jet type is Jet. 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 + +#include +#include +#include +#include +#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 +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 +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 +inline void Take1stOrderPart(const int M, const JetT *src, T *dst) { + DCHECK(src); + DCHECK(dst); + for (int i = 0; i < M; ++i) { + Eigen::Map >(dst + N * i, N) = + src[i].v.template segment(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 +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 JetT; + FixedArray 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( \ + 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::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(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_ diff --git a/gtsam_unstable/nonlinear/ceres_eigen.h b/gtsam_unstable/nonlinear/ceres_eigen.h new file mode 100644 index 000000000..18a602cf4 --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_eigen.h @@ -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 + +namespace ceres { + +typedef Eigen::Matrix Vector; +typedef Eigen::Matrix Matrix; +typedef Eigen::Map VectorRef; +typedef Eigen::Map MatrixRef; +typedef Eigen::Map ConstVectorRef; +typedef Eigen::Map ConstMatrixRef; + +// Column major matrices for DenseSparseMatrix/DenseQRSolver +typedef Eigen::Matrix ColMajorMatrix; + +typedef Eigen::Map > ColMajorMatrixRef; + +typedef Eigen::Map > ConstColMajorMatrixRef; + + + +// C++ does not support templated typdefs, thus the need for this +// struct so that we can support statically sized Matrix and Maps. +template +struct EigenTypes { + typedef Eigen::Matrix + Matrix; + + typedef Eigen::Map< + Eigen::Matrix > + MatrixRef; + + typedef Eigen::Matrix + Vector; + + typedef Eigen::Map < + Eigen::Matrix > + VectorRef; + + + typedef Eigen::Map< + const Eigen::Matrix > + ConstMatrixRef; + + typedef Eigen::Map < + const Eigen::Matrix > + ConstVectorRef; +}; + +} // namespace ceres + +#endif // CERES_INTERNAL_EIGEN_H_ diff --git a/gtsam_unstable/nonlinear/ceres_fixed_array.h b/gtsam_unstable/nonlinear/ceres_fixed_array.h new file mode 100644 index 000000000..4586fe524 --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_fixed_array.h @@ -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 +#include +#include +#include + +namespace ceres { +namespace internal { + +// A FixedArray 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 FixedArray 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 +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 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 inline_space_[kInlineElements]; +}; + +// Implementation details follow + +template +inline FixedArray::FixedArray(typename FixedArray::size_type n) + : size_(n), + array_((n <= kInlineElements + ? reinterpret_cast(inline_space_) + : new InnerContainer[n])) { + // Construct only the elements actually used. + if (array_ == reinterpret_cast(inline_space_)) { + for (size_t i = 0; i != size_; ++i) { + inline_space_[i].Init(); + } + } +} + +template +inline FixedArray::~FixedArray() { + if (array_ != reinterpret_cast(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_ diff --git a/gtsam_unstable/nonlinear/ceres_fpclassify.h b/gtsam_unstable/nonlinear/ceres_fpclassify.h new file mode 100644 index 000000000..da8a4d086 --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_fpclassify.h @@ -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 +#endif + +#include + +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::infinity() || + x == -std::numeric_limits::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_ diff --git a/gtsam_unstable/nonlinear/ceres_jet.h b/gtsam_unstable/nonlinear/ceres_jet.h new file mode 100644 index 000000000..ed4834caf --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_jet.h @@ -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 +// T f(const T &x, const T &y) { +// return x * x + x * y; +// } +// +// // The "2" means there should be 2 dual number components. +// Jet x(0); // Pick the 0th dual number for x. +// Jet y(1); // Pick the 1st dual number for y. +// Jet 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 +#include +#include // NOLINT +#include +#include + +#include +#include + +namespace ceres { + +template +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). 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 + EIGEN_STRONG_INLINE Jet(const T& a, const Eigen::DenseBase &v) + : a(a), v(v) { + } + + // Compound operators + Jet& operator+=(const Jet &y) { + *this = *this + y; + return *this; + } + + Jet& operator-=(const Jet &y) { + *this = *this - y; + return *this; + } + + Jet& operator*=(const Jet &y) { + *this = *this * y; + return *this; + } + + Jet& operator/=(const Jet &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 v; +}; + +// Unary + +template inline +Jet const& operator+(const Jet& f) { + return f; +} + +// TODO(keir): Try adding __attribute__((always_inline)) to these functions to +// see if it causes a performance increase. + +// Unary - +template inline +Jet operator-(const Jet&f) { + return Jet(-f.a, -f.v); +} + +// Binary + +template inline +Jet operator+(const Jet& f, + const Jet& g) { + return Jet(f.a + g.a, f.v + g.v); +} + +// Binary + with a scalar: x + s +template inline +Jet operator+(const Jet& f, T s) { + return Jet(f.a + s, f.v); +} + +// Binary + with a scalar: s + x +template inline +Jet operator+(T s, const Jet& f) { + return Jet(f.a + s, f.v); +} + +// Binary - +template inline +Jet operator-(const Jet& f, + const Jet& g) { + return Jet(f.a - g.a, f.v - g.v); +} + +// Binary - with a scalar: x - s +template inline +Jet operator-(const Jet& f, T s) { + return Jet(f.a - s, f.v); +} + +// Binary - with a scalar: s - x +template inline +Jet operator-(T s, const Jet& f) { + return Jet(s - f.a, -f.v); +} + +// Binary * +template inline +Jet operator*(const Jet& f, + const Jet& g) { + return Jet(f.a * g.a, f.a * g.v + f.v * g.a); +} + +// Binary * with a scalar: x * s +template inline +Jet operator*(const Jet& f, T s) { + return Jet(f.a * s, f.v * s); +} + +// Binary * with a scalar: s * x +template inline +Jet operator*(T s, const Jet& f) { + return Jet(f.a * s, f.v * s); +} + +// Binary / +template inline +Jet operator/(const Jet& f, + const Jet& 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(f.a * g_a_inverse, (f.v - f_a_by_g_a * g.v) * g_a_inverse); +} + +// Binary / with a scalar: s / x +template inline +Jet operator/(T s, const Jet& g) { + const T minus_s_g_a_inverse2 = -s / (g.a * g.a); + return Jet(s / g.a, g.v * minus_s_g_a_inverse2); +} + +// Binary / with a scalar: x / s +template inline +Jet operator/(const Jet& f, T s) { + const T s_inverse = 1.0 / s; + return Jet(f.a * s_inverse, f.v * s_inverse); +} + +// Binary comparison operators for both scalars and jets. +#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \ +template inline \ +bool operator op(const Jet& f, const Jet& g) { \ + return f.a op g.a; \ +} \ +template inline \ +bool operator op(const T& s, const Jet& g) { \ + return s op g.a; \ +} \ +template inline \ +bool operator op(const Jet& 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 inline +Jet abs(const Jet& f) { + return f.a < T(0.0) ? -f : f; +} + +// log(a + h) ~= log(a) + h / a +template inline +Jet log(const Jet& f) { + const T a_inverse = T(1.0) / f.a; + return Jet(log(f.a), f.v * a_inverse); +} + +// exp(a + h) ~= exp(a) + exp(a) h +template inline +Jet exp(const Jet& f) { + const T tmp = exp(f.a); + return Jet(tmp, tmp * f.v); +} + +// sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a)) +template inline +Jet sqrt(const Jet& f) { + const T tmp = sqrt(f.a); + const T two_a_inverse = T(1.0) / (T(2.0) * tmp); + return Jet(tmp, f.v * two_a_inverse); +} + +// cos(a + h) ~= cos(a) - sin(a) h +template inline +Jet cos(const Jet& f) { + return Jet(cos(f.a), - sin(f.a) * f.v); +} + +// acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h +template inline +Jet acos(const Jet& f) { + const T tmp = - T(1.0) / sqrt(T(1.0) - f.a * f.a); + return Jet(acos(f.a), tmp * f.v); +} + +// sin(a + h) ~= sin(a) + cos(a) h +template inline +Jet sin(const Jet& f) { + return Jet(sin(f.a), cos(f.a) * f.v); +} + +// asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h +template inline +Jet asin(const Jet& f) { + const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a); + return Jet(asin(f.a), tmp * f.v); +} + +// tan(a + h) ~= tan(a) + (1 + tan(a)^2) h +template inline +Jet tan(const Jet& f) { + const T tan_a = tan(f.a); + const T tmp = T(1.0) + tan_a * tan_a; + return Jet(tan_a, tmp * f.v); +} + +// atan(a + h) ~= atan(a) + 1 / (1 + a^2) h +template inline +Jet atan(const Jet& f) { + const T tmp = T(1.0) / (T(1.0) + f.a * f.a); + return Jet(atan(f.a), tmp * f.v); +} + +// sinh(a + h) ~= sinh(a) + cosh(a) h +template inline +Jet sinh(const Jet& f) { + return Jet(sinh(f.a), cosh(f.a) * f.v); +} + +// cosh(a + h) ~= cosh(a) + sinh(a) h +template inline +Jet cosh(const Jet& f) { + return Jet(cosh(f.a), sinh(f.a) * f.v); +} + +// tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h +template inline +Jet tanh(const Jet& f) { + const T tanh_a = tanh(f.a); + const T tmp = T(1.0) - tanh_a * tanh_a; + return Jet(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 inline +bool IsFinite(const Jet& 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 inline +bool IsInfinite(const Jet& 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 inline +bool IsNaN(const Jet& 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 inline +bool IsNormal(const Jet& 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 inline +Jet atan2(const Jet& g, const Jet& 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(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 inline +Jet pow(const Jet& f, double g) { + T const tmp = g * pow(f.a, g - T(1.0)); + return Jet(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 inline +Jet pow(double f, const Jet& g) { + T const tmp = pow(f, g.a); + return Jet(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 inline +Jet pow(const Jet& f, const Jet& 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(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 ... > 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 inline const Jet& ei_conj(const Jet& x) { return x; } // NOLINT +template inline const Jet& ei_real(const Jet& x) { return x; } // NOLINT +template inline Jet ei_imag(const Jet& ) { return Jet(0.0); } // NOLINT +template inline Jet ei_abs (const Jet& x) { return fabs(x); } // NOLINT +template inline Jet ei_abs2(const Jet& x) { return x * x; } // NOLINT +template inline Jet ei_sqrt(const Jet& x) { return sqrt(x); } // NOLINT +template inline Jet ei_exp (const Jet& x) { return exp(x); } // NOLINT +template inline Jet ei_log (const Jet& x) { return log(x); } // NOLINT +template inline Jet ei_sin (const Jet& x) { return sin(x); } // NOLINT +template inline Jet ei_cos (const Jet& x) { return cos(x); } // NOLINT +template inline Jet ei_tan (const Jet& x) { return tan(x); } // NOLINT +template inline Jet ei_atan(const Jet& x) { return atan(x); } // NOLINT +template inline Jet ei_sinh(const Jet& x) { return sinh(x); } // NOLINT +template inline Jet ei_cosh(const Jet& x) { return cosh(x); } // NOLINT +template inline Jet ei_tanh(const Jet& x) { return tanh(x); } // NOLINT +template inline Jet ei_pow (const Jet& x, Jet 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 +inline std::ostream &operator<<(std::ostream &s, const Jet& 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 +struct NumTraits > { + typedef ceres::Jet Real; + typedef ceres::Jet NonInteger; + typedef ceres::Jet Nested; + + static typename ceres::Jet dummy_precision() { + return ceres::Jet(1e-12); + } + + static inline Real epsilon() { + return Real(std::numeric_limits::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_ diff --git a/gtsam_unstable/nonlinear/ceres_macros.h b/gtsam_unstable/nonlinear/ceres_macros.h new file mode 100644 index 000000000..1ed55be6e --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_macros.h @@ -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 // 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 +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 +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(!(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_ diff --git a/gtsam_unstable/nonlinear/ceres_manual_constructor.h b/gtsam_unstable/nonlinear/ceres_manual_constructor.h new file mode 100644 index 000000000..7ea723d2a --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_manual_constructor.h @@ -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 + +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 struct AlignType { }; +template 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 struct AlignType { \ + 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::result + +#endif // !defined(CERES_ALIGN_ATTRIBUTE) + +#endif // CERES_ALIGNED_CHAR_ARRAY + +template +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(space_); + } + inline const Type* get() const { + return reinterpret_cast(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(space_); + } + + // You can pass up to four constructor arguments as arguments of Init(). + inline void Init() { + new(space()) Type; + } + + template + inline void Init(const T1& p1) { + new(space()) Type(p1); + } + + template + inline void Init(const T1& p1, const T2& p2) { + new(space()) Type(p1, p2); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3) { + new(space()) Type(p1, p2, p3); + } + + template + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4) { + new(space()) Type(p1, p2, p3, p4); + } + + template + 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 + 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 + 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 + 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 + 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 + 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 + 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_ diff --git a/gtsam_unstable/nonlinear/ceres_rotation.h b/gtsam_unstable/nonlinear/ceres_rotation.h new file mode 100644 index 000000000..896761296 --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_rotation.h @@ -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 +#include + +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 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 +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 +MatrixAdapter ColumnMajorAdapter3x3(T* pointer); + +template +MatrixAdapter 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 +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 +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 +void RotationMatrixToAngleAxis(const T* R, T* angle_axis); + +template +void RotationMatrixToAngleAxis( + const MatrixAdapter& R, + T* angle_axis); + +template +void AngleAxisToRotationMatrix(const T* angle_axis, T* R); + +template +void AngleAxisToRotationMatrix( + const T* angle_axis, + const MatrixAdapter& 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 +void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R); + +template +void EulerAnglesToRotationMatrix( + const T* euler, + const MatrixAdapter& 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 inline +void QuaternionToScaledRotation(const T q[4], T R[3 * 3]); + +template inline +void QuaternionToScaledRotation( + const T q[4], + const MatrixAdapter& 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 inline +void QuaternionToRotation(const T q[4], T R[3 * 3]); + +template inline +void QuaternionToRotation( + const T q[4], + const MatrixAdapter& 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 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 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 inline +void QuaternionProduct(const T z[4], const T w[4], T zw[4]); + +// xy = x cross y; +template inline +void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]); + +template inline +T DotProduct(const T x[3], const T y[3]); + +// y = R(angle_axis) * x; +template inline +void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]); + +// --- IMPLEMENTATION + +template +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 +MatrixAdapter ColumnMajorAdapter3x3(T* pointer) { + return MatrixAdapter(pointer); +} + +template +MatrixAdapter RowMajorAdapter3x3(T* pointer) { + return MatrixAdapter(pointer); +} + +template +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 +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 +inline void RotationMatrixToAngleAxis(const T* R, T* angle_axis) { + RotationMatrixToAngleAxis(ColumnMajorAdapter3x3(R), angle_axis); +} + +template +void RotationMatrixToAngleAxis( + const MatrixAdapter& 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 +inline void AngleAxisToRotationMatrix(const T* angle_axis, T* R) { + AngleAxisToRotationMatrix(angle_axis, ColumnMajorAdapter3x3(R)); +} + +template +void AngleAxisToRotationMatrix( + const T* angle_axis, + const MatrixAdapter& R) { + static const T kOne = T(1.0); + const T theta2 = DotProduct(angle_axis, angle_axis); + if (theta2 > T(std::numeric_limits::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 +inline void EulerAnglesToRotationMatrix(const T* euler, + const int row_stride_parameter, + T* R) { + DCHECK(row_stride_parameter==3); + EulerAnglesToRotationMatrix(euler, RowMajorAdapter3x3(R)); +} + +template +void EulerAnglesToRotationMatrix( + const T* euler, + const MatrixAdapter& 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 inline +void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) { + QuaternionToScaledRotation(q, RowMajorAdapter3x3(R)); +} + +template inline +void QuaternionToScaledRotation( + const T q[4], + const MatrixAdapter& 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 inline +void QuaternionToRotation(const T q[4], T R[3 * 3]) { + QuaternionToRotation(q, RowMajorAdapter3x3(R)); +} + +template inline +void QuaternionToRotation(const T q[4], + const MatrixAdapter& 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 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 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 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 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 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 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::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_ diff --git a/gtsam_unstable/nonlinear/ceres_variadic_evaluate.h b/gtsam_unstable/nonlinear/ceres_variadic_evaluate.h new file mode 100644 index 000000000..7d22fe22e --- /dev/null +++ b/gtsam_unstable/nonlinear/ceres_variadic_evaluate.h @@ -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 + +#include +#include +#include + +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 +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 +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], + output); + } +}; + +template +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], + output); + } +}; + +template +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], + output); + } +}; + +template +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], + output); + } +}; + +template +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], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + output); + } +}; + +template +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + output); + } +}; + +template +struct VariadicEvaluate { + 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_ diff --git a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp new file mode 100644 index 000000000..053acdd34 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#undef CHECK +#include + +#include +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 Camera; + +/* ************************************************************************* */ +// Some Ceres Snippets copied for testing +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +template 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(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 + 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 + 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 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(Projective(), P, X); + Matrix E2 = numericalDerivative22(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::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( + SnavelyProjection(), P, X); + Matrix E2 = numericalDerivative22( + 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::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 +class AdaptAutoDiff { + + static const int N = traits::dimension::value; + static const int M1 = traits::dimension::value; + static const int M2 = traits::dimension::value; + + typedef Eigen::Matrix RowMajor1; + typedef Eigen::Matrix RowMajor2; + + typedef Canonical CanonicalT; + typedef Canonical Canonical1; + typedef Canonical 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 JacobianTA1; + typedef Eigen::Matrix JacobianTA2; + + T operator()(const A1& a1, const A2& a2, boost::optional H1 = + boost::none, boost::optional 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::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); + *H2 = Eigen::Map(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 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(Adaptor(), P, X); + Matrix E2 = numericalDerivative22(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 P(1); + Expression X(2); + typedef AdaptAutoDiff Adaptor; + Expression expression(Adaptor(), P, X); + set expected = list_of(1)(2); + EXPECT(expected == expression.keys()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index c66cc3b8b..1997bdb53 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -18,7 +18,6 @@ */ #include -#include #include #include #include diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index b2174f8a9..41f216b77 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -315,8 +315,9 @@ public: // Jacobian w.r.t. Vel1 if (H2){ - Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); - Matrix H2_Vel = numericalDerivative11(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(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Vel = numericalDerivative11(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(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); - Matrix H5_Vel = numericalDerivative11(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(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + Matrix H5_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); *H5 = stack(2, &H5_Pose, &H5_Vel); } diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 5ca736c01..76f6d02d5 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -242,8 +242,9 @@ public: // Jacobian w.r.t. Vel1 if (H2){ - Matrix H2_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); - Matrix H2_Vel = gtsam::numericalDerivative11(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(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Vel = gtsam::numericalDerivative11(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(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); - Matrix H5_Vel = gtsam::numericalDerivative11(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(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + Matrix H5_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); *H5 = stack(2, &H5_Pose, &H5_Vel); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index d61787358..d2a784b0f 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -104,11 +104,15 @@ public: boost::optional H1=boost::none, boost::optional H2=boost::none) const { - if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, _1, landmark), pose); + if (H1) { + (*H1) = numericalDerivative11( + boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, _1, + landmark), pose); } - if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, _1), landmark); + if (H2) { + (*H2) = numericalDerivative11( + boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, + _1), landmark); } return inverseDepthError(pose, landmark); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 9d4113431..3c95a5010 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -107,11 +107,15 @@ public: boost::optional H1=boost::none, boost::optional H2=boost::none) const { - if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, _1, landmark), pose); + if (H1) { + (*H1) = numericalDerivative11( + boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, _1, + landmark), pose); } - if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, _1), landmark); + if (H2) { + (*H2) = numericalDerivative11( + boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, + _1), landmark); } return inverseDepthError(pose, landmark); diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index aef15638f..2f94227dc 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -108,10 +108,10 @@ public: boost::optional H2=boost::none) const { if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose); + (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose); } if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark); + (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark); } return inverseDepthError(pose, landmark); @@ -228,13 +228,13 @@ public: boost::optional H3=boost::none) const { if(H1) { - (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1); + (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1); } if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2); + (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2); } if(H3) { - (*H3) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); + (*H3) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); } return inverseDepthError(pose1, pose2, landmark); diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 7756e79d3..57a19ee78 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -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& factor) { +Pose3 predictionErrorPose(const Pose3& p1, const LieVector& v1, + const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, + const InertialNavFactor_GlobalVelocity& 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& 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& 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(boost::bind(&predictionRq, _1, q), angles); +// J_expected = numericalDerivative11(boost::bind(&predictionRq, _1, q), angles); // // cout<<"J_hyp"<(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; + H1_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedPose = numericalDerivative11(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(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; + H1_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedVel = numericalDerivative11(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(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; + H1_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedPose = numericalDerivative11(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(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; + H1_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedVel = numericalDerivative11(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)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 24535854d..503a4b953 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -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(landmarkKey); - values.at(poseKey1).print("Pose1 Before:\n"); - result.at(poseKey1).print("Pose1 After:\n"); - pose1.print("Pose1 Truth:\n"); - std::cout << std::endl << std::endl; - values.at(poseKey2).print("Pose2 Before:\n"); - result.at(poseKey2).print("Pose2 After:\n"); - pose2.print("Pose2 Truth:\n"); - std::cout << std::endl << std::endl; - values.at(landmarkKey).print("Landmark Before:\n"); - result.at(landmarkKey).print("Landmark After:\n"); - expected.print("Landmark Truth:\n"); - std::cout << std::endl << std::endl; +// values.at(poseKey1).print("Pose1 Before:\n"); +// result.at(poseKey1).print("Pose1 After:\n"); +// pose1.print("Pose1 Truth:\n"); +// cout << endl << endl; +// values.at(poseKey2).print("Pose2 Before:\n"); +// result.at(poseKey2).print("Pose2 After:\n"); +// pose2.print("Pose2 Truth:\n"); +// cout << endl << endl; +// values.at(landmarkKey).print("Landmark Before:\n"); +// result.at(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; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index e99c9bcdf..49e5fc2aa 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -73,19 +73,18 @@ TEST( InvDepthFactorVariant2, optimize) { Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); LieVector actual = result.at(landmarkKey); - - values.at(poseKey1).print("Pose1 Before:\n"); - result.at(poseKey1).print("Pose1 After:\n"); - pose1.print("Pose1 Truth:\n"); - std::cout << std::endl << std::endl; - values.at(poseKey2).print("Pose2 Before:\n"); - result.at(poseKey2).print("Pose2 After:\n"); - pose2.print("Pose2 Truth:\n"); - std::cout << std::endl << std::endl; - values.at(landmarkKey).print("Landmark Before:\n"); - result.at(landmarkKey).print("Landmark After:\n"); - expected.print("Landmark Truth:\n"); - std::cout << std::endl << std::endl; +// values.at(poseKey1).print("Pose1 Before:\n"); +// result.at(poseKey1).print("Pose1 After:\n"); +// pose1.print("Pose1 Truth:\n"); +// std::cout << std::endl << std::endl; +// values.at(poseKey2).print("Pose2 Before:\n"); +// result.at(poseKey2).print("Pose2 After:\n"); +// pose2.print("Pose2 Truth:\n"); +// std::cout << std::endl << std::endl; +// values.at(landmarkKey).print("Landmark Before:\n"); +// result.at(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)); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index e65b7cacb..11a91f687 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -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(); diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp index bf9dc0e8e..d0af4af62 100644 --- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -176,8 +176,8 @@ TEST( PoseBetweenFactor, Jacobian ) { Point3(-3.37493895, 6.14660244, -8.93650986)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); + Matrix expectedH2 = numericalDerivative11(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(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); + Matrix expectedH2 = numericalDerivative11(boost::bind(&TestPoseBetweenFactor::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); // Use the factor to calculate the derivative Matrix actualH1; diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp index cbfa45431..a01cfedba 100644 --- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -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(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); + Matrix expectedH1 = numericalDerivative11(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(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); + Matrix expectedH1 = numericalDerivative11(boost::bind(&TestPosePriorFactor::evaluateError, &factor, _1, boost::none), pose); // Use the factor to calculate the derivative Matrix actualH1; diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp index 3a7bf0cd0..aacca18ea 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp @@ -170,7 +170,7 @@ TEST( ProjectionFactor, Jacobian ) { CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); // Verify H2 with numerical derivative - Matrix H2Expected = numericalDerivative32( + Matrix H2Expected = numericalDerivative32( boost::function( 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( + Matrix H2Expected = numericalDerivative32( boost::function( boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3, boost::none, boost::none, boost::none)), pose, body_P_sensor, point); diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp index 5b572fb69..20490a8e4 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp @@ -136,11 +136,11 @@ TEST( ProjectionFactor, Jacobian ) { CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); // Verify H2 and H4 with numerical derivatives - Matrix H2Expected = numericalDerivative11( + Matrix H2Expected = numericalDerivative11( boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point, *K1, boost::none, boost::none, boost::none, boost::none), Pose3()); - Matrix H4Expected = numericalDerivative11( + Matrix H4Expected = numericalDerivative11( 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( + Matrix H2Expected = numericalDerivative11( boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point, *K1, boost::none, boost::none, boost::none, boost::none), body_P_sensor); - Matrix H4Expected = numericalDerivative11( + Matrix H4Expected = numericalDerivative11( boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point, _1, boost::none, boost::none, boost::none, boost::none), *K1); diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp index ffc7344cf..0d5cb2e36 100644 --- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp @@ -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( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( 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( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( 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( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( 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( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( 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( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( 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( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( 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( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp index 44dca9b8e..a8a3ae5e9 100644 --- a/gtsam_unstable/slam/tests/testTSAMFactors.cpp +++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp @@ -44,10 +44,10 @@ TEST( DeltaFactor, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11( + H1Expected = numericalDerivative11( boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none, boost::none), pose); - H2Expected = numericalDerivative11( + H2Expected = numericalDerivative11( 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( + H1Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2, point, boost::none, boost::none, boost::none, boost::none), base1); - H2Expected = numericalDerivative11( + H2Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2, point, boost::none, boost::none, boost::none, boost::none), pose); - H3Expected = numericalDerivative11( + H3Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1, point, boost::none, boost::none, boost::none, boost::none), base2); - H4Expected = numericalDerivative11( + H4Expected = numericalDerivative11( 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( + H1Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2, pose2, boost::none, boost::none, boost::none, boost::none), base1); - H2Expected = numericalDerivative11( + H2Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2, pose2, boost::none, boost::none, boost::none, boost::none), pose1); - H3Expected = numericalDerivative11( + H3Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1, pose2, boost::none, boost::none, boost::none, boost::none), base2); - H4Expected = numericalDerivative11( + H4Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, base2, _1, boost::none, boost::none, boost::none, boost::none), pose2); diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp new file mode 100644 index 000000000..2c3b20434 --- /dev/null +++ b/tests/testManifold.cpp @@ -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 +#include +#include +#include +#include +#include +#include + +#undef CHECK +#include + +#include +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 Camera; + +/* ************************************************************************* */ +// is_manifold +TEST(Manifold, _is_manifold) { + using namespace traits; + EXPECT(!is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); + EXPECT(is_manifold::value); +} + +/* ************************************************************************* */ +// dimension +TEST(Manifold, _dimension) { + using namespace traits; + EXPECT_LONGS_EQUAL(2, dimension::value); + EXPECT_LONGS_EQUAL(8, dimension::value); + EXPECT_LONGS_EQUAL(1, dimension::value); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); + EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); +} + +/* ************************************************************************* */ +// charts +TEST(Manifold, DefaultChart) { + + DefaultChart 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 chart2(Vector2(0, 0)); + EXPECT(assert_equal(v2,chart2.apply(Vector2(1,0)))); + EXPECT(chart2.retract(v2)==Vector2(1,0)); + + DefaultChart 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 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 chart5(I); + EXPECT(assert_equal(v3,chart5.apply(R))); + EXPECT(assert_equal(chart5.retract(v3),R)); + // Check zero vector + DefaultChart chart6(R); + EXPECT(assert_equal(zero(3),chart6.apply(R))); +} + +/* ************************************************************************* */ +// zero +TEST(Manifold, _zero) { + EXPECT(assert_equal(Pose3(),traits::zero::value())); + Cal3Bundler cal(0, 0, 0); + EXPECT(assert_equal(cal,traits::zero::value())); + EXPECT(assert_equal(Camera(Pose3(),cal),traits::zero::value())); +} + +/* ************************************************************************* */ +// charts +TEST(Manifold, Canonical) { + + Canonical 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 chart2; + EXPECT(assert_equal(v2,chart2.apply(Vector2(1,0)))); + EXPECT(chart2.retract(v2)==Vector2(1,0)); + + Canonical chart3; + Eigen::Matrix v1; + v1 << 1; + EXPECT(chart3.apply(1)==v1); + EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9); + + Canonical 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 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 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::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); +} +/* ************************************************************************* */ +