From 959716ae92d834104722ee23d75fd41a1e13d052 Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Sat, 13 Dec 2014 12:47:52 +0100 Subject: [PATCH] A lot more progress making things work. Still a long way to go. --- gtsam/base/ChartValue.h | 9 ++-- gtsam/base/LieScalar.h | 3 +- gtsam/base/Manifold.h | 9 ++-- gtsam/base/OptionalJacobian.h | 4 ++ gtsam/base/concepts.h | 2 +- gtsam/base/numericalDerivative.h | 4 +- gtsam/geometry/Quaternion.h | 17 +++++++- gtsam/geometry/Rot3.h | 1 + gtsam/geometry/tests/testQuaternion.cpp | 4 +- gtsam/nonlinear/Expression-inl.h | 58 +++---------------------- gtsam/nonlinear/Expression.h | 4 +- 11 files changed, 45 insertions(+), 70 deletions(-) diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h index 3cc6a041c..a63b51f29 100644 --- a/gtsam/base/ChartValue.h +++ b/gtsam/base/ChartValue.h @@ -19,6 +19,7 @@ #pragma once +#include #include #include #include @@ -209,11 +210,11 @@ const Chart& Value::getChart() const { template ChartValue convertToChartValue(const T& value, boost::optional< - Eigen::Matrix::value, - traits::dimension::value>&> H = boost::none) { + Eigen::Matrix::dimension, + traits_x::dimension>&> H = boost::none) { if (H) { - *H = Eigen::Matrix::value, - traits::dimension::value>::Identity(); + *H = Eigen::Matrix::dimension, + traits_x::dimension>::Identity(); } return ChartValue(value); } diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index c68458364..33e5914a1 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -17,7 +17,8 @@ #pragma once -#warning "LieScalar.h is deprecated. Please use double/float instead." +// TODO(ASL) reenable +//#warning "LieScalar.h is deprecated. Please use double/float instead." #include #include #include diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index c5554fd1a..783605b33 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include #include @@ -69,10 +70,10 @@ struct is_manifold: public boost::false_type { // dimension, can return Eigen::Dynamic (-1) if not known at compile time // defaults to dynamic, TODO makes sense ? typedef boost::integral_constant Dynamic; -template +//template //struct dimension: public Dynamic { -struct dimension : public boost::integral_constant { // just temporary fix to minimize compiler errors while refactoring -}; +//struct dimension : public boost::integral_constant { // just temporary fix to minimize compiler errors while refactoring +//}; /** * zero::value is intended to be the origin of a canonical coordinate system @@ -147,7 +148,7 @@ template struct DefaultChart { //BOOST_STATIC_ASSERT(traits::is_manifold::value); typedef T type; - typedef Eigen::Matrix::value, 1> vector; + typedef Eigen::Matrix::dimension, 1> vector; static vector local(const T& origin, const T& other) { return origin.localCoordinates(other); diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 5651816ba..bae1fb3d6 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -73,6 +73,7 @@ public: } /// Constructor that will resize a dynamic matrix (unless already correct) + OptionalJacobian(Eigen::MatrixXd& dynamic) : map_(NULL) { dynamic.resize(Rows, Cols); // no malloc if correct size @@ -111,5 +112,8 @@ public: Eigen::Map* operator->(){ return &map_; } }; +template<> +class OptionalJacobian<-1, -1> {}; + } // namespace gtsam diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 6d5a5d74d..8dac2d55d 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -38,7 +38,7 @@ struct additive_group_tag {}; // TODO: Remove namespace traits { template -struct dimension; +struct dimension{}; } template struct traits_x { // TODO: remove anything in here ASAP. diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 188647525..efd357d73 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -71,7 +71,7 @@ Vector numericalGradient(boost::function h, const X& x, BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, "Template argument X must be a manifold type."); - static const int N = traits::dimension::value; + static const int N = traits_x::dimension; BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); typedef DefaultChart ChartX; typedef typename ChartX::vector TangentX; @@ -118,7 +118,7 @@ Matrix numericalDerivative11(boost::function h, const X& x, BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); - static const int N = traits::dimension::value; + static const int N = traits_x::dimension; BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); typedef DefaultChart ChartX; typedef typename ChartX::vector TangentX; diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index c0d05bf6a..5b3d2dfa3 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -111,14 +111,27 @@ struct traits_x { } } - static TangentVector Local(const Q& origin, const Q& other, ChartJacobian Horigin=boost::none, ChartJacobian Hother=boost::none) { + static TangentVector Local(const Q& origin, const Q& other, ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { return Logmap(Between(origin,other,Horigin,Hother)); // TODO: incorporate Jacobian of Logmap } - static Q Retract(const Q& origin, const TangentVector& v, ChartJacobian Horigin, ChartJacobian Hv) { + static Q Retract(const Q& origin, const TangentVector& v, ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) { return Compose(origin, Expmap(v), Horigin, Hv); // TODO : incorporate Jacobian of Expmap } + + static void Print(const Q& q, const std::string& str = "") { + if(str.size() == 0) { + std::cout << "Eigen::Quaternion: "; + } else { + std::cout << str << " "; + } + std::cout << q.vec().transpose() << std::endl; + } + + static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) { + return Between(q1,q2).vec().array().abs().maxCoeff() < tol; + } }; typedef Eigen::Quaternion Quaternion; diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 618e8fd0c..b043527d6 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -475,3 +475,4 @@ namespace gtsam { template<> struct traits_x : public internal::LieGroup {}; } + diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 3d96e342e..19a0338fd 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -50,7 +50,7 @@ TEST(Quaternion , Local) { Q q2(Eigen::AngleAxisd(0.1, z_axis)); QuaternionJacobian H1,H2; Vector3 expected(0, 0, 0.1); - Vector3 actual = traits::Local(q1, q2, H1, H2); + Vector3 actual = traits_x::Local(q1, q2, H1, H2); EXPECT(assert_equal((Vector)expected,actual)); } @@ -61,7 +61,7 @@ TEST(Quaternion , Retract) { Q expected(Eigen::AngleAxisd(0.1, z_axis)); Vector3 v(0, 0, 0.1); QuaternionJacobian Hq,Hv; - Q actual = traits::Retract(q, v, Hq, Hv); + Q actual = traits_x::Retract(q, v, Hq, Hv); EXPECT(actual.isApprox(expected)); } diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index b91bbd478..88a92d181 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -300,57 +300,11 @@ public: } }; -//----------------------------------------------------------------------------- -/// Leaf Expression -template > -class LeafExpression: public ExpressionNode { - typedef ChartValue value_type; // perhaps this can be something else like a std::pair ?? - - /// The key into values - Key key_; - - /// Constructor with a single key - LeafExpression(Key key) : - key_(key) { - } - // todo: do we need a virtual destructor here too? - - friend class Expression ; - -public: - - /// Return keys that play in this expression - virtual std::set keys() const { - std::set keys; - keys.insert(key_); - return keys; - } - - /// Return dimensions for each argument - virtual void dims(std::map& map) const { - // get dimension from the chart; only works for fixed dimension charts - map[key_] = traits::dimension::value; - } - - /// Return value - virtual const value_type& value(const Values& values) const { - return dynamic_cast(values.at(key_)); - } - - /// Construct an execution trace for reverse AD - virtual const value_type& traceExecution(const Values& values, - ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - trace.setLeaf(key_); - return dynamic_cast(values.at(key_)); - } - -}; //----------------------------------------------------------------------------- /// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value template -class LeafExpression > : public ExpressionNode { +class LeafExpression : public ExpressionNode { typedef T value_type; /// The key into values @@ -375,7 +329,7 @@ public: /// Return dimensions for each argument virtual void dims(std::map& map) const { - map[key_] = traits::dimension::value; + map[key_] = traits_x::dimension; } /// Return value @@ -451,15 +405,15 @@ public: /// meta-function to generate fixed-size JacobianTA type template struct Jacobian { - typedef Eigen::Matrix::value, - traits::dimension::value> type; + typedef Eigen::Matrix::dimension, + traits_x::dimension> type; }; /// meta-function to generate JacobianTA optional reference template struct MakeOptionalJacobian { - typedef OptionalJacobian::value, - traits::dimension::value> type; + typedef OptionalJacobian::dimension, + traits_x::dimension> type; }; /** diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 498e865b9..a755ab838 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -173,7 +173,7 @@ private: assert(H.size()==keys.size()); // Pre-allocate and zero VerticalBlockMatrix - static const int Dim = traits::dimension::value; + static const int Dim = traits_x::dimension; VerticalBlockMatrix Ab(dims, Dim); Ab.matrix().setZero(); JacobianMap jacobianMap(keys, Ab); @@ -223,7 +223,7 @@ private: template struct apply_compose { typedef T result_type; - static const int Dim = traits::dimension::value; + static const int Dim = traits_x::dimension; T operator()(const T& x, const T& y, OptionalJacobian H1 = boost::none, OptionalJacobian H2 = boost::none) const { return x.compose(y, H1, H2);