Loading...
Searching...
No Matches
ShonanGaugeFactor.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/geometry/SOn.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
class GTSAM_EXPORT ShonanGaugeFactor : public NonlinearFactor {
// Row dimension, equal to the dimensionality of SO(p-d)
size_t rows_;
boost::shared_ptr<JacobianFactor> whitenedJacobian_;
public:
ShonanGaugeFactor(Key key, size_t p, size_t d = 3,
boost::optional<double> gamma = boost::none)
: NonlinearFactor(KeyVector{key}) {
if (p < d) {
throw std::invalid_argument("ShonanGaugeFactor must have p>=d.");
}
// Calculate dimensions
size_t q = p - d;
size_t P = SOn::Dimension(p); // dimensionality of SO(p)
rows_ = SOn::Dimension(q); // dimensionality of SO(q), the gauge
// Create constant Jacobian as a rows_*P matrix: there are rows_ penalized
// dimensions, but it is a bit tricky to find them among the P columns.
// The key is to look at how skew-symmetric matrices are laid out in SOn.h:
// the first tangent dimension will always be included, but beyond that we
// have to be careful. We always need to skip the d top-rows of the skew-
// symmetric matrix as they below to K, part of the Stiefel manifold.
Matrix A(rows_, P);
A.setZero();
double invSigma = gamma ? std::sqrt(*gamma) : 1.0;
size_t i = 0, j = 0, n = p - 1 - d;
while (i < rows_) {
A.block(i, j, n, n) = invSigma * Matrix::Identity(n, n);
i += n;
j += n + d; // skip d columns
n -= 1;
}
// TODO(frank): assign the right one in the right columns
whitenedJacobian_ =
boost::make_shared<JacobianFactor>(key, A, Vector::Zero(rows_));
}
~ShonanGaugeFactor() override {}
double error(const Values &c) const override { return 0; }
size_t dim() const override { return rows_; }
boost::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
return whitenedJacobian_;
}
};
// \ShonanGaugeFactor
} // namespace gtsam
SOn.h
N*N matrix representation of SO(N).
JacobianFactor.h
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::NonlinearFactor::NonlinearFactor
NonlinearFactor()
Default constructor for I/O only.
Definition NonlinearFactor.h:58
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
gtsam::ShonanGaugeFactor::error
double error(const Values &c) const override
Calculate the error of the factor: always zero.
Definition ShonanGaugeFactor.h:94
gtsam::ShonanGaugeFactor::~ShonanGaugeFactor
~ShonanGaugeFactor() override
Destructor.
Definition ShonanGaugeFactor.h:91
gtsam::ShonanGaugeFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override
linearize to a GaussianFactor
Definition ShonanGaugeFactor.h:100
gtsam::ShonanGaugeFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition ShonanGaugeFactor.h:97
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:58