gtsam 4.1.1 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
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
9
10 * -------------------------------------------------------------------------- */
11
19#pragma once
20
21#include <gtsam/geometry/SOn.h>
24
25#include <boost/assign/list_of.hpp>
26
27namespace gtsam {
47class 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
54public:
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 ~ShonanGaugeFactor() override {}
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
N*N matrix representation of SO(N).
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:63
The ShonanGaugeFactor creates a constraint on a single SO(n) to avoid moving in the stabilizer.
Definition: ShonanGaugeFactor.h:47
double error(const Values &c) const override
Calculate the error of the factor: always zero.
Definition: ShonanGaugeFactor.h:96
~ShonanGaugeFactor() override
Destructor.
Definition: ShonanGaugeFactor.h:93
boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override
linearize to a GaussianFactor
Definition: ShonanGaugeFactor.h:102
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition: ShonanGaugeFactor.h:99
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