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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
gtsam/geometry/SOn-inl.h
View File

@ -22,62 +22,16 @@
namespace gtsam {
// Implementation for N>5 just uses dynamic version
template <int N>
Matrix SO<N>::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;
typename SO<N>::MatrixNN SO<N>::Hat(const TangentVector& xi) {
return SOn::Hat(xi);
}
// Implementation for N>5 just uses dynamic version
template <int N>
Vector SO<N>::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;
}
typename SO<N>::TangentVector SO<N>::Vee(const MatrixNN& X) {
return SOn::Vee(X);
}
template <int N>

58
gtsam/geometry/SOn.cpp
View File

@ -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<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 <>
SOn LieGroup<SOn, Eigen::Dynamic>::compose(const SOn& g, DynamicJacobian H1,
DynamicJacobian H2) const {

24
gtsam/geometry/SOn.h
View File

@ -83,6 +83,10 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
template <typename Derived>
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
template <int N_ = N, typename = IsSO3<N_>>
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
* 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<SO<N>, internal::DimensionSO(N)> {
using SOn = SO<Eigen::Dynamic>;
/*
* 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<Eigen::Dynamic, Eigen::Dynamic>;