commit
						3b28dcab6a
					
				
							
								
								
									
										6
									
								
								gtsam.h
								
								
								
								
							
							
						
						
									
										6
									
								
								gtsam.h
								
								
								
								
							|  | @ -156,7 +156,7 @@ virtual class Value { | |||
|   size_t dim() const; | ||||
| }; | ||||
| 
 | ||||
| #include <gtsam/base/LieScalar.h> | ||||
| #include <gtsam/base/LieScalar_Deprecated.h> | ||||
| class LieScalar { | ||||
|   // Standard constructors
 | ||||
|   LieScalar(); | ||||
|  | @ -185,7 +185,7 @@ class LieScalar { | |||
|   static Vector Logmap(const gtsam::LieScalar& p); | ||||
| }; | ||||
| 
 | ||||
| #include <gtsam/base/LieVector.h> | ||||
| #include <gtsam/base/LieVector_Deprecated.h> | ||||
| class LieVector { | ||||
|   // Standard constructors
 | ||||
|   LieVector(); | ||||
|  | @ -217,7 +217,7 @@ class LieVector { | |||
|   void serialize() const; | ||||
| }; | ||||
| 
 | ||||
| #include <gtsam/base/LieMatrix.h> | ||||
| #include <gtsam/base/LieMatrix_Deprecated.h> | ||||
| class LieMatrix { | ||||
|   // Standard constructors
 | ||||
|   LieMatrix(); | ||||
|  |  | |||
|  | @ -15,7 +15,7 @@ | |||
|  * @author Richard Roberts and Alex Cunningham | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/base/LieMatrix.h> | ||||
| #include <gtsam/base/LieMatrix_Deprecated.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
|  |  | |||
|  | @ -11,147 +11,16 @@ | |||
| 
 | ||||
| /**
 | ||||
|  * @file LieMatrix.h | ||||
|  * @brief A wrapper around Matrix providing Lie compatibility | ||||
|  * @author Richard Roberts and Alex Cunningham | ||||
|  * @brief External deprecation warning, see LieMatrix_Deprecated.h for details | ||||
|  * @author Paul Drews | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <cstdarg> | ||||
| 
 | ||||
| #ifdef _MSC_VER | ||||
| #pragma message("LieMatrix.h is deprecated. Please use Eigen::Matrix instead.") | ||||
| #else | ||||
| #warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead." | ||||
| #endif | ||||
| 
 | ||||
| #include <gtsam/base/VectorSpace.h> | ||||
| #include <boost/serialization/nvp.hpp> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /**
 | ||||
|  * @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as | ||||
|  * we can directly add double, Vector, and Matrix into values now, because of | ||||
|  * gtsam::traits. | ||||
|  */ | ||||
| struct LieMatrix : public Matrix { | ||||
| 
 | ||||
|   /// @name Constructors
 | ||||
|   /// @{
 | ||||
|   enum { dimension = Eigen::Dynamic }; | ||||
| 
 | ||||
|   /** default constructor - only for serialize */ | ||||
|   LieMatrix() {} | ||||
| 
 | ||||
|   /** initialize from a normal matrix */ | ||||
|   LieMatrix(const Matrix& v) : Matrix(v) {} | ||||
| 
 | ||||
|   template <class M> | ||||
|   LieMatrix(const M& v) : Matrix(v) {} | ||||
| 
 | ||||
| // Currently TMP constructor causes ICE on MSVS 2013
 | ||||
| #if (_MSC_VER < 1800) | ||||
|   /** initialize from a fixed size normal vector */ | ||||
|   template<int M, int N> | ||||
|   LieMatrix(const Eigen::Matrix<double, M, N>& v) : Matrix(v) {} | ||||
| #endif | ||||
| 
 | ||||
|   /** constructor with size and initial data, row order ! */ | ||||
|   LieMatrix(size_t m, size_t n, const double* const data) : | ||||
|       Matrix(Eigen::Map<const Matrix>(data, m, n)) {} | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Testable interface
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /** print @param s optional string naming the object */ | ||||
|   GTSAM_EXPORT void print(const std::string& name="") const; | ||||
| 
 | ||||
|   /** equality up to tolerance */ | ||||
|   inline bool equals(const LieMatrix& expected, double tol=1e-5) const { | ||||
|     return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); | ||||
|   } | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Standard Interface
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /** get the underlying matrix */ | ||||
|   inline Matrix matrix() const { | ||||
|     return static_cast<Matrix>(*this); | ||||
|   } | ||||
| 
 | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// @name Group
 | ||||
|   /// @{
 | ||||
|   LieMatrix compose(const LieMatrix& q) { return (*this)+q;} | ||||
|   LieMatrix between(const LieMatrix& q) { return q-(*this);} | ||||
|   LieMatrix inverse() { return -(*this);} | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// @name Manifold
 | ||||
|   /// @{
 | ||||
|   Vector localCoordinates(const LieMatrix& q) { return between(q).vector();} | ||||
|   LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));} | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// @name Lie Group
 | ||||
