diff --git a/gtsam.h b/gtsam.h index f8e0af28e..1aee996dc 100644 --- a/gtsam.h +++ b/gtsam.h @@ -156,7 +156,7 @@ virtual class Value { size_t dim() const; }; -#include +#include class LieScalar { // Standard constructors LieScalar(); @@ -185,7 +185,7 @@ class LieScalar { static Vector Logmap(const gtsam::LieScalar& p); }; -#include +#include class LieVector { // Standard constructors LieVector(); @@ -217,7 +217,7 @@ class LieVector { void serialize() const; }; -#include +#include class LieMatrix { // Standard constructors LieMatrix(); diff --git a/gtsam/base/LieMatrix.cpp b/gtsam/base/LieMatrix.cpp index 69054fc1c..cf5b57536 100644 --- a/gtsam/base/LieMatrix.cpp +++ b/gtsam/base/LieMatrix.cpp @@ -15,7 +15,7 @@ * @author Richard Roberts and Alex Cunningham */ -#include +#include namespace gtsam { diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 90b7207a2..e24f339e4 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -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 - #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 -#include - -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 - 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 - LieMatrix(const Eigen::Matrix& 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(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(*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 RowMajor; - Eigen::Map(&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 - void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Matrix", - boost::serialization::base_object(*this)); - - } - -}; - - -template<> -struct traits : public internal::VectorSpace { - - // 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 RowMajor; - return origin + Eigen::Map(&v(0), origin.rows(), origin.cols()); - } - -}; - -} // \namespace gtsam +#include "gtsam/base/LieMatrix_Deprecated.h" diff --git a/gtsam/base/LieMatrix_Deprecated.h b/gtsam/base/LieMatrix_Deprecated.h new file mode 100644 index 000000000..5dbe9935f --- /dev/null +++ b/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 + +#include +#include + +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 + 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 + LieMatrix(const Eigen::Matrix& 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(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(*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 RowMajor; + Eigen::Map(&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 + void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("Matrix", + boost::serialization::base_object(*this)); + + } + +}; + + +template<> +struct traits : public internal::VectorSpace { + + // 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 RowMajor; + return origin + Eigen::Map(&v(0), origin.rows(), origin.cols()); + } + +}; + +} // \namespace gtsam diff --git a/gtsam/base/LieScalar.cpp b/gtsam/base/LieScalar.cpp index 5f59c29bc..4c5a6a257 100644 --- a/gtsam/base/LieScalar.cpp +++ b/gtsam/base/LieScalar.cpp @@ -8,7 +8,7 @@ -#include +#include namespace gtsam { void LieScalar::print(const std::string& name) const { diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 9f6c56b28..650e2bb21 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -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 -#include - -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< - struct traits : public internal::ScalarTraits {}; - -} // \namespace gtsam +#include diff --git a/gtsam/base/LieScalar_Deprecated.h b/gtsam/base/LieScalar_Deprecated.h new file mode 100644 index 000000000..6ffc76d37 --- /dev/null +++ b/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 +#include + +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< + struct traits : public internal::ScalarTraits {}; + +} // \namespace gtsam diff --git a/gtsam/base/LieVector.cpp b/gtsam/base/LieVector.cpp index 84afdabc8..3e4eeecf2 100644 --- a/gtsam/base/LieVector.cpp +++ b/gtsam/base/LieVector.cpp @@ -15,8 +15,8 @@ * @author Alex Cunningham */ +#include #include -#include using namespace std; diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index c2003df3c..813431775 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -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 - -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 - 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 - LieVector(const Eigen::Matrix& 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(*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 - void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Vector", - boost::serialization::base_object(*this)); - } -}; - - -template<> -struct traits : public internal::VectorSpace {}; - -} // \namespace gtsam +#include diff --git a/gtsam/base/LieVector_Deprecated.h b/gtsam/base/LieVector_Deprecated.h new file mode 100644 index 000000000..3cb73319d --- /dev/null +++ b/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 + +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 + 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 + LieVector(const Eigen::Matrix& 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(*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 + void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("Vector", + boost::serialization::base_object(*this)); + } +}; + + +template<> +struct traits : public internal::VectorSpace {}; + +} // \namespace gtsam diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp index 7cc649e66..09caadabd 100644 --- a/gtsam/base/tests/testLieMatrix.cpp +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -15,10 +15,10 @@ */ #include +#include #include #include -#include using namespace gtsam; diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index 060087c2a..141b55761 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -15,10 +15,10 @@ */ #include +#include #include #include -#include using namespace gtsam; diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index 81e03c63c..ed3afac8c 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -15,10 +15,10 @@ */ #include +#include #include #include -#include using namespace gtsam; diff --git a/gtsam/base/tests/testTestableAssertions.cpp b/gtsam/base/tests/testTestableAssertions.cpp index d8c62b121..364834e2a 100644 --- a/gtsam/base/tests/testTestableAssertions.cpp +++ b/gtsam/base/tests/testTestableAssertions.cpp @@ -15,8 +15,8 @@ */ #include +#include -#include #include using namespace gtsam; diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 95e635516..9aa32e1c3 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -477,7 +477,7 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor { DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model); }; -#include +#include #include virtual class VelocityConstraint3 : gtsam::NonlinearFactor { diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 31995e769..a598fb689 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -5,6 +5,8 @@ * @author Alex Cunningham */ +#include +#include #include #include @@ -22,9 +24,6 @@ #include #include #include -#include -#include -//#include #include #include #include diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp index ff7307840..a86306a8d 100644 --- a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp +++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -20,9 +20,8 @@ #include #include #include -#include - #include +#include using namespace std; using namespace gtsam; diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index b43f0d3ef..4e14d49b3 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -23,10 +23,9 @@ #include #include #include -#include - #include #include +#include using namespace boost::assign; #include namespace br { using namespace boost::adaptors; using namespace boost::range; }