gtsam  4.1.0
gtsam
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 #include <boost/assign/list_of.hpp>
26 
27 namespace gtsam {
47 class GTSAM_EXPORT ShonanGaugeFactor : public NonlinearFactor {
48  // Row dimension, equal to the dimensionality of SO(p-d)
49  size_t rows_;
50 
52  boost::shared_ptr<JacobianFactor> whitenedJacobian_;
53 
54 public:
60  ShonanGaugeFactor(Key key, size_t p, size_t d = 3,
61  boost::optional<double> gamma = boost::none)
62  : NonlinearFactor(boost::assign::cref_list_of<1>(key)) {
63  if (p < d) {
64  throw std::invalid_argument("ShonanGaugeFactor must have p>=d.");
65  }
66  // Calculate dimensions
67  size_t q = p - d;
68  size_t P = SOn::Dimension(p); // dimensionality of SO(p)
69  rows_ = SOn::Dimension(q); // dimensionality of SO(q), the gauge
70 
71  // Create constant Jacobian as a rows_*P matrix: there are rows_ penalized
72  // dimensions, but it is a bit tricky to find them among the P columns.
73  // The key is to look at how skew-symmetric matrices are laid out in SOn.h:
74  // the first tangent dimension will always be included, but beyond that we
75  // have to be careful. We always need to skip the d top-rows of the skew-
76  // symmetric matrix as they below to K, part of the Stiefel manifold.
77  Matrix A(rows_, P);
78  A.setZero();
79  double invSigma = gamma ? std::sqrt(*gamma) : 1.0;
80  size_t i = 0, j = 0, n = p - 1 - d;
81  while (i < rows_) {
82  A.block(i, j, n, n) = invSigma * Matrix::Identity(n, n);
83  i += n;
84  j += n + d; // skip d columns
85  n -= 1;
86  }
87  // TODO(frank): assign the right one in the right columns
88  whitenedJacobian_ =
89  boost::make_shared<JacobianFactor>(key, A, Vector::Zero(rows_));
90  }
91 
93  virtual ~ShonanGaugeFactor() {}
94 
96  double error(const Values &c) const override { return 0; }
97 
99  size_t dim() const override { return rows_; }
100 
102  boost::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
103  return whitenedJacobian_;
104  }
105 };
106 // \ShonanGaugeFactor
107 
108 } // namespace gtsam
SOn.h
N*N matrix representation of SO(N).
JacobianFactor.h
gtsam::ShonanGaugeFactor
The ShonanGaugeFactor creates a constraint on a single SO(n) to avoid moving in the stabilizer.
Definition: ShonanGaugeFactor.h:47
gtsam::ShonanGaugeFactor::error
double error(const Values &c) const override
Calculate the error of the factor: always zero.
Definition: ShonanGaugeFactor.h:96
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::ShonanGaugeFactor::~ShonanGaugeFactor
virtual ~ShonanGaugeFactor()
Destructor.
Definition: ShonanGaugeFactor.h:93
gtsam::ShonanGaugeFactor::ShonanGaugeFactor
ShonanGaugeFactor(Key key, size_t p, size_t d=3, boost::optional< double > gamma=boost::none)
Construct from key for an SO(p) matrix, for base dimension d (2 or 3) If parameter gamma is given,...
Definition: ShonanGaugeFactor.h:60
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::ShonanGaugeFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition: ShonanGaugeFactor.h:99
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::ShonanGaugeFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override
linearize to a GaussianFactor
Definition: ShonanGaugeFactor.h:102