|   /// @{
 | ||||
|   static Vector Logmap(const LieMatrix& p) {return p.vector();} | ||||
|   static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);} | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// @name VectorSpace requirements
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /** Returns dimensionality of the tangent space */ | ||||
|   inline size_t dim() const { return size(); } | ||||
| 
 | ||||
|   /** Convert to vector, is done row-wise - TODO why? */ | ||||
|   inline Vector vector() const { | ||||
|     Vector result(size()); | ||||
|     typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, | ||||
|         Eigen::RowMajor> RowMajor; | ||||
|     Eigen::Map<RowMajor>(&result(0), rows(), cols()) = *this; | ||||
|     return result; | ||||
|   } | ||||
| 
 | ||||
|   /** identity - NOTE: no known size at compile time - so zero length */ | ||||
|   inline static LieMatrix identity() { | ||||
|     throw std::runtime_error("LieMatrix::identity(): Don't use this function"); | ||||
|     return LieMatrix(); | ||||
|   } | ||||
|   /// @}
 | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|   // Serialization function
 | ||||
|   friend class boost::serialization::access; | ||||
|   template<class Archive> | ||||
|   void serialize(Archive & ar, const unsigned int version) { | ||||
|     ar & boost::serialization::make_nvp("Matrix", | ||||
|        boost::serialization::base_object<Matrix>(*this)); | ||||
| 
 | ||||
|   } | ||||
| 
 | ||||
| }; | ||||
| 
 | ||||
| 
 | ||||
| template<> | ||||
| struct traits<LieMatrix> : public internal::VectorSpace<LieMatrix> { | ||||
| 
 | ||||
|   // Override Retract, as the default version does not know how to initialize
 | ||||
|   static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v, | ||||
|       ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { | ||||
|     if (H1) *H1 = Eye(origin); | ||||
|     if (H2) *H2 = Eye(origin); | ||||
|     typedef const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, | ||||
|         Eigen::RowMajor> RowMajor; | ||||
|     return origin + Eigen::Map<RowMajor>(&v(0), origin.rows(), origin.cols()); | ||||
|   } | ||||
| 
 | ||||
| }; | ||||
| 
 | ||||
| } // \namespace gtsam
 | ||||
| #include "gtsam/base/LieMatrix_Deprecated.h" | ||||
|  |  | |||
|  | @ -0,0 +1,151 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * 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 LieMatrix.h | ||||
|  * @brief A wrapper around Matrix providing Lie compatibility | ||||
|  * @author Richard Roberts and Alex Cunningham | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <cstdarg> | ||||
| 
 | ||||
| #include <gtsam/base/VectorSpace.h> | ||||
| #include <boost/serialization/nvp.hpp> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /**
 | ||||
