ShonanGaugeFactor.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #pragma once
20 
21 #include <gtsam/geometry/SOn.h>
24 
25 namespace gtsam {
45 class GTSAM_EXPORT ShonanGaugeFactor : public NonlinearFactor {
46  // Row dimension, equal to the dimensionality of SO(p-d)
47  size_t rows_;
48 
50  std::shared_ptr<JacobianFactor> whitenedJacobian_;
51 
52 public:
58  ShonanGaugeFactor(Key key, size_t p, size_t d = 3,
59  std::optional<double> gamma = {})
60  : NonlinearFactor(KeyVector{key}) {
61  if (p < d) {
62  throw std::invalid_argument("ShonanGaugeFactor must have p>=d.");
63  }
64  // Calculate dimensions
65  size_t q = p - d;
66  size_t P = SOn::Dimension(p); // dimensionality of SO(p)
67  rows_ = SOn::Dimension(q); // dimensionality of SO(q), the gauge
68 
69  // Create constant Jacobian as a rows_*P matrix: there are rows_ penalized
70  // dimensions, but it is a bit tricky to find them among the P columns.
71  // The key is to look at how skew-symmetric matrices are laid out in SOn.h:
72  // the first tangent dimension will always be included, but beyond that we
73  // have to be careful. We always need to skip the d top-rows of the skew-
74  // symmetric matrix as they below to K, part of the Stiefel manifold.
75  Matrix A(rows_, P);
76  A.setZero();
77  double invSigma = gamma ? std::sqrt(*gamma) : 1.0;
78  size_t i = 0, j = 0, n = p - 1 - d;
79  while (i < rows_) {
80  A.block(i, j, n, n) = invSigma * Matrix::Identity(n, n);
81  i += n;
82  j += n + d; // skip d columns
83  n -= 1;
84  }
85  // TODO(frank): assign the right one in the right columns
86  whitenedJacobian_ =
87  std::make_shared<JacobianFactor>(key, A, Vector::Zero(rows_));
88  }
89 
91  ~ShonanGaugeFactor() override {}
92 
94  double error(const Values &c) const override { return 0; }
95 
97  size_t dim() const override { return rows_; }
98 
100  std::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
101  return whitenedJacobian_;
102  }
103 };
104 // \ShonanGaugeFactor
105 
106 } // namespace gtsam
const gtsam::Symbol key('X', 0)
double error(const Values &c) const override
Calculate the error of the factor: always zero.
ShonanGaugeFactor(Key key, size_t p, size_t d=3, std::optional< double > gamma={})
int n
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
static size_t Dimension(size_t n)
Definition: SOn.h:209
std::shared_ptr< GaussianFactor > linearize(const Values &c) const override
linearize to a GaussianFactor
EIGEN_DEVICE_FUNC const Scalar & q
~ShonanGaugeFactor() override
Destructor.
traits
Definition: chartTesting.h:28
std::shared_ptr< JacobianFactor > whitenedJacobian_
Constant Jacobian.
Non-linear factor base classes.
float * p
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
N*N matrix representation of SO(N). N can be Eigen::Dynamic.
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
std::ptrdiff_t j
size_t dim() const override
get the dimension of the factor (number of rows on linearization)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:43