diff --git a/gtsam/geometry/SOn-inl.h b/gtsam/geometry/SOn-inl.h index d1c9efe29..1c05eac47 100644 --- a/gtsam/geometry/SOn-inl.h +++ b/gtsam/geometry/SOn-inl.h @@ -22,62 +22,16 @@ namespace gtsam { +// Implementation for N>5 just uses dynamic version template -Matrix SO::Hat(const Vector& xi) { - size_t n = AmbientDim(xi.size()); - if (n < 2) throw std::invalid_argument("SO::Hat: n<2 not supported"); - - Matrix X(n, n); // allocate space for n*n skew-symmetric matrix - X.setZero(); - if (n == 2) { - // Handle SO(2) case as recursion bottom - assert(xi.size() == 1); - X << 0, -xi(0), xi(0), 0; - } else { - // Recursively call SO(n-1) call for top-left block - const size_t dmin = (n - 1) * (n - 2) / 2; - X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin)); - - // Now fill last row and column - double sign = 1.0; - for (size_t i = 0; i < n - 1; i++) { - const size_t j = n - 2 - i; - X(n - 1, j) = sign * xi(i); - X(j, n - 1) = -X(n - 1, j); - sign = -sign; - } - } - return X; +typename SO::MatrixNN SO::Hat(const TangentVector& xi) { + return SOn::Hat(xi); } +// Implementation for N>5 just uses dynamic version template -Vector SO::Vee(const Matrix& X) { - const size_t n = X.rows(); - if (n < 2) throw std::invalid_argument("SO::Hat: n<2 not supported"); - - if (n == 2) { - // Handle SO(2) case as recursion bottom - Vector xi(1); - xi(0) = X(1, 0); - return xi; - } else { - // Calculate dimension and allocate space - const size_t d = n * (n - 1) / 2; - Vector xi(d); - - // Fill first n-1 spots from last row of X - double sign = 1.0; - for (size_t i = 0; i < n - 1; i++) { - const size_t j = n - 2 - i; - xi(i) = sign * X(n - 1, j); - sign = -sign; - } - - // Recursively call Vee to fill remainder of x - const size_t dmin = (n - 1) * (n - 2) / 2; - xi.tail(dmin) = Vee(X.topLeftCorner(n - 1, n - 1)); - return xi; - } +typename SO::TangentVector SO::Vee(const MatrixNN& X) { + return SOn::Vee(X); } template diff --git a/gtsam/geometry/SOn.cpp b/gtsam/geometry/SOn.cpp index 67b780db8..06d24472e 100644 --- a/gtsam/geometry/SOn.cpp +++ b/gtsam/geometry/SOn.cpp @@ -20,6 +20,64 @@ namespace gtsam { +template <> +Matrix SOn::Hat(const Vector& xi) { + size_t n = AmbientDim(xi.size()); + if (n < 2) throw std::invalid_argument("SO::Hat: n<2 not supported"); + + Matrix X(n, n); // allocate space for n*n skew-symmetric matrix + X.setZero(); + if (n == 2) { + // Handle SO(2) case as recursion bottom + assert(xi.size() == 1); + X << 0, -xi(0), xi(0), 0; + } else { + // Recursively call SO(n-1) call for top-left block + const size_t dmin = (n - 1) * (n - 2) / 2; + X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin)); + + // Now fill last row and column + double sign = 1.0; + for (size_t i = 0; i < n - 1; i++) { + const size_t j = n - 2 - i; + X(n - 1, j) = sign * xi(i); + X(j, n - 1) = -X(n - 1, j); + sign = -sign; + } + } + return X; +} + +template <> +Vector SOn::Vee(const Matrix& X) { + const size_t n = X.rows(); + if (n < 2) throw std::invalid_argument("SO::Hat: n<2 not supported"); + + if (n == 2) { + // Handle SO(2) case as recursion bottom + Vector xi(1); + xi(0) = X(1, 0); + return xi; + } else { + // Calculate dimension and allocate space + const size_t d = n * (n - 1) / 2; + Vector xi(d); + + // Fill first n-1 spots from last row of X + double sign = 1.0; + for (size_t i = 0; i < n - 1; i++) { + const size_t j = n - 2 - i; + xi(i) = sign * X(n - 1, j); + sign = -sign; + } + + // Recursively call Vee to fill remainder of x + const size_t dmin = (n - 1) * (n - 2) / 2; + xi.tail(dmin) = Vee(X.topLeftCorner(n - 1, n - 1)); + return xi; + } +} + template <> SOn LieGroup::compose(const SOn& g, DynamicJacobian H1, DynamicJacobian H2) const { diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 45fe43519..56cdeb3fe 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -83,6 +83,10 @@ class SO : public LieGroup, internal::DimensionSO(N)> { template explicit SO(const Eigen::MatrixBase& R) : matrix_(R.eval()) {} + /// Construct dynamic SO(n) from Fixed SO + template > + explicit SO(const SO& R) : matrix_(R.matrix()) {} + /// Constructor from AngleAxisd template > SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {} @@ -188,12 +192,12 @@ class SO : public LieGroup, internal::DimensionSO(N)> { * -d c -b a 0 * This scheme behaves exactly as expected for SO(2) and SO(3). */ - static Matrix Hat(const Vector& xi); + static MatrixNN Hat(const TangentVector& xi); /** * Inverse of Hat. See note about xi element order in Hat. */ - static Vector Vee(const Matrix& X); + static TangentVector Vee(const MatrixNN& X); // Chart at origin struct ChartAtOrigin { @@ -259,8 +263,20 @@ class SO : public LieGroup, internal::DimensionSO(N)> { using SOn = SO; /* - * Fully specialize compose and between, because the derivative is unknowable by - * the LieGroup implementations, who return a fixed-size matrix for H2. + * Specialize dynamic Hat and Vee, because recursion depends on dynamic nature. + * The definition is in SOn.cpp. Fixed-size SO3 and SO4 have their own version, + * and implementation for other fixed N is in SOn-inl.h. + */ + +template <> +Matrix SOn::Hat(const Vector& xi); + +template <> +Vector SOn::Vee(const Matrix& X); + +/* + * Specialize dynamic compose and between, because the derivative is unknowable + * by the LieGroup implementations, who return a fixed-size matrix for H2. */ using DynamicJacobian = OptionalJacobian;