|  * @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as | ||||
|  * we can directly add double, Vector, and Matrix into values now, because of | ||||
|  * gtsam::traits. | ||||
|  */ | ||||
| struct LieMatrix : public Matrix { | ||||
| 
 | ||||
|   /// @name Constructors
 | ||||
|   /// @{
 | ||||
|   enum { dimension = Eigen::Dynamic }; | ||||
| 
 | ||||
|   /** default constructor - only for serialize */ | ||||
|   LieMatrix() {} | ||||
| 
 | ||||
|   /** initialize from a normal matrix */ | ||||
|   LieMatrix(const Matrix& v) : Matrix(v) {} | ||||
| 
 | ||||
|   template <class M> | ||||
|   LieMatrix(const M& v) : Matrix(v) {} | ||||
| 
 | ||||
| // Currently TMP constructor causes ICE on MSVS 2013
 | ||||
| #if (_MSC_VER < 1800) | ||||
|   /** initialize from a fixed size normal vector */ | ||||
|   template<int M, int N> | ||||
|   LieMatrix(const Eigen::Matrix<double, M, N>& v) : Matrix(v) {} | ||||
| #endif | ||||
| 
 | ||||
|   /** constructor with size and initial data, row order ! */ | ||||
|   LieMatrix(size_t m, size_t n, const double* const data) : | ||||
|       Matrix(Eigen::Map<const Matrix>(data, m, n)) {} | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Testable interface
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /** print @param s optional string naming the object */ | ||||
|   GTSAM_EXPORT void print(const std::string& name="") const; | ||||
| 
 | ||||
|   /** equality up to tolerance */ | ||||
|   inline bool equals(const LieMatrix& expected, double tol=1e-5) const { | ||||
|     return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); | ||||
|   } | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Standard Interface
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /** get the underlying matrix */ | ||||
|   inline Matrix matrix() const { | ||||
|     return static_cast<Matrix>(*this); | ||||
|   } | ||||
| 
 | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// @name Group
 | ||||
|   /// @{
 | ||||
|   LieMatrix compose(const LieMatrix& q) { return (*this)+q;} | ||||
|   LieMatrix between(const LieMatrix& q) { return q-(*this);} | ||||
|   LieMatrix inverse() { return -(*this);} | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// @name Manifold
 | ||||
|   /// @{
 | ||||
|   Vector localCoordinates(const LieMatrix& q) { return between(q).vector();} | ||||
|   LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));} | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// @name Lie Group
 | ||||
|   /// @{
 | ||||
|   static Vector Logmap(const LieMatrix& p) {return p.vector();} | ||||
|   static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);} | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// @name VectorSpace requirements
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /** Returns dimensionality of the tangent space */ | ||||
|   inline size_t dim() const { return size(); } | ||||
| 
 | ||||
|   /** Convert to vector, is done row-wise - TODO why? */ | ||||
|   inline Vector vector() const { | ||||
|     Vector result(size()); | ||||
|     typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, | ||||
|         Eigen::RowMajor> RowMajor; | ||||
|     Eigen::Map<RowMajor>(&result(0), rows(), cols()) = *this; | ||||
|     return result; | ||||
|   } | ||||
| 
 | ||||
|   /** identity - NOTE: no known size at compile time - so zero length */ | ||||
|   inline static LieMatrix identity() { | ||||
|     throw std::runtime_error("LieMatrix::identity(): Don't use this function"); | ||||
|     return LieMatrix(); | ||||
|   } | ||||
|   /// @}
 | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|   // Serialization function
 | ||||
|   friend class boost::serialization::access; | ||||
|   template<class Archive> | ||||
|   void serialize(Archive & ar, const unsigned int version) { | ||||
|     ar & boost::serialization::make_nvp("Matrix", | ||||
|        boost::serialization::base_object<Matrix>(*this)); | ||||
| 
 | ||||
|   } | ||||
| 
 | ||||
| }; | ||||
| 
 | ||||
| 
 | ||||
