Loading...
Searching...
No Matches
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 =
boost::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;
boost::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
Vector.h
typedef and functions to augment Eigen's VectorXd
Matrix.h
typedef and functions to augment Eigen's MatrixXd
Rot3.h
3D rotation represented as a rotation matrix or quaternion
Rot2.h
2D rotation
VectorValues.h
Factor Graph Values.
AcceleratedPowerMethod.h
accelerated power method for fast eigenvalue and eigenvector computation
PowerMethod.h
Power method for fast eigenvalue and eigenvector computation.
LevenbergMarquardtParams.h
Parameters for Levenberg-Marquardt trust-region scheme.
BinaryMeasurement.h
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
dataset.h
utility functions for loading datasets
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::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::SO< Eigen::Dynamic >::Lift
static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R)
Definition SOn.h:101
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
gtsam::LevenbergMarquardtOptimizer
This class performs Levenberg-Marquardt nonlinear optimization.
Definition LevenbergMarquardtOptimizer.h:35
gtsam::LevenbergMarquardtParams
Parameters for Levenberg-Marquardt optimization.
Definition LevenbergMarquardtParams.h:35
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
gtsam::Values::extract
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
Extract a subset of values of the given type ValueType.
Definition Values-inl.h:94
gtsam::Values::insert
void insert(Key j, const Value &val)
Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present.
Definition Values.cpp:157
gtsam::BinaryMeasurement
Definition BinaryMeasurement.h:36
gtsam::ShonanAveragingParameters
Parameters governing optimization etc.
Definition ShonanAveraging.h:46
gtsam::ShonanAveragingParameters< 2 >::alpha
double alpha
Definition ShonanAveraging.h:55
gtsam::ShonanAveragingParameters::print
void print(const std::string &s="") const
Print the parameters and flags used for rotation averaging.
Definition ShonanAveraging.h:94
gtsam::ShonanAveragingParameters< 2 >::lm
LevenbergMarquardtParams lm
Definition ShonanAveraging.h:52
gtsam::ShonanAveragingParameters< 2 >::useHuber
bool useHuber
Definition ShonanAveraging.h:59
gtsam::ShonanAveragingParameters< 2 >::optimalityThreshold
double optimalityThreshold
Definition ShonanAveraging.h:53
gtsam::ShonanAveragingParameters< 2 >::beta
double beta
Definition ShonanAveraging.h:56
gtsam::ShonanAveragingParameters< 2 >::gamma
double gamma
Definition ShonanAveraging.h:57
gtsam::ShonanAveragingParameters< 2 >::anchor
Anchor anchor
Definition ShonanAveraging.h:54
gtsam::ShonanAveragingParameters< 2 >::certifyOptimality
bool certifyOptimality
Definition ShonanAveraging.h:61
gtsam::ShonanAveraging::createOptimizerAt
boost::shared_ptr< LevenbergMarquardtOptimizer > createOptimizerAt(size_t p, const Values &initial) const
Try to create optimizer at SO(p).
Definition ShonanAveraging.cpp:176
gtsam::ShonanAveraging::D
Sparse D() const
Sparse version of D.
Definition ShonanAveraging.h:215
gtsam::ShonanAveraging::computeMinEigenVector
std::pair< double, Vector > computeMinEigenVector(const Values &values) const
Compute minimum eigenvalue for optimality check.
Definition ShonanAveraging.cpp:756
gtsam::ShonanAveraging::measurement
const BinaryMeasurement< Rot > & measurement(size_t k) const
k^th binary measurement
Definition ShonanAveraging.h:171
gtsam::ShonanAveraging::makeNoiseModelRobust
Measurements makeNoiseModelRobust(const Measurements &measurements, double k=1.345) const
Update factors to use robust Huber loss.
Definition ShonanAveraging.h:181
gtsam::ShonanAveraging::roundSolution
Values roundSolution(const Values &values) const
Project from SO(p)^N to Rot2^N or Rot3^N Values should be of type SO(p).
Definition ShonanAveraging.cpp:309
gtsam::ShonanAveraging::roundSolutionS
Values roundSolutionS(const Matrix &S) const
Project pxdN Stiefel manifold matrix S to Rot3^N.
gtsam::ShonanAveraging::maybeRobust
std::vector< BinaryMeasurement< T > > maybeRobust(const std::vector< BinaryMeasurement< T > > &measurements, bool useRobustModel=false) const
Helper function to convert measurements to robust noise model if flag is set.
Definition ShonanAveraging.h:418
gtsam::ShonanAveraging::initializeWithDescent
Values initializeWithDescent(size_t p, const Values &values, const Vector &minEigenVector, double minEigenValue, double gradienTolerance=1e-2, double preconditionedGradNormTolerance=1e-4) const
Given some values at p-1, return new values at p, by doing a line search along the descent direction,...
Definition ShonanAveraging.cpp:829
gtsam::ShonanAveraging::L
Sparse L() const
Sparse version of L.
Definition ShonanAveraging.h:219
gtsam::ShonanAveraging::TangentVectorValues
static VectorValues TangentVectorValues(size_t p, const Vector &v)
Create a VectorValues with eigenvector v_i.
Definition ShonanAveraging.cpp:772
gtsam::ShonanAveraging::measured
const Rot & measured(size_t k) const
k^th measurement, as a Rot.
Definition ShonanAveraging.h:206
gtsam::ShonanAveraging::LiftwithDescent
static Values LiftwithDescent(size_t p, const Values &values, const Vector &minEigenVector)
Lift up the dimension of values in type SO(p-1) with descent direction provided by minEigenVector and...
Definition ShonanAveraging.cpp:820
gtsam::ShonanAveraging::Q
Sparse Q() const
Sparse version of Q.
Definition ShonanAveraging.h:217
gtsam::ShonanAveraging::denseD
Matrix denseD() const
Dense version of D.
Definition ShonanAveraging.h:216
gtsam::ShonanAveraging::nrUnknowns
size_t nrUnknowns() const
Return number of unknowns.
Definition ShonanAveraging.h:165
gtsam::ShonanAveraging::projectFrom
Values projectFrom(size_t p, const Values &values) const
Project from SO(p) to Rot2 or Rot3 Values should be of type SO(p).
gtsam::ShonanAveraging::numberMeasurements
size_t numberMeasurements() const
Return number of measurements.
Definition ShonanAveraging.h:168
gtsam::ShonanAveraging::denseL
Matrix denseL() const
Dense version of L.
Definition ShonanAveraging.h:220
gtsam::ShonanAveraging::computeLambda_
Matrix computeLambda_(const Matrix &S) const
Dense versions of computeLambda for wrapper/testing.
Definition ShonanAveraging.h:231
gtsam::ShonanAveraging::computeLambda
Sparse computeLambda(const Matrix &S) const
Version that takes pxdN Stiefel manifold elements.
Definition ShonanAveraging.cpp:444
gtsam::ShonanAveraging::denseQ
Matrix denseQ() const
Dense version of Q.
Definition ShonanAveraging.h:218
gtsam::ShonanAveraging::costAt
double costAt(size_t p, const Values &values) const
Calculate cost for SO(p) Values should be of type SO(p).
Definition ShonanAveraging.cpp:168
gtsam::ShonanAveraging::keys
const KeyVector & keys(size_t k) const
Keys for k^th measurement, as a vector of Key values.
Definition ShonanAveraging.h:209
gtsam::ShonanAveraging::ShonanAveraging
ShonanAveraging(const Measurements &measurements, const Parameters &parameters=Parameters())
Construct from set of relative measurements (given as BetweenFactor<Rot3> for now) NoiseModel must be...
Definition ShonanAveraging.cpp:122
gtsam::ShonanAveraging::checkOptimality
bool checkOptimality(const Values &values) const
Check optimality.
Definition ShonanAveraging.cpp:765
gtsam::ShonanAveraging::computeA
Sparse computeA(const Values &values) const
Compute A matrix whose Eigenvalues we will examine.
Definition ShonanAveraging.cpp:484
gtsam::ShonanAveraging::tryOptimizingAt
Values tryOptimizingAt(size_t p, const Values &initial) const
Try to optimize at SO(p).
Definition ShonanAveraging.cpp:197
gtsam::ShonanAveraging::computeLambda_
Matrix computeLambda_(const Values &values) const
Dense versions of computeLambda for wrapper/testing.
Definition ShonanAveraging.h:226
gtsam::ShonanAveraging::initializeRandomlyAt
Values initializeRandomlyAt(size_t p, std::mt19937 &rng) const
Create initial Values of type SO(p).
Definition ShonanAveraging.cpp:884
gtsam::ShonanAveraging::riemannianGradient
Matrix riemannianGradient(size_t p, const Values &values) const
Calculate the riemannian gradient of F(values) at values.
Definition ShonanAveraging.cpp:794
gtsam::ShonanAveraging::computeA_
Matrix computeA_(const Values &values) const
Dense version of computeA for wrapper/testing.
Definition ShonanAveraging.h:242
gtsam::ShonanAveraging::buildGraphAt
NonlinearFactorGraph buildGraphAt(size_t p) const
Build graph for SO(p).
Definition ShonanAveraging.cpp:143
gtsam::ShonanAveraging::LiftTo
static Values LiftTo(size_t p, const Values &values)
Lift Values of type T to SO(p).
Definition ShonanAveraging.h:367