gtsam 4.1.1
gtsam
gtsam::ShonanAveraging< d > Class Template Reference

Detailed Description

template<size_t d>
class gtsam::ShonanAveraging< d >

Class that implements Shonan Averaging from our ECCV'20 paper.

Note: The "basic" API uses all Rot values (Rot2 or Rot3, depending on value of d), whereas the different levels and "advanced" API at SO(p) needs Values of type SOn<Dynamic>.

The template parameter d can be 2 or 3. Both are specialized in the .cpp file.

If you use this code in your work, please consider citing our paper: Shonan Rotation Averaging, Global Optimality by Surfing SO(p)^n Frank Dellaert, David M. Rosen, Jing Wu, Robert Mahony, and Luca Carlone, European Computer Vision Conference, 2020. You can view our ECCV spotlight video at https://youtu.be/5ppaqMyHtE0

Matrix API (advanced use, debugging)

Sparse D () const
 Sparse version of D.
 
Matrix denseD () const
 Dense version of D.
 
Sparse Q () const
 Sparse version of Q.
 
Matrix denseQ () const
 Dense version of Q.
 
Sparse L () const
 Sparse version of L.
 
Matrix denseL () const
 Dense version of L.
 
Sparse computeLambda (const Matrix &S) const
 Version that takes pxdN Stiefel manifold elements.
 
Matrix computeLambda_ (const Values &values) const
 Dense versions of computeLambda for wrapper/testing.
 
Matrix computeLambda_ (const Matrix &S) const
 Dense versions of computeLambda for wrapper/testing.
 
Sparse computeA (const Values &values) const
 Compute A matrix whose Eigenvalues we will examine.
 
Sparse computeA (const Matrix &S) const
 Version that takes pxdN Stiefel manifold elements.
 
Matrix computeA_ (const Values &values) const
 Dense version of computeA for wrapper/testing.
 
double computeMinEigenValue (const Values &values, Vector *minEigenVector=nullptr) const
 Compute minimum eigenvalue for optimality check. More...
 
double computeMinEigenValueAP (const Values &values, Vector *minEigenVector=nullptr) const
 Compute minimum eigenvalue with accelerated power method. More...
 
Values roundSolutionS (const Matrix &S) const
 Project pxdN Stiefel manifold matrix S to Rot3^N.
 
Matrix riemannianGradient (size_t p, const Values &values) const
 Calculate the riemannian gradient of F(values) at values.
 
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, computed from the minimum eigenvector at p-1. More...
 
static Matrix StiefelElementMatrix (const Values &values)
 Project to pxdN Stiefel manifold.
 
static VectorValues TangentVectorValues (size_t p, const Vector &v)
 Create a VectorValues with eigenvector v_i.
 
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 return new values in type SO(p)
 

Advanced API

NonlinearFactorGraph buildGraphAt (size_t p) const
 Build graph for SO(p) More...
 
Values initializeRandomlyAt (size_t p, std::mt19937 &rng) const
 Create initial Values of type SO(p) More...
 
Values initializeRandomlyAt (size_t p) const
 Version of initializeRandomlyAt with fixed random seed.
 
double costAt (size_t p, const Values &values) const
 Calculate cost for SO(p) Values should be of type SO(p)
 
Sparse computeLambda (const Values &values) const
 Given an estimated local minimum Yopt for the (possibly lifted) relaxation, this function computes and returns the block-diagonal elements of the corresponding Lagrange multiplier.
 
std::pair< double, Vector > computeMinEigenVector (const Values &values) const
 Compute minimum eigenvalue for optimality check. More...
 
bool checkOptimality (const Values &values) const
 Check optimality. More...
 
boost::shared_ptr< LevenbergMarquardtOptimizercreateOptimizerAt (size_t p, const Values &initial) const
 Try to create optimizer at SO(p) More...
 
Values tryOptimizingAt (size_t p, const Values &initial) const
 Try to optimize at SO(p) More...
 
Values projectFrom (size_t p, const Values &values) const
 Project from SO(p) to Rot2 or Rot3 Values should be of type SO(p)
 
Values roundSolution (const Values &values) const
 Project from SO(p)^N to Rot2^N or Rot3^N Values should be of type SO(p)
 
template<class T >
static Values LiftTo (size_t p, const Values &values)
 Lift Values of type T to SO(p)
 

Public Member Functions

template<typename T >
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. More...
 
Values projectFrom (size_t p, const Values &values) const
 
Values projectFrom (size_t p, const Values &values) const
 
Values roundSolutionS (const Matrix &S) const
 
Values roundSolutionS (const Matrix &S) const
 
Standard Constructors
 ShonanAveraging (const Measurements &measurements, const Parameters &parameters=Parameters())
 Construct from set of relative measurements (given as BetweenFactor<Rot3> for now) NoiseModel must be isotropic.
 
Query properties
size_t nrUnknowns () const
 Return number of unknowns.
 
size_t nrMeasurements () const
 Return number of measurements.
 
const BinaryMeasurement< Rot > & measurement (size_t k) const
 k^th binary measurement
 
Measurements makeNoiseModelRobust (const Measurements &measurements, double k=1.345) const
 Update factors to use robust Huber loss. More...
 