| template<> | ||||
| struct traits<LieMatrix> : public internal::VectorSpace<LieMatrix> { | ||||
| 
 | ||||
|   // Override Retract, as the default version does not know how to initialize
 | ||||
|   static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v, | ||||
|       ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { | ||||
|     if (H1) *H1 = Eye(origin); | ||||
|     if (H2) *H2 = Eye(origin); | ||||
|     typedef const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, | ||||
|         Eigen::RowMajor> RowMajor; | ||||
|     return origin + Eigen::Map<RowMajor>(&v(0), origin.rows(), origin.cols()); | ||||
|   } | ||||
| 
 | ||||
| }; | ||||
| 
 | ||||
| } // \namespace gtsam
 | ||||
|  | @ -8,7 +8,7 @@ | |||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| #include <gtsam/base/LieScalar.h> | ||||
| #include <gtsam/base/LieScalar_Deprecated.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
|   void LieScalar::print(const std::string& name) const { | ||||
|  |  | |||
|  | @ -11,7 +11,7 @@ | |||
| 
 | ||||
| /**
 | ||||
|  * @file LieScalar.h | ||||
|  * @brief A wrapper around scalar providing Lie compatibility | ||||
|  * @brief External deprecation warning, see LieScalar_Deprecated.h for details | ||||
|  * @author Kai Ni | ||||
|  */ | ||||
| 
 | ||||
|  | @ -23,69 +23,4 @@ | |||
|   #warning "LieScalar.h is deprecated. Please use double/float instead." | ||||
| #endif | ||||
| 
 | ||||
| #include <gtsam/dllexport.h> | ||||
| #include <gtsam/base/VectorSpace.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
|   /**
 | ||||
|    * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as | ||||
|    * we can directly add double, Vector, and Matrix into values now, because of | ||||
|    * gtsam::traits. | ||||
|    */ | ||||
|   struct GTSAM_EXPORT LieScalar { | ||||
| 
 | ||||
|     enum { dimension = 1 }; | ||||
| 
 | ||||
|     /** default constructor */ | ||||
|     LieScalar() : d_(0.0) {} | ||||
| 
 | ||||
|     /** wrap a double */ | ||||
|     /*explicit*/ LieScalar(double d) : d_(d) {} | ||||
| 
 | ||||
|     /** access the underlying value */ | ||||
|     double value() const { return d_; } | ||||
| 
 | ||||
|     /** Automatic conversion to underlying value */ | ||||
|     operator double() const { return d_; } | ||||
| 
 | ||||
|     /** convert vector */ | ||||
|     Vector1 vector() const { Vector1 v; v<<d_; return v; } | ||||
| 
 | ||||
|     /// @name Testable
 | ||||
|     /// @{
 | ||||
|     void print(const std::string& name="") const; | ||||
|     bool equals(const LieScalar& expected, double tol=1e-5) const { | ||||
|       return fabs(expected.d_ - d_) <= tol; | ||||
|     } | ||||
|     /// @}
 | ||||
| 
 | ||||
|     /// @name Group
 | ||||
|     /// @{
 | ||||
|     static LieScalar identity() { return LieScalar(0);} | ||||
|     LieScalar compose(const LieScalar& q) { return (*this)+q;} | ||||
|     LieScalar between(const LieScalar& q) { return q-(*this);} | ||||
|     LieScalar inverse() { return -(*this);} | ||||
|     /// @}
 | ||||
| 
 | ||||
|     /// @name Manifold
 | ||||
|     /// @{
 | ||||
|     size_t dim() const { return 1; } | ||||
|     Vector1 localCoordinates(const LieScalar& q) { return between(q).vector();} | ||||
|     LieScalar retract(const Vector1& v) {return compose(LieScalar(v[0]));} | ||||
|     /// @}
 | ||||
| 
 | ||||
|     /// @name Lie Group
 | ||||
|     /// @{
 | ||||
|     static Vector1 Logmap(const LieScalar& p) { return p.vector();} | ||||
|     static LieScalar Expmap(const Vector1& v) { return LieScalar(v[0]);} | ||||
|     /// @}
 | ||||
| 
 | ||||
|   private: | ||||
|       double d_; | ||||
|   }; | ||||
| 
 | ||||
|   template<> | ||||
|   struct traits<LieScalar> : public internal::ScalarTraits<LieScalar> {}; | ||||
| 
 | ||||
| } // \namespace gtsam
 | ||||
