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_;

  std::shared_ptr<JacobianFactor> whitenedJacobian_;

public:
  ShonanGaugeFactor(Key key, size_t p, size_t d = 3,
                    std::optional<double> gamma = {})
      : 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_ =
        std::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_; }

  std::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
    return whitenedJacobian_;
  }
};
// \ShonanGaugeFactor

} // namespace gtsam