Specialized Hat/Vee

release/4.3a0
Frank Dellaert 2019-05-06 18:45:16 -04:00 committed by Fan Jiang
parent d376d0158d
commit ab1be9c4de
3 changed files with 84 additions and 56 deletions

View File

@ -22,62 +22,16 @@
namespace gtsam { namespace gtsam {
// Implementation for N>5 just uses dynamic version
template <int N> template <int N>
Matrix SO<N>::Hat(const Vector& xi) { typename SO<N>::MatrixNN SO<N>::Hat(const TangentVector& xi) {
size_t n = AmbientDim(xi.size()); return SOn::Hat(xi);
if (n < 2) throw std::invalid_argument("SO<N>::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;
} }
// Implementation for N>5 just uses dynamic version
template <int N> template <int N>
Vector SO<N>::Vee(const Matrix& X) { typename SO<N>::TangentVector SO<N>::Vee(const MatrixNN& X) {
const size_t n = X.rows(); return SOn::Vee(X);
if (n < 2) throw std::invalid_argument("SO<N>::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 <int N> template <int N>

View File

@ -20,6 +20,64 @@
namespace gtsam { namespace gtsam {
template <>
Matrix SOn::Hat(const Vector& xi) {
size_t n = AmbientDim(xi.size());
if (n < 2) throw std::invalid_argument("SO<N>::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<N>::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 <> template <>
SOn LieGroup<SOn, Eigen::Dynamic>::compose(const SOn& g, DynamicJacobian H1, SOn LieGroup<SOn, Eigen::Dynamic>::compose(const SOn& g, DynamicJacobian H1,
DynamicJacobian H2) const { DynamicJacobian H2) const {

View File

@ -83,6 +83,10 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
template <typename Derived> template <typename Derived>
explicit SO(const Eigen::MatrixBase<Derived>& R) : matrix_(R.eval()) {} explicit SO(const Eigen::MatrixBase<Derived>& R) : matrix_(R.eval()) {}
/// Construct dynamic SO(n) from Fixed SO<M>
template <int M, int N_ = N, typename = IsDynamic<N_>>
explicit SO(const SO<M>& R) : matrix_(R.matrix()) {}
/// Constructor from AngleAxisd /// Constructor from AngleAxisd
template <int N_ = N, typename = IsSO3<N_>> template <int N_ = N, typename = IsSO3<N_>>
SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {} SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {}
@ -188,12 +192,12 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
* -d c -b a 0 * -d c -b a 0
* This scheme behaves exactly as expected for SO(2) and SO(3). * 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. * 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 // Chart at origin
struct ChartAtOrigin { struct ChartAtOrigin {
@ -259,8 +263,20 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
using SOn = SO<Eigen::Dynamic>; using SOn = SO<Eigen::Dynamic>;
/* /*
* Fully specialize compose and between, because the derivative is unknowable by * Specialize dynamic Hat and Vee, because recursion depends on dynamic nature.
* the LieGroup implementations, who return a fixed-size matrix for H2. * 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<Eigen::Dynamic, Eigen::Dynamic>; using DynamicJacobian = OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>;