| #include <gtsam/base/LieScalar_Deprecated.h> | ||||
|  |  | |||
|  | @ -0,0 +1,85 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * 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 LieScalar.h | ||||
|  * @brief A wrapper around scalar providing Lie compatibility | ||||
|  * @author Kai Ni | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/dllexport.h> | ||||
| #include <gtsam/base/VectorSpace.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
|   /**
 | ||||
|    * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as | ||||
|    * we can directly add double, Vector, and Matrix into values now, because of | ||||
|    * gtsam::traits. | ||||
|    */ | ||||
|   struct GTSAM_EXPORT LieScalar { | ||||
| 
 | ||||
|     enum { dimension = 1 }; | ||||
| 
 | ||||
|     /** default constructor */ | ||||
|     LieScalar() : d_(0.0) {} | ||||
| 
 | ||||
|     /** wrap a double */ | ||||
|     /*explicit*/ LieScalar(double d) : d_(d) {} | ||||
| 
 | ||||
|     /** access the underlying value */ | ||||
|     double value() const { return d_; } | ||||
| 
 | ||||
|     /** Automatic conversion to underlying value */ | ||||
|     operator double() const { return d_; } | ||||
| 
 | ||||
|     /** convert vector */ | ||||
|     Vector1 vector() const { Vector1 v; v<<d_; return v; } | ||||
| 
 | ||||
|     /// @name Testable
 | ||||
|     /// @{
 | ||||
|     void print(const std::string& name="") const; | ||||
|     bool equals(const LieScalar& expected, double tol=1e-5) const { | ||||
|       return fabs(expected.d_ - d_) <= tol; | ||||
|     } | ||||
|     /// @}
 | ||||
| 
 | ||||
|     /// @name Group
 | ||||
|     /// @{
 | ||||
|     static LieScalar identity() { return LieScalar(0);} | ||||
|     LieScalar compose(const LieScalar& q) { return (*this)+q;} | ||||
|     LieScalar between(const LieScalar& q) { return q-(*this);} | ||||
|     LieScalar inverse() { return -(*this);} | ||||
|     /// @}
 | ||||
| 
 | ||||
|     /// @name Manifold
 | ||||
|     /// @{
 | ||||
|     size_t dim() const { return 1; } | ||||
|     Vector1 localCoordinates(const LieScalar& q) { return between(q).vector();} | ||||
|     LieScalar retract(const Vector1& v) {return compose(LieScalar(v[0]));} | ||||
|     /// @}
 | ||||
| 
 | ||||
|     /// @name Lie Group
 | ||||
|     /// @{
 | ||||
|     static Vector1 Logmap(const LieScalar& p) { return p.vector();} | ||||
|     static LieScalar Expmap(const Vector1& v) { return LieScalar(v[0]);} | ||||
|     /// @}
 | ||||
| 
 | ||||
|   private: | ||||
|       double d_; | ||||
|   }; | ||||
| 
 | ||||
|   template<> | ||||
|   struct traits<LieScalar> : public internal::ScalarTraits<LieScalar> {}; | ||||
| 
 | ||||
| } // \namespace gtsam
 | ||||
|  | @ -15,8 +15,8 @@ | |||
|  * @author Alex Cunningham | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/base/LieVector_Deprecated.h> | ||||
| #include <cstdarg> | ||||
| #include <gtsam/base/LieVector.h> | ||||
| 
 | ||||
| using namespace std; | ||||
| 
 | ||||
|  |  | |||
|  | @ -11,8 +11,8 @@ | |||
| 
 | ||||