const Rot & measured (size_t k) const
 k^th measurement, as a Rot.
 
const KeyVectorkeys (size_t k) const
 Keys for k^th measurement, as a vector of Key values.
 
Basic API
double cost (const Values &values) const
 Calculate cost for SO(3) Values should be of type Rot3.
 
Values initializeRandomly (std::mt19937 &rng) const
 Initialize randomly at SO(d) More...
 
Values initializeRandomly () const
 Random initialization for wrapper, fixed random seed.
 
std::pair< Values, double > run (const Values &initialEstimate, size_t pMin=d, size_t pMax=10) const
 Optimize at different values of p until convergence. More...
 

Public Types

using Sparse = Eigen::SparseMatrix< double >
 
using Parameters = ShonanAveragingParameters< d >
 
using Rot = typename Parameters::Rot
 
using Measurements = std::vector< BinaryMeasurement< Rot > >
 

Member Function Documentation

◆ buildGraphAt()

template<size_t d>
NonlinearFactorGraph gtsam::ShonanAveraging< d >::buildGraphAt ( size_t  p) const

Build graph for SO(p)

Parameters
pthe dimensionality of the rotation manifold to optimize over

◆ checkOptimality()

template<size_t d>
bool gtsam::ShonanAveraging< d >::checkOptimality ( const Values values) const

Check optimality.

Parameters
valuesshould be of type SOn

◆ computeMinEigenValue()

template<size_t d>
double gtsam::ShonanAveraging< d >::computeMinEigenValue ( const Values values,
Vector *  minEigenVector = nullptr 
) const

Compute minimum eigenvalue for optimality check.

Parameters
valuesshould be of type SOn

◆ computeMinEigenValueAP()

template<size_t d>
double gtsam::ShonanAveraging< d >::computeMinEigenValueAP ( const Values values,
Vector *  minEigenVector = nullptr 
) const

Compute minimum eigenvalue with accelerated power method.

Parameters
valuesshould be of type SOn

◆ computeMinEigenVector()

template<size_t d>
std::pair< double, Vector > gtsam::ShonanAveraging< d >::computeMinEigenVector ( const Values values) const

Compute minimum eigenvalue for optimality check.

Parameters
valuesshould be of type SOn
Returns
minEigenVector and minEigenValue

◆ createOptimizerAt()

template<size_t d>
boost::shared_ptr< LevenbergMarquardtOptimizer > gtsam::ShonanAveraging< d >::createOptimizerAt ( size_t  p,
const Values initial 
) const

Try to create optimizer at SO(p)

Parameters
pthe dimensionality of the rotation manifold to optimize over
initialinitial SO(p) values
Returns
lm optimizer

◆ initializeRandomly()

template<size_t d>
Values gtsam::ShonanAveraging< d >::initializeRandomly ( std::mt19937 &  rng) const

Initialize randomly at SO(d)

Parameters
rngrandom number generator Example: std::mt19937 rng(42); Values initial = initializeRandomly(rng, p);

◆ initializeRandomlyAt()

template<size_t d>
Values gtsam::ShonanAveraging< d >::initializeRandomlyAt ( size_t  p,
std::mt19937 &  rng 
) const

Create initial Values of type SO(p)

Parameters
pthe dimensionality of the rotation manifold

◆ initializeWithDescent()

template<size_t d>
Values gtsam::ShonanAveraging< d >::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, computed from the minimum eigenvector at p-1.

Parameters
valuesshould be of type SO(p-1)
minEigenVectorcorresponding to minEigenValue at level p-1
Returns
values of type SO(p)

◆ makeNoiseModelRobust()

template<size_t d>
Measurements gtsam::ShonanAveraging< d >::makeNoiseModelRobust ( const Measurements &  measurements,
double  k = 1.345 
) const
inline

Update factors to use robust Huber loss.

Parameters
measurementsVector of BinaryMeasurements.
kHuber noise model threshold.

◆ maybeRobust()

template<size_t d>
template<typename T >
std::vector< BinaryMeasurement< T > > gtsam::ShonanAveraging< d >::maybeRobust ( const std::vector< BinaryMeasurement< T > > &  measurements,
bool  useRobustModel = false 
) const
inline

Helper function to convert measurements to robust noise model if flag is set.

Template Parameters
Tthe type of measurement, e.g. Rot3.
Parameters
measurementsvector of BinaryMeasurements of type T.
useRobustModelflag indicating whether use robust noise model instead.

◆ run()

template<size_t d>
std::pair< Values, double > gtsam::ShonanAveraging< d >::run ( const Values initialEstimate,
size_t  pMin = d,
size_t  pMax = 10 
) const

Optimize at different values of p until convergence.

Parameters
initialinitial Rot3 values
pMinvalue of p to start Riemanian staircase at (default: d).
pMaxmaximum value of p to try (default: 10)
Returns
(Rot3 values, minimum eigenvalue)

◆ tryOptimizingAt()

template<size_t d>
Values gtsam::ShonanAveraging< d >::tryOptimizingAt ( size_t  p,
const Values initial 
) const

Try to optimize at SO(p)

Parameters
pthe dimensionality of the rotation manifold to optimize over
initialinitial SO(p) values
Returns
SO(p) values

The documentation for this class was generated from the following files: