12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Made matrix a member variable

release/4.3a0
Frank Dellaert 2019-05-03 08:16:04 -04:00 committed by Fan Jiang
parent 37c53aa23f
commit a35709295a
1 changed files with 27 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
gtsam/geometry/tests/testSOn.cpp
View File

@ -31,53 +31,64 @@ constexpr int DimensionSO(int N) {
} }
} // namespace internal } // namespace internal
/**
* Space of special orthogonal rotation matrices SO<N>
*/
template <int N> template <int N>
class SO : public Eigen::Matrix<double, N, N>, class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
public LieGroup<SO<N>, internal::DimensionSO(N)> {
public: public:
enum { dimension = internal::DimensionSO(N) }; enum { dimension = internal::DimensionSO(N) };
using MatrixNN = Eigen::Matrix<double, N, N>; using MatrixNN = Eigen::Matrix<double, N, N>;
using VectorD = Eigen::Matrix<double, dimension, 1>; using VectorD = Eigen::Matrix<double, dimension, 1>;
using MatrixDD = Eigen::Matrix<double, dimension, dimension>; using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
MatrixNN matrix_; ///< Rotation matrix
/// @name Constructors /// @name Constructors
/// @{ /// @{
/// Construct SO<N> identity for N >= 2 /// Construct SO<N> identity for N >= 2
template <int N_ = N, typename = boost::enable_if_t<N_ >= 2, void>> template <int N_ = N, typename = boost::enable_if_t<N_ >= 2, void>>
SO() { SO() : matrix_(MatrixNN::Identity()) {}
*this << MatrixNN::Identity();
}
/// Construct SO<N> identity for N == Eigen::Dynamic /// Construct SO<N> identity for N == Eigen::Dynamic
template <int N_ = N, template <int N_ = N,
typename = boost::enable_if_t<N_ == Eigen::Dynamic, void>> typename = boost::enable_if_t<N_ == Eigen::Dynamic, void>>
explicit SO(size_t n = 0) { explicit SO(size_t n = 0) {
if (n == 0) throw std::runtime_error("SO: Dimensionality not known."); if (n == 0) throw std::runtime_error("SO: Dimensionality not known.");
this->resize(n, n); matrix_ = Eigen::MatrixXd::Identity(n, n);
*this << Eigen::MatrixXd::Identity(n, n);
} }
/// Constructor from Eigen Matrix /// Constructor from Eigen Matrix
template <typename Derived> template <typename Derived>
SO(const Eigen::MatrixBase<Derived>& R) : MatrixNN(R.eval()) {} explicit SO(const Eigen::MatrixBase<Derived>& R) : matrix_(R.eval()) {}
/// @}
/// @name Standard methods
/// @{
size_t rows() const { return matrix_.rows(); }
size_t cols() const { return matrix_.cols(); }
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{
void print(const std::string& s) const { void print(const std::string& s) const {
std::cout << s << *this << std::endl; std::cout << s << matrix_ << std::endl;
} }
bool equals(const SO& R, double tol) const { bool equals(const SO& other, double tol) const {
return equal_with_abs_tol(*this, R, tol); return equal_with_abs_tol(matrix_, other.matrix_, tol);
} }
/// @} /// @}
/// @name Group /// @name Group
/// @{ /// @{
/// Multiplication
SO operator*(const SO& other) const { return SO(matrix_ * other.matrix_); }
/// SO<N> identity for N >= 2 /// SO<N> identity for N >= 2
template <int N_ = N, typename = boost::enable_if_t<N_ >= 2, void>> template <int N_ = N, typename = boost::enable_if_t<N_ >= 2, void>>
static SO identity() { static SO identity() {
@ -92,7 +103,7 @@ class SO : public Eigen::Matrix<double, N, N>,
} }
/// inverse of a rotation = transpose /// inverse of a rotation = transpose
SO inverse() const { return this->transpose(); } SO inverse() const { return SO(matrix_.transpose()); }
/// @} /// @}
/// @name Manifold /// @name Manifold
@ -119,12 +130,14 @@ class SO : public Eigen::Matrix<double, N, N>,
/** /**
* Exponential map at identity - create a rotation from canonical coordinates * Exponential map at identity - create a rotation from canonical coordinates
*/ */
static SO Expmap(const VectorD& omega, OptionalJacobian<dimension, dimension> H = boost::none); static SO Expmap(const VectorD& omega,
OptionalJacobian<dimension, dimension> H = boost::none);
/** /**
* Log map at identity - returns the canonical coordinates of this rotation * Log map at identity - returns the canonical coordinates of this rotation
*/ */
static VectorD Logmap(const SO& R, OptionalJacobian<dimension, dimension> H = boost::none); static VectorD Logmap(const SO& R,
OptionalJacobian<dimension, dimension> H = boost::none);
// inverse with optional derivative // inverse with optional derivative
using LieGroup<SO<N>, internal::DimensionSO(N)>::inverse; using LieGroup<SO<N>, internal::DimensionSO(N)>::inverse;