gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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
25namespace gtsam {
45class GTSAM_EXPORT ShonanGaugeFactor : public NonlinearFactor {
46 // Row dimension, equal to the dimensionality of SO(p-d)
47 size_t rows_;
48
50 boost::shared_ptr<JacobianFactor> whitenedJacobian_;
51
52public:
58 ShonanGaugeFactor(Key key, size_t p, size_t d = 3,
59 boost::optional<double> gamma = boost::none)
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 boost::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 boost::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
101 return whitenedJacobian_;
102 }
103};
104// \ShonanGaugeFactor
105
106} // 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
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
Nonlinear factor base class.
Definition NonlinearFactor.h:42
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
The ShonanGaugeFactor creates a constraint on a single SO(n) to avoid moving in the stabilizer.
Definition ShonanGaugeFactor.h:45
double error(const Values &c) const override
Calculate the error of the factor: always zero.
Definition ShonanGaugeFactor.h:94
~ShonanGaugeFactor() override
Destructor.
Definition ShonanGaugeFactor.h:91
boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override
linearize to a GaussianFactor
Definition ShonanGaugeFactor.h:100
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition ShonanGaugeFactor.h:97
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:58