diff --git a/.cproject b/.cproject index b3709d422..1dcc51dfe 100644 --- a/.cproject +++ b/.cproject @@ -592,6 +592,7 @@ make + tests/testBayesTree.run true false @@ -599,6 +600,7 @@ make + testBinaryBayesNet.run true false @@ -646,6 +648,7 @@ make + testSymbolicBayesNet.run true false @@ -653,6 +656,7 @@ make + tests/testSymbolicFactor.run true false @@ -660,6 +664,7 @@ make + testSymbolicFactorGraph.run true false @@ -675,6 +680,7 @@ make + tests/testBayesTree true false @@ -1122,6 +1128,7 @@ make + testErrors.run true false @@ -1351,6 +1358,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 @@ -1433,7 +1480,6 @@ make - testSimulated2DOriented.run true false @@ -1473,7 +1519,6 @@ make - testSimulated2D.run true false @@ -1481,7 +1526,6 @@ make - testSimulated3D.run true false @@ -1495,46 +1539,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 @@ -1792,6 +1796,7 @@ cpack + -G DEB true false @@ -1799,6 +1804,7 @@ cpack + -G RPM true false @@ -1806,6 +1812,7 @@ cpack + -G TGZ true false @@ -1813,6 +1820,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2627,6 +2635,7 @@ make + testGraph.run true false @@ -2634,6 +2643,7 @@ make + testJunctionTree.run true false @@ -2641,6 +2651,7 @@ make + testSymbolicBayesNetB.run true false @@ -2758,6 +2769,14 @@ true true + + make + -j4 + testBasisDecompositions.run + true + true + true + make -j2 @@ -3168,7 +3187,6 @@ make - tests/testGaussianISAM2 true false diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 0839e8fb5..a3a5b029b 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -235,7 +235,7 @@ struct DefaultChart { static double retract(double origin, const vector& d) { return origin + d[0]; } - static const int getDimension(double) { + static int getDimension(double) { return 1; } }; diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 42ecd8c74..ce56c78c1 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -111,6 +111,18 @@ double Point3::norm(boost::optional H) const { return r; } +/* ************************************************************************* */ +double Point3::norm(boost::optional&> H) const { + double r = norm(); + if (H) { + if (fabs(r) > 1e-10) + *H << x_ / r, y_ / r, z_ / r; + else + *H << 1, 1, 1; // really infinity, why 1 ? + } + return r; +} + /* ************************************************************************* */ Point3 Point3::normalize(boost::optional H) const { Point3 normalized = *this / norm(); diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index d2dc8d892..b7f7f8ffa 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -186,6 +186,9 @@ namespace gtsam { /** Distance of the point from the origin, with Jacobian */ double norm(boost::optional H) const; + /** Distance of the point from the origin, with Jacobian */ + double norm(boost::optional&> H) const; + /** normalize, with optional Jacobian */ Point3 normalize(boost::optional H = boost::none) const; diff --git a/gtsam_unstable/nonlinear/CallRecord.h b/gtsam_unstable/nonlinear/CallRecord.h index 3806f1803..f1ac0b044 100644 --- a/gtsam_unstable/nonlinear/CallRecord.h +++ b/gtsam_unstable/nonlinear/CallRecord.h @@ -46,7 +46,7 @@ namespace internal { * it just passes dense Eigen matrices through. */ template -struct ConvertToDynamicRowsIf { +struct ConvertToVirtualFunctionSupportedMatrixType { template static Eigen::Matrix convert( const Eigen::MatrixBase & x) { @@ -55,7 +55,13 @@ struct ConvertToDynamicRowsIf { }; template<> -struct ConvertToDynamicRowsIf { +struct ConvertToVirtualFunctionSupportedMatrixType { + template + static const Eigen::Matrix convert( + const Eigen::MatrixBase & x) { + return x; + } + // special treatment of matrices that don't need conversion template static const Eigen::Matrix & convert( const Eigen::Matrix & x) { @@ -143,11 +149,11 @@ struct CallRecord: virtual private internal::ReverseADInterface< _startReverseAD(jacobians); } - template - inline void reverseAD(const Eigen::Matrix & dFdT, + template + inline void reverseAD(const Eigen::MatrixBase & dFdT, JacobianMap& jacobians) const { _reverseAD( - internal::ConvertToDynamicRowsIf<(Rows > MaxVirtualStaticRows)>::convert( + internal::ConvertToVirtualFunctionSupportedMatrixType<(Derived::RowsAtCompileTime > MaxVirtualStaticRows)>::convert( dFdT), jacobians); } diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index a98ab349f..7f59dcb7d 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -64,18 +64,36 @@ public: }; //----------------------------------------------------------------------------- -/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians -template -void handleLeafCase(const Eigen::Matrix& dTdA, - JacobianMap& jacobians, Key key) { - jacobians(key).block(0, 0) += dTdA; // block makes HUGE difference + +namespace internal { + +template +struct UseBlockIf { + static void addToJacobian(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key){ + // block makes HUGE difference + jacobians(key).block(0, 0) += dTdA; + }; +}; +/// Handle Leaf Case for Dynamic Matrix type (slower) +template +struct UseBlockIf { + static void addToJacobian(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + jacobians(key) += dTdA; + } +}; } -/// Handle Leaf Case for Dynamic ROWS Matrix type (slower) -template -inline void handleLeafCase( - const Eigen::Matrix& dTdA, + +/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians +template +void handleLeafCase(const Eigen::MatrixBase& dTdA, JacobianMap& jacobians, Key key) { - jacobians(key) += dTdA; + internal::UseBlockIf< + Derived::RowsAtCompileTime != Eigen::Dynamic && + Derived::ColsAtCompileTime != Eigen::Dynamic, + Derived> + ::addToJacobian(dTdA, jacobians, key); } //----------------------------------------------------------------------------- @@ -166,9 +184,9 @@ public: void reverseAD(const Eigen::MatrixBase & dTdA, JacobianMap& jacobians) const { if (kind == Leaf) - handleLeafCase(dTdA.eval(), jacobians, content.key); + handleLeafCase(dTdA, jacobians, content.key); else if (kind == Function) - content.ptr->reverseAD(dTdA.eval(), jacobians); + content.ptr->reverseAD(dTdA, jacobians); } /// Define type so we can apply it as a meta-function diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 37a89af6b..f609071cb 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -81,6 +81,8 @@ public: */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { + // TODO(PTF) Is this a place for custom charts? + DefaultChart chart; if (H) { // H should be pre-allocated assert(H->size()==size()); @@ -95,18 +97,19 @@ public: T value = expression_.value(x, map); // <<< Reverse AD happens here ! // Copy blocks into the vector of jacobians passed in - for (DenseIndex i = 0; i < size(); i++) + for (DenseIndex i = 0; i < static_cast(size()); i++) H->at(i) = Ab(i); - return measurement_.localCoordinates(value); + return chart.local(measurement_, value); } else { const T& value = expression_.value(x); - return measurement_.localCoordinates(value); + return chart.local(measurement_, value); } } virtual boost::shared_ptr linearize(const Values& x) const { - + // TODO(PTF) Is this a place for custom charts? + DefaultChart chart; // Only linearize if the factor is active if (!active(x)) return boost::shared_ptr(); @@ -128,7 +131,7 @@ public: // Evaluate error to get Jacobians and RHS vector b T value = expression_.value(x, map); // <<< Reverse AD happens here ! - Ab(size()).col(0) = -measurement_.localCoordinates(value); + Ab(size()).col(0) = -chart.local(measurement_, value); // Whiten the corresponding system, Ab already contains RHS Vector dummy(Dim); diff --git a/gtsam_unstable/nonlinear/expressions.h b/gtsam_unstable/nonlinear/expressions.h new file mode 100644 index 000000000..eba85c33c --- /dev/null +++ b/gtsam_unstable/nonlinear/expressions.h @@ -0,0 +1,26 @@ +/** + * @file expressions.h + * @brief Common expressions, both linear and non-linear + * @date Nov 23, 2014 + * @author Frank Dellaert + */ + +#pragma once + +#include +#include + +namespace gtsam { + +// Generics + +template +Expression between(const Expression& t1, const Expression& t2) { + return Expression(t1, &T::between, t2); +} + +typedef Expression double_; +typedef Expression Vector3_; + +} // \namespace gtsam + diff --git a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp new file mode 100644 index 000000000..f113a4f64 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp @@ -0,0 +1,119 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testBasisDecompositions.cpp + * @date November 23, 2014 + * @author Frank Dellaert + * @brief unit tests for Basis Decompositions w Expressions + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include +using boost::assign::list_of; + +using namespace std; +using namespace gtsam; + +noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); + +/// Fourier +template +class Fourier { + +public: + + typedef Eigen::Matrix Coefficients; + typedef Eigen::Matrix Jacobian; + +private: + + double x_; + Jacobian H_; + +public: + + /// Constructor + Fourier(double x) : + x_(x) { + H_(0, 0) = 1; + for (size_t i = 1; i < N; i += 2) { + H_(0, i) = cos(i * x); + H_(0, i + 1) = sin(i * x); + } + } + + /// Given coefficients c, predict value for x + double operator()(const Coefficients& c, boost::optional H) { + if (H) + (*H) = H_; + return H_ * c; + } +}; + +//****************************************************************************** +TEST(BasisDecompositions, Fourier) { + Fourier<3> fx(0); + Eigen::Matrix expectedH, actualH; + Vector3 c(1.5661, 1.2717, 1.2717); + expectedH = numericalDerivative11( + boost::bind(&Fourier<3>::operator(), fx, _1, boost::none), c); + EXPECT_DOUBLES_EQUAL(c[0]+c[1], fx(c,actualH), 1e-9); + EXPECT(assert_equal((Matrix)expectedH, actualH)); +} + +//****************************************************************************** +TEST(BasisDecompositions, FourierExpression) { + + // Create linear factor graph + GaussianFactorGraph g; + Key key(1); + Vector3_ c(key); + for (size_t i = 0; i < 16; i++) { + double x = i * M_PI / 8, y = exp(sin(x) + cos(x)); + + // Manual JacobianFactor + Matrix A(1, 3); + A << 1, cos(x), sin(x); + Vector b(1); + b << y; + JacobianFactor f1(key, A, b, model); + + // With ExpressionFactor + Expression expression(Fourier<3>(x), c); + ExpressionFactor f2(model, y, expression); + + g.add(f1); + } + + // Solve + VectorValues actual = g.optimize(); + + // Check + Vector3 expected(1.5661, 1.2717, 1.2717); + EXPECT(assert_equal((Vector) expected, actual.at(key),1e-4)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index d660d28b5..4e032b9f2 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -73,27 +73,84 @@ TEST(Expression, Leaves) { } /* ************************************************************************* */ +// Unary(Leaf) +namespace unary { +Point2 f0(const Point3& p, boost::optional H) { + return Point2(); +} +LieScalar f1(const Point3& p, boost::optional&> H) { + return LieScalar(0.0); +} +double f2(const Point3& p, boost::optional&> H) { + return 0.0; +} +Expression p(1); +set expected = list_of(1); +} +TEST(Expression, Unary0) { + using namespace unary; + Expression e(f0, p); + EXPECT(expected == e.keys()); +} +TEST(Expression, Unary1) { + using namespace unary; + Expression e(f1, p); + EXPECT(expected == e.keys()); +} +TEST(Expression, Unary2) { + using namespace unary; + Expression e(f2, p); + EXPECT(expected == e.keys()); +} +/* ************************************************************************* */ +//Nullary Method +TEST(Expression, NullaryMethod) { -//TEST(Expression, NullaryMethod) { -// Expression p(67); -// Expression norm(p, &Point3::norm); -// Values values; -// values.insert(67,Point3(3,4,5)); -// Augmented a = norm.augmented(values); -// EXPECT(a.value() == sqrt(50)); -// JacobianMap expected; -// expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50)); -// EXPECT(assert_equal(expected.at(67),a.jacobians().at(67))); -//} + // Create expression + Expression p(67); + Expression norm(p, &Point3::norm); + + // Create Values + Values values; + values.insert(67, Point3(3, 4, 5)); + + // Pre-allocate JacobianMap + FastVector keys; + keys.push_back(67); + FastVector dims; + dims.push_back(3); + VerticalBlockMatrix Ab(dims, 1); + JacobianMap map(keys, Ab); + + // Get value and Jacobian + double actual = norm.value(values, map); + + // Check all + EXPECT(actual == sqrt(50)); + Matrix expected(1, 3); + expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0); + EXPECT(assert_equal(expected,Ab(0))); +} /* ************************************************************************* */ // Binary(Leaf,Leaf) namespace binary { // Create leaves +double doubleF(const Pose3& pose, const Point3& point, + boost::optional&> H1, + boost::optional&> H2) { + return 0.0; +} Expression x(1); Expression p(2); Expression p_cam(x, &Pose3::transform_to, p); } /* ************************************************************************* */ +// Check that creating an expression to double compiles +TEST(Expression, BinaryToDouble) { + using namespace binary; + Expression p_cam(doubleF, x, p); +} +/* ************************************************************************* */ // keys TEST(Expression, BinaryKeys) { set expected = list_of(1)(2); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index e609af953..1e992d26e 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -22,8 +22,6 @@ #include #include #include -#include -#include #include #include diff --git a/gtsam_unstable/slam/expressions.h b/gtsam_unstable/slam/expressions.h index d9cbd5716..4a63a7de0 100644 --- a/gtsam_unstable/slam/expressions.h +++ b/gtsam_unstable/slam/expressions.h @@ -7,20 +7,12 @@ #pragma once -#include +#include #include #include -#include namespace gtsam { -// Generics - -template -Expression between(const Expression& t1, const Expression& t2) { - return Expression(t1, &T::between, t2); -} - // 2D Geometry typedef Expression Point2_;