ShonanAveraging.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/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/dllexport.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
#include <gtsam/sfm/BinaryMeasurement.h>
#include <gtsam/linear/PowerMethod.h>
#include <gtsam/linear/AcceleratedPowerMethod.h>
#include <gtsam/slam/dataset.h>

#include <Eigen/Sparse>
#include <map>
#include <string>
#include <type_traits>
#include <utility>
#include <vector>

namespace gtsam {
class NonlinearFactorGraph;
class LevenbergMarquardtOptimizer;

template <size_t d>
struct GTSAM_EXPORT ShonanAveragingParameters {
  // Select Rot2 or Rot3 interface based template parameter d
  using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
  using Anchor = std::pair<size_t, Rot>;

  // Parameters themselves:
  LevenbergMarquardtParams lm;  
  double optimalityThreshold;   
  Anchor anchor;                
  double alpha;                 
  double beta;                  
  double gamma;                 
  bool useHuber;
  bool certifyOptimality;

  ShonanAveragingParameters(const LevenbergMarquardtParams &lm =
                                LevenbergMarquardtParams::CeresDefaults(),
                            const std::string &method = "JACOBI",
                            double optimalityThreshold = -1e-4,
                            double alpha = 0.0, double beta = 1.0,
                            double gamma = 0.0);

  LevenbergMarquardtParams getLMParams() const { return lm; }

  void setOptimalityThreshold(double value) { optimalityThreshold = value; }
  double getOptimalityThreshold() const { return optimalityThreshold; }

  void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; }
  std::pair<size_t, Rot> getAnchor() const { return anchor; }

  void setAnchorWeight(double value) { alpha = value; }
  double getAnchorWeight() const { return alpha; }

  void setKarcherWeight(double value) { beta = value; }
  double getKarcherWeight() const { return beta; }

  void setGaugesWeight(double value) { gamma = value; }
  double getGaugesWeight() const { return gamma; }

  void setUseHuber(bool value) { useHuber = value; }
  bool getUseHuber() const { return useHuber; }

  void setCertifyOptimality(bool value) { certifyOptimality = value; }
  bool getCertifyOptimality() const { return certifyOptimality; }

  void print(const std::string &s = "") const {
    std::cout << (s.empty() ? s : s + " ");
    std::cout << " ShonanAveragingParameters: " << std::endl;
    std::cout << " alpha: " << alpha << std::endl;
    std::cout << " beta: " << beta << std::endl;
    std::cout << " gamma: " << gamma << std::endl;
    std::cout << " useHuber: " << useHuber << std::endl;
  }
};

using ShonanAveragingParameters2 = ShonanAveragingParameters<2>;
using ShonanAveragingParameters3 = ShonanAveragingParameters<3>;

template <size_t d>
class GTSAM_EXPORT ShonanAveraging {
 public:
  using Sparse = Eigen::SparseMatrix<double>;

  // Define the Parameters type and use its typedef of the rotation type:
  using Parameters = ShonanAveragingParameters<d>;
  using Rot = typename Parameters::Rot;

  // We store SO(d) BetweenFactors to get noise model
  using Measurements = std::vector<BinaryMeasurement<Rot>>;

 private:
  Parameters parameters_;
  Measurements measurements_;
  size_t nrUnknowns_;
  Sparse D_;  // Sparse (diagonal) degree matrix
  Sparse Q_;  // Sparse measurement matrix, == \tilde{R} in Eriksson18cvpr
  Sparse L_;  // connection Laplacian L = D - Q, needed for optimality check

  Sparse buildQ() const;

  Sparse buildD() const;

 public:

  ShonanAveraging(const Measurements &measurements,
                  const Parameters &parameters = Parameters());


  size_t nrUnknowns() const { return nrUnknowns_; }

  size_t numberMeasurements() const { return measurements_.size(); }

  const BinaryMeasurement<Rot> &measurement(size_t k) const {
    return measurements_[k];
  }

  Measurements makeNoiseModelRobust(const Measurements &measurements,
                                    double k = 1.345) const {
    Measurements robustMeasurements;
    for (auto &measurement : measurements) {
      auto model = measurement.noiseModel();
      const auto &robust =
          std::dynamic_pointer_cast<noiseModel::Robust>(model);

      SharedNoiseModel robust_model;
      // Check if the noise model is already robust
      if (robust) {
        robust_model = model;
      } else {
        // make robust
        robust_model = noiseModel::Robust::Create(
            noiseModel::mEstimator::Huber::Create(k), model);
      }
      BinaryMeasurement<Rot> meas(measurement.key1(), measurement.key2(),
                                  measurement.measured(), robust_model);
      robustMeasurements.push_back(meas);
    }
    return robustMeasurements;
  }