| /**
 | ||||
|  * @file LieVector.h | ||||
|  * @brief A wrapper around vector providing Lie compatibility | ||||
|  * @author Alex Cunningham | ||||
|  * @brief Deprecation warning for LieVector_Deprecated.h, see LieVector_Deprecated.h for details. | ||||
|  * @author Paul Drews | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
|  | @ -23,100 +23,4 @@ | |||
| #warning "LieVector.h is deprecated. Please use Eigen::Vector instead." | ||||
| #endif | ||||
| 
 | ||||
| #include <gtsam/base/VectorSpace.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /**
 | ||||
|  * @deprecated: LieVector, LieVector and LieMatrix are obsolete in GTSAM 4.0 as | ||||
|  * we can directly add double, Vector, and Matrix into values now, because of | ||||
|  * gtsam::traits. | ||||
|  */ | ||||
| struct LieVector : public Vector { | ||||
| 
 | ||||
|   enum { dimension = Eigen::Dynamic }; | ||||
| 
 | ||||
|   /** default constructor - should be unnecessary */ | ||||
|   LieVector() {} | ||||
| 
 | ||||
|   /** initialize from a normal vector */ | ||||
|   LieVector(const Vector& v) : Vector(v) {} | ||||
|    | ||||
|   template <class V> | ||||
|   LieVector(const V& v) : Vector(v) {} | ||||
| 
 | ||||
| // Currently TMP constructor causes ICE on MSVS 2013
 | ||||
| #if (_MSC_VER < 1800) | ||||
|   /** initialize from a fixed size normal vector */ | ||||
|   template<int N> | ||||
|   LieVector(const Eigen::Matrix<double, N, 1>& v) : Vector(v) {} | ||||
| #endif | ||||
| 
 | ||||
|   /** wrap a double */ | ||||
|   LieVector(double d) : Vector((Vector(1) << d).finished()) {} | ||||
| 
 | ||||
|   /** constructor with size and initial data, row order ! */ | ||||
|   GTSAM_EXPORT LieVector(size_t m, const double* const data); | ||||
| 
 | ||||
|   /// @name Testable
 | ||||
|   /// @{
 | ||||
|   GTSAM_EXPORT void print(const std::string& name="") const; | ||||
|   bool equals(const LieVector& expected, double tol=1e-5) const { | ||||
|     return gtsam::equal(vector(), expected.vector(), tol); | ||||
|   } | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// @name Group
 | ||||
|   /// @{
 | ||||
|   LieVector compose(const LieVector& q) { return (*this)+q;} | ||||
|   LieVector between(const LieVector& q) { return q-(*this);} | ||||
|   LieVector inverse() { return -(*this);} | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// @name Manifold
 | ||||
|   /// @{
 | ||||
|   Vector localCoordinates(const LieVector& q) { return between(q).vector();} | ||||
|   LieVector retract(const Vector& v) {return compose(LieVector(v));} | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// @name Lie Group
 | ||||
|   /// @{
 | ||||
|   static Vector Logmap(const LieVector& p) {return p.vector();} | ||||
|   static LieVector Expmap(const Vector& v) { return LieVector(v);} | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// @name VectorSpace requirements
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /** get the underlying vector */ | ||||
|   Vector vector() const { | ||||
|     return static_cast<Vector>(*this); | ||||
|   } | ||||
| 
 | ||||
|   /** Returns dimensionality of the tangent space */ | ||||
|   size_t dim() const { return this->size(); } | ||||
| 
 | ||||
|   /** identity - NOTE: no known size at compile time - so zero length */ | ||||
|   static LieVector identity() { | ||||
|     throw std::runtime_error("LieVector::identity(): Don't use this function"); | ||||
|     return LieVector(); | ||||
|   } | ||||
| 
 | ||||
|   /// @}
 | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|   // Serialization function
 | ||||
|   friend class boost::serialization::access; | ||||
|   template<class Archive> | ||||
|   void serialize(Archive & ar, const unsigned int version) { | ||||
|     ar & boost::serialization::make_nvp("Vector", | ||||
|        boost::serialization::base_object<Vector>(*this)); | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| 
 | ||||
| template<> | ||||
| struct traits<LieVector> : public internal::VectorSpace<LieVector> {}; | ||||
| 
 | ||||
| } // \namespace gtsam
 | ||||
| #include <gtsam/base/LieVector_Deprecated.h> | ||||
|  |  | |||
|  | @ -0,0 +1,116 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * 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 LieVector.h | ||||
|  * @brief A wrapper around vector providing Lie compatibility | ||||
|  * @author Alex Cunningham | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/base/VectorSpace.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /**
 | ||||
|  * @deprecated: LieVector, LieVector and LieMatrix are obsolete in GTSAM 4.0 as | ||||
|  * we can directly add double, Vector, and Matrix into values now, because of | ||||
|  * gtsam::traits. | ||||
|  */ | ||||
| struct LieVector : public Vector { | ||||
| 
 | ||||
|   enum { dimension = Eigen::Dynamic }; | ||||
| 
 | ||||
|   /** default constructor - should be unnecessary */ | ||||
|   LieVector() {} | ||||
| 
 | ||||
|   /** initialize from a normal vector */ | ||||
|   LieVector(const Vector& v) : Vector(v) {} | ||||
|    | ||||
|   template <class V> | ||||
|   LieVector(const V& v) : Vector(v) {} | ||||
| 
 | ||||
| // Currently TMP constructor causes ICE on MSVS 2013
 | ||||
| #if (_MSC_VER < 1800) | ||||
|   /** initialize from a fixed size normal vector */ | ||||
|   template<int N> | ||||
|   LieVector(const Eigen::Matrix<double, N, 1>& v) : Vector(v) {} | ||||
| #endif | ||||
| 
 | ||||
|   /** wrap a double */ | ||||
|   LieVector(double d) : Vector((Vector(1) << d).finished()) {} | ||||
| 
 | ||||
|   /** constructor with size and initial data, row order ! */ | ||||
|   GTSAM_EXPORT LieVector(size_t m, const double* const data); | ||||
| 
 | ||||
|   /// @name Testable
 | ||||
|   /// @{
 | ||||
|   GTSAM_EXPORT void print(const std::string& name="") const; | ||||
|   bool equals(const LieVector& expected, double tol=1e-5) const { | ||||
|     return gtsam::equal(vector(), expected.vector(), tol); | ||||
|   } | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// @name Group
 | ||||
|   /// @{
 | ||||
|   LieVector compose(const LieVector& q) { return (*this)+q;} | ||||
|   LieVector between(const LieVector& q) { return q-(*this);} | ||||
|   LieVector inverse() { return -(*this);} | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// @name Manifold
 | ||||
|   /// @{
 | ||||
|   Vector localCoordinates(const LieVector& q) { return between(q).vector();} | ||||
|   LieVector retract(const Vector& v) {return compose(LieVector(v));} | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// @name Lie Group
 | ||||
|   /// @{
 | ||||
|   static Vector Logmap(const LieVector& p) {return p.vector();} | ||||
|   static LieVector Expmap(const Vector& v) { return LieVector(v);} | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// @name VectorSpace requirements
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /** get the underlying vector */ | ||||
|   Vector vector() const { | ||||
|     return static_cast<Vector>(*this); | ||||
|   } | ||||
| 
 | ||||
|   /** Returns dimensionality of the tangent space */ | ||||
|   size_t dim() const { return this->size(); } | ||||
| 
 | ||||
|   /** identity - NOTE: no known size at compile time - so zero length */ | ||||
|   static LieVector identity() { | ||||
|     throw std::runtime_error("LieVector::identity(): Don't use this function"); | ||||
|     return LieVector(); | ||||
|   } | ||||
| 
 | ||||
|   /// @}
 | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|   // Serialization function
 | ||||
|   friend class boost::serialization::access; | ||||
|   template<class Archive> | ||||
|   void serialize(Archive & ar, const unsigned int version) { | ||||
|     ar & boost::serialization::make_nvp("Vector", | ||||
|        boost::serialization::base_object<Vector>(*this)); | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| 
 | ||||
| template<> | ||||
| struct traits<LieVector> : public internal::VectorSpace<LieVector> {}; | ||||
| 
 | ||||
| } // \namespace gtsam
 | ||||
|  | @ -15,10 +15,10 @@ | |||
|  */ | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <gtsam/base/LieMatrix_Deprecated.h> | ||||
| 
 | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/base/Manifold.h> | ||||
| #include <gtsam/base/LieMatrix.h> | ||||
| 
 | ||||
| using namespace gtsam; | ||||
| 
 | ||||
|  |  | |||
|  | @ -15,10 +15,10 @@ | |||
|  */ | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <gtsam/base/LieScalar_Deprecated.h> | ||||
| 
 | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/base/Manifold.h> | ||||
| #include <gtsam/base/LieScalar.h> | ||||
| 
 | ||||
| using namespace gtsam; | ||||
| 
 | ||||
|  |  | |||
|  | @ -15,10 +15,10 @@ | |||
|  */ | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <gtsam/base/LieVector_Deprecated.h> | ||||
| 
 | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/base/Manifold.h> | ||||
| #include <gtsam/base/LieVector.h> | ||||
| 
 | ||||
| using namespace gtsam; | ||||
| 
 | ||||
|  |  | |||
|  | @ -15,8 +15,8 @@ | |||
|  */ | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <gtsam/base/LieScalar_Deprecated.h> | ||||
| 
 | ||||
| #include <gtsam/base/LieScalar.h> | ||||
| #include <gtsam/base/TestableAssertions.h> | ||||
| 
 | ||||
| using namespace gtsam; | ||||
|  |  | |||
|  | @ -477,7 +477,7 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor { | |||
|   DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model); | ||||
| }; | ||||
| 
 | ||||
| #include <gtsam/base/LieScalar.h> | ||||
| #include <gtsam/base/LieScalar_Deprecated.h> | ||||
| 
 | ||||
| #include <gtsam_unstable/dynamics/VelocityConstraint3.h> | ||||
| virtual class VelocityConstraint3 : gtsam::NonlinearFactor { | ||||
|  |  | |||
|  | @ -5,6 +5,8 @@ | |||
|  * @author Alex Cunningham | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/base/LieMatrix_Deprecated.h> | ||||
| #include <gtsam/base/LieVector_Deprecated.h> | ||||
| #include <gtsam/slam/serialization.h> | ||||
| #include <gtsam/base/serialization.h> | ||||
| 
 | ||||
|  | @ -22,9 +24,6 @@ | |||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/linear/GaussianISAM.h> | ||||
| #include <gtsam/linear/GaussianMultifrontalSolver.h> | ||||
| #include <gtsam/base/LieVector.h> | ||||
| #include <gtsam/base/LieMatrix.h> | ||||
| //#include <gtsam/geometry/StereoPoint2.h>
 | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/geometry/Pose3.h> | ||||
| #include <gtsam/geometry/Cal3DS2.h> | ||||
|  |  | |||
|  | @ -20,9 +20,8 @@ | |||
| #include <gtsam/nonlinear/Values.h> | ||||
| #include <gtsam/inference/Key.h> | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| #include <gtsam/base/LieVector.h> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <gtsam/base/LieVector_Deprecated.h> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
|  |  | |||
|  | @ -23,10 +23,9 @@ | |||
| #include <gtsam/base/debug.h> | ||||
| #include <gtsam/base/TestableAssertions.h> | ||||
| #include <gtsam/base/treeTraversal-inst.h> | ||||
| #include <gtsam/base/LieScalar.h> | ||||
| 
 | ||||
| #include <boost/foreach.hpp> | ||||
| #include <boost/assign/list_of.hpp> | ||||
| #include <gtsam/base/LieScalar_Deprecated.h> | ||||
| using namespace boost::assign; | ||||
| #include <boost/range/adaptor/map.hpp> | ||||
| namespace br { using namespace boost::adaptors; using namespace boost::range; } | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue