Fix deprecation warnings
parent
ae5994c262
commit
8a88f101db
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