  const Rot &measured(size_t k) const { return measurements_[k].measured(); }

  const KeyVector &keys(size_t k) const { return measurements_[k].keys(); }


  Sparse D() const { return D_; }               
  Matrix denseD() const { return Matrix(D_); }  
  Sparse Q() const { return Q_; }               
  Matrix denseQ() const { return Matrix(Q_); }  
  Sparse L() const { return L_; }               
  Matrix denseL() const { return Matrix(L_); }  

  Sparse computeLambda(const Matrix &S) const;

  Matrix computeLambda_(const Values &values) const {
    return Matrix(computeLambda(values));
  }

  Matrix computeLambda_(const Matrix &S) const {
    return Matrix(computeLambda(S));
  }

  Sparse computeA(const Values &values) const;

  Sparse computeA(const Matrix &S) const;

  Matrix computeA_(const Values &values) const {
    return Matrix(computeA(values));
  }

  static Matrix StiefelElementMatrix(const Values &values);

  double computeMinEigenValue(const Values &values,
                              Vector *minEigenVector = nullptr) const;

  double computeMinEigenValueAP(const Values &values,
                                Vector *minEigenVector = nullptr) const;

  Values roundSolutionS(const Matrix &S) const;

  static VectorValues TangentVectorValues(size_t p, const Vector &v);

  Matrix riemannianGradient(size_t p, const Values &values) const;

  static Values LiftwithDescent(size_t p, const Values &values,
                                const Vector &minEigenVector);

  Values initializeWithDescent(
      size_t p, const Values &values, const Vector &minEigenVector,
      double minEigenValue, double gradienTolerance = 1e-2,
      double preconditionedGradNormTolerance = 1e-4) const;

  NonlinearFactorGraph buildGraphAt(size_t p) const;

  Values initializeRandomlyAt(size_t p, std::mt19937 &rng) const;

  Values initializeRandomlyAt(size_t p) const;

  double costAt(size_t p, const Values &values) const;

  Sparse computeLambda(const Values &values) const;

  std::pair<double, Vector> computeMinEigenVector(const Values &values) const;

  bool checkOptimality(const Values &values) const;

  std::shared_ptr<LevenbergMarquardtOptimizer> createOptimizerAt(
      size_t p, const Values &initial) const;

  Values tryOptimizingAt(size_t p, const Values &initial) const;

  Values projectFrom(size_t p, const Values &values) const;

  Values roundSolution(const Values &values) const;

  template <class T>
  static Values LiftTo(size_t p, const Values &values) {
    Values result;
    for (const auto it : values.extract<T>()) {
      result.insert(it.first, SOn::Lift(p, it.second.matrix()));
    }
    return result;
  }


  double cost(const Values &values) const;

  Values initializeRandomly(std::mt19937 &rng) const;

  Values initializeRandomly() const;

  std::pair<Values, double> run(const Values &initialEstimate, size_t pMin = d,
                                size_t pMax = 10) const;

  template <typename T>
  inline std::vector<BinaryMeasurement<T>> maybeRobust(
      const std::vector<BinaryMeasurement<T>> &measurements,
      bool useRobustModel = false) const {
    return useRobustModel ? makeNoiseModelRobust(measurements) : measurements;
  }
};

// Subclasses for d=2 and d=3 that explicitly instantiate, as well as provide a
// convenience interface with file access.

class GTSAM_EXPORT ShonanAveraging2 : public ShonanAveraging<2> {
 public:
  ShonanAveraging2(const Measurements &measurements,
                   const Parameters &parameters = Parameters());
  explicit ShonanAveraging2(std::string g2oFile,
                            const Parameters &parameters = Parameters());
  ShonanAveraging2(const BetweenFactorPose2s &factors,
                   const Parameters &parameters = Parameters());
};

class GTSAM_EXPORT ShonanAveraging3 : public ShonanAveraging<3> {
 public:
  ShonanAveraging3(const Measurements &measurements,
                   const Parameters &parameters = Parameters());
  explicit ShonanAveraging3(std::string g2oFile,
                            const Parameters &parameters = Parameters());

  // TODO(frank): Deprecate after we land pybind wrapper
  ShonanAveraging3(const BetweenFactorPose3s &factors,
                   const Parameters &parameters = Parameters());
};
}  // namespace gtsam