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

VectorizedGenerators

release/4.3a0
Frank Dellaert 2020-08-01 15:41:19 -04:00
parent 94f744ecae
commit 458a33dade
2 changed files with 33 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -26,13 +26,13 @@ using namespace std;
namespace gtsam { namespace gtsam {
// Implementation for N>5 just uses dynamic version // Implementation for N>=5 just uses dynamic version
template <int N> template <int N>
typename SO<N>::MatrixNN SO<N>::Hat(const TangentVector& xi) { typename SO<N>::MatrixNN SO<N>::Hat(const TangentVector& xi) {
return SOn::Hat(xi); return SOn::Hat(xi);
} }
// Implementation for N>5 just uses dynamic version // Implementation for N>=5 just uses dynamic version
template <int N> template <int N>
typename SO<N>::TangentVector SO<N>::Vee(const MatrixNN& X) { typename SO<N>::TangentVector SO<N>::Vee(const MatrixNN& X) {
return SOn::Vee(X); return SOn::Vee(X);
@ -99,12 +99,8 @@ typename SO<N>::VectorN2 SO<N>::vec(
if (H) { if (H) {
// Calculate P matrix of vectorized generators // Calculate P matrix of vectorized generators
// TODO(duy): Should we refactor this as the jacobian of Hat? // TODO(duy): Should we refactor this as the jacobian of Hat?
Matrix P = VectorizedGenerators(n);
const size_t d = dim(); const size_t d = dim();
Matrix P(n2, d);
for (size_t j = 0; j < d; j++) {
const auto X = Hat(Eigen::VectorXd::Unit(d, j));
P.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
}
H->resize(n2, d); H->resize(n2, d);
for (size_t i = 0; i < n; i++) { for (size_t i = 0; i < n; i++) {
H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d); H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d);

31
gtsam/geometry/SOn.h
View File

@ -290,7 +290,34 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
* */ * */
VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H = VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H =
boost::none) const; boost::none) const;
/// @}
/// Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
template <int N_ = N, typename = IsFixed<N_>>
static Matrix VectorizedGenerators() {
constexpr size_t N2 = static_cast<size_t>(N * N);
Matrix G(N2, dimension);
for (size_t j = 0; j < dimension; j++) {
const auto X = Hat(Vector::Unit(dimension, j));
G.col(j) = Eigen::Map<const Matrix>(X.data(), N2, 1);
}
return G;
}
/// Calculate n^2 x dim matrix of vectorized Lie algebra generators for SO(n)
template <int N_ = N, typename = IsDynamic<N_>>
static Matrix VectorizedGenerators(size_t n = 0) {
const size_t n2 = n * n, dim = Dimension(n);
Matrix G(n2, dim);
for (size_t j = 0; j < dim; j++) {
const auto X = Hat(Vector::Unit(dim, j));
G.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
}
return G;
}
/// @{
/// @name Serialization
/// @{
template <class Archive> template <class Archive>
friend void save(Archive&, SO&, const unsigned int); friend void save(Archive&, SO&, const unsigned int);
@ -300,6 +327,8 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
friend void serialize(Archive&, SO&, const unsigned int); friend void serialize(Archive&, SO&, const unsigned int);
friend class boost::serialization::access; friend class boost::serialization::access;
friend class Rot3; // for serialize friend class Rot3; // for serialize
/// @}
}; };
using SOn = SO<Eigen::Dynamic>; using SOn = SO<Eigen::Dynamic>;