#include <ShonanAveraging.h>

Public Types
using	Measurements = std::vector< BinaryMeasurement< Rot > >

using	Parameters = ShonanAveragingParameters< d >

using	Rot = typename Parameters::Rot

using	Sparse = Eigen::SparseMatrix< double >

Public Member Functions
template<typename T >
std::vector< BinaryMeasurement< T > >	maybeRobust (const std::vector< BinaryMeasurement< T >> &measurements, bool useRobustModel=false) const

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())

Query properties
size_t	nrUnknowns () const
	Return number of unknowns. More...

size_t	numberMeasurements () const
	Return number of measurements. More...

const BinaryMeasurement< Rot > &	measurement (size_t k) const
	k^th binary measurement More...

Measurements	makeNoiseModelRobust (const Measurements &measurements, double k=1.345) const

const Rot &	measured (size_t k) const
	k^th measurement, as a Rot. More...

const KeyVector &	keys (size_t k) const
	Keys for k^th measurement, as a vector of Key values. More...

Basic API
double	cost (const Values &values) const

Values	initializeRandomly (std::mt19937 &rng) const

Values	initializeRandomly () const
	Random initialization for wrapper, fixed random seed. More...

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

Private Member Functions
Sparse	buildD () const
	Build 3Nx3N sparse degree matrix D. More...

Sparse	buildQ () const

Private Attributes
Sparse	D_

Sparse	L_

Measurements	measurements_

size_t	nrUnknowns_

Parameters	parameters_

Sparse	Q_

Matrix API (advanced use, debugging)
Sparse	computeLambda (const Matrix &S) const
	Version that takes pxdN Stiefel manifold elements. More...

Matrix	computeLambda_ (const Values &values) const
	Dense versions of computeLambda for wrapper/testing. More...

Matrix	computeLambda_ (const Matrix &S) const
	Dense versions of computeLambda for wrapper/testing. More...

Sparse	computeA (const Values &values) const
	Compute A matrix whose Eigenvalues we will examine. More...

Sparse	computeA (const Matrix &S) const
	Version that takes pxdN Stiefel manifold elements. More...

Matrix	computeA_ (const Values &values) const
	Dense version of computeA for wrapper/testing. More...

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

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

Values	roundSolutionS (const Matrix &S) const
	Project pxdN Stiefel manifold matrix S to Rot3^N. More...

Matrix	riemannianGradient (size_t p, const Values &values) const
	Calculate the riemannian gradient of F(values) at values. More...

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

static Matrix	StiefelElementMatrix (const Values &values)
	Project to pxdN Stiefel manifold. More...

static VectorValues	TangentVectorValues (size_t p, const Vector &v)
	Create a VectorValues with eigenvector v_i. More...

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

Advanced API
NonlinearFactorGraph	buildGraphAt (size_t p) const

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

Values	initializeRandomlyAt (size_t p) const
	Version of initializeRandomlyAt with fixed random seed. More...

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)
	Lift Values of type T to SO(p) More...

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

Definition at line 123 of file ShonanAveraging.h.

Member Typedef Documentation

◆ Measurements

template<size_t d>

using gtsam::ShonanAveraging< d >::Measurements = std::vector<BinaryMeasurement<Rot> >

Definition at line 132 of file ShonanAveraging.h.

◆ Parameters

template<size_t d>

using gtsam::ShonanAveraging< d >::Parameters = ShonanAveragingParameters<d>

Definition at line 128 of file ShonanAveraging.h.

◆ Rot

template<size_t d>

using gtsam::ShonanAveraging< d >::Rot = typename Parameters::Rot

Definition at line 129 of file ShonanAveraging.h.

◆ Sparse

template<size_t d>

using gtsam::ShonanAveraging< d >::Sparse = Eigen::SparseMatrix<double>

Definition at line 125 of file ShonanAveraging.h.

Constructor & Destructor Documentation

◆ ShonanAveraging()

template<size_t d>

gtsam::ShonanAveraging< d >::ShonanAveraging	(	const Measurements &	measurements,
		const Parameters &	parameters = `Parameters()`
	)

Construct from set of relative measurements (given as BetweenFactor<Rot3> for now) NoiseModel must be isotropic.

Definition at line 117 of file ShonanAveraging.cpp.

Member Function Documentation

◆ buildD()

template<size_t d>

Sparse gtsam::ShonanAveraging< d >::buildD

private

Build 3Nx3N sparse degree matrix D.

Definition at line 362 of file ShonanAveraging.cpp.

◆ buildGraphAt()

template<size_t d>

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

Build graph for SO(p)

Parameters

p	the dimensionality of the rotation manifold to optimize over

Definition at line 138 of file ShonanAveraging.cpp.

◆ buildQ()

template<size_t d>

Sparse gtsam::ShonanAveraging< d >::buildQ

private

Build 3Nx3N sparse matrix consisting of rotation measurements, arranged as (i,j) and (j,i) blocks within a sparse matrix.

Definition at line 397 of file ShonanAveraging.cpp.

◆ checkOptimality()

template<size_t d>

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

Check optimality

Parameters

values should be of type SOn

Definition at line 756 of file ShonanAveraging.cpp.

◆ computeA() [1/2]

template<size_t d>

Sparse gtsam::ShonanAveraging< d >::computeA ( const Matrix & S ) const

Version that takes pxdN Stiefel manifold elements.

Definition at line 486 of file ShonanAveraging.cpp.

◆ computeA() [2/2]

template<size_t d>

Sparse gtsam::ShonanAveraging< d >::computeA ( const Values & values ) const

Compute A matrix whose Eigenvalues we will examine.

Definition at line 477 of file ShonanAveraging.cpp.

◆ computeA_()

template<size_t d>

Matrix gtsam::ShonanAveraging< d >::computeA_ ( const Values & values ) const

inline

Dense version of computeA for wrapper/testing.

Definition at line 242 of file ShonanAveraging.h.

◆ computeLambda() [1/2]

template<size_t d>

Sparse gtsam::ShonanAveraging< d >::computeLambda ( const Matrix & S ) const

Version that takes pxdN Stiefel manifold elements.

Definition at line 437 of file ShonanAveraging.cpp.

◆ computeLambda() [2/2]

template<size_t d>

Sparse gtsam::ShonanAveraging< d >::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.

Definition at line 468 of file ShonanAveraging.cpp.

◆ computeLambda_() [1/2]

template<size_t d>

Matrix gtsam::ShonanAveraging< d >::computeLambda_ ( const Matrix & S ) const

inline

Dense versions of computeLambda for wrapper/testing.

Definition at line 231 of file ShonanAveraging.h.

◆ computeLambda_() [2/2]

template<size_t d>

Matrix gtsam::ShonanAveraging< d >::computeLambda_ ( const Values & values ) const

inline

Dense versions of computeLambda for wrapper/testing.

Definition at line 226 of file ShonanAveraging.h.

◆ computeMinEigenValue()

template<size_t d>

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

Compute minimum eigenvalue for optimality check.

Parameters

values should be of type SOn

Definition at line 713 of file ShonanAveraging.cpp.

◆ 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

values should be of type SOn

Definition at line 730 of file ShonanAveraging.cpp.

◆ computeMinEigenVector()

template<size_t d>

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

Compute minimum eigenvalue for optimality check.

Parameters

values should be of type SOn

Returns: minEigenVector and minEigenValue

Definition at line 747 of file ShonanAveraging.cpp.

◆ cost()

template<size_t d>

double gtsam::ShonanAveraging< d >::cost ( const Values & values ) const

Calculate cost for SO(3) Values should be of type Rot3

Definition at line 311 of file ShonanAveraging.cpp.

◆ costAt()

template<size_t d>

double gtsam::ShonanAveraging< d >::costAt	(	size_t	p,
		const Values &	values
	)		const

Calculate cost for SO(p) Values should be of type SO(p)

Definition at line 163 of file ShonanAveraging.cpp.

◆ createOptimizerAt()

template<size_t d>

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

Try to create optimizer at SO(p)

Parameters

p	the dimensionality of the rotation manifold to optimize over
initial	initial SO(p) values

Returns: lm optimizer

Definition at line 171 of file ShonanAveraging.cpp.

◆ initializeRandomly() [1/2]

template<size_t d>

Values gtsam::ShonanAveraging< d >::initializeRandomly

Random initialization for wrapper, fixed random seed.

Definition at line 869 of file ShonanAveraging.cpp.

◆ initializeRandomly() [2/2]

template<size_t d>

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

Initialize randomly at SO(d)

Parameters

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

Definition at line 859 of file ShonanAveraging.cpp.

◆ initializeRandomlyAt() [1/2]

template<size_t d>

Values gtsam::ShonanAveraging< d >::initializeRandomlyAt ( size_t p ) const

Version of initializeRandomlyAt with fixed random seed.

Definition at line 883 of file ShonanAveraging.cpp.

◆ initializeRandomlyAt() [2/2]

template<size_t d>

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

Create initial Values of type SO(p)

Parameters

p	the dimensionality of the rotation manifold
rng	random number generator

Definition at line 875 of file ShonanAveraging.cpp.

◆ 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

values	should be of type SO(p-1)
minEigenVector	corresponding to minEigenValue at level p-1

Returns: values of type SO(p)

Definition at line 820 of file ShonanAveraging.cpp.

◆ keys()

template<size_t d>

const KeyVector& gtsam::ShonanAveraging< d >::keys ( size_t k ) const

inline

Keys for k^th measurement, as a vector of Key values.

Definition at line 209 of file ShonanAveraging.h.

◆ LiftTo()

template<size_t d>

template<class T >

static Values gtsam::ShonanAveraging< d >::LiftTo	(	size_t	p,
		const Values &	values
	)

inlinestatic

Lift Values of type T to SO(p)

Definition at line 367 of file ShonanAveraging.h.

◆ LiftwithDescent()

template<size_t d>

Values gtsam::ShonanAveraging< d >::LiftwithDescent	(	size_t	p,
		const Values &	values,
		const Vector &	minEigenVector
	)

static

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)

Definition at line 811 of file ShonanAveraging.cpp.

◆ 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

measurements	Vector of BinaryMeasurements.
k	Huber noise model threshold.

Definition at line 181 of file ShonanAveraging.h.

◆ 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

T	the type of measurement, e.g. Rot3.

Parameters

measurements	vector of BinaryMeasurements of type T.
useRobustModel	flag indicating whether use robust noise model instead.

Definition at line 418 of file ShonanAveraging.h.

◆ measured()

template<size_t d>

const Rot& gtsam::ShonanAveraging< d >::measured ( size_t k ) const

inline

k^th measurement, as a Rot.

Definition at line 206 of file ShonanAveraging.h.

◆ measurement()

template<size_t d>

const BinaryMeasurement<Rot>& gtsam::ShonanAveraging< d >::measurement ( size_t k ) const

inline

k^th binary measurement

Definition at line 171 of file ShonanAveraging.h.

◆ nrUnknowns()

template<size_t d>

size_t gtsam::ShonanAveraging< d >::nrUnknowns ( ) const

inline

Return number of unknowns.

Definition at line 165 of file ShonanAveraging.h.

◆ numberMeasurements()

template<size_t d>

size_t gtsam::ShonanAveraging< d >::numberMeasurements ( ) const

inline

Return number of measurements.

Definition at line 168 of file ShonanAveraging.h.

◆ projectFrom() [1/3]

Values gtsam::ShonanAveraging< 2 >::projectFrom	(	size_t	p,
		const Values &	values
	)		const

Definition at line 212 of file ShonanAveraging.cpp.

◆ projectFrom() [2/3]

Values gtsam::ShonanAveraging< 3 >::projectFrom	(	size_t	p,
		const Values &	values
	)		const

Definition at line 224 of file ShonanAveraging.cpp.

◆ projectFrom() [3/3]

template<size_t d>

Values gtsam::ShonanAveraging< d >::projectFrom	(	size_t	p,
		const Values &	values
	)		const

Project from SO(p) to Rot2 or Rot3 Values should be of type SO(p)

◆ riemannianGradient()

template<size_t d>

Matrix gtsam::ShonanAveraging< d >::riemannianGradient	(	size_t	p,
		const Values &	values
	)		const

Calculate the riemannian gradient of F(values) at values.

Definition at line 785 of file ShonanAveraging.cpp.

◆ roundSolution()

template<size_t d>

Values gtsam::ShonanAveraging< d >::roundSolution ( const Values & values ) const

Project from SO(p)^N to Rot2^N or Rot3^N Values should be of type SO(p)

Definition at line 302 of file ShonanAveraging.cpp.

◆ roundSolutionS() [1/3]

template<size_t d>

Values gtsam::ShonanAveraging< d >::roundSolutionS ( const Matrix & S ) const

Project pxdN Stiefel manifold matrix S to Rot3^N.

◆ roundSolutionS() [2/3]

Values gtsam::ShonanAveraging< 2 >::roundSolutionS ( const Matrix & S ) const

Definition at line 273 of file ShonanAveraging.cpp.

◆ roundSolutionS() [3/3]

Values gtsam::ShonanAveraging< 3 >::roundSolutionS ( const Matrix & S ) const

Definition at line 287 of file ShonanAveraging.cpp.

◆ 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

initial	initial Rot3 values
pMin	value of p to start Riemanian staircase at (default: d).
pMax	maximum value of p to try (default: 10)

Returns: (Rot3 values, minimum eigenvalue)

Definition at line 889 of file ShonanAveraging.cpp.

◆ StiefelElementMatrix()

template<size_t d>

Matrix gtsam::ShonanAveraging< d >::StiefelElementMatrix ( const Values & values )

static

Project to pxdN Stiefel manifold.

Definition at line 199 of file ShonanAveraging.cpp.

◆ TangentVectorValues()

template<size_t d>

VectorValues gtsam::ShonanAveraging< d >::TangentVectorValues	(	size_t	p,
		const Vector &	v
	)

static

Create a VectorValues with eigenvector v_i.

Definition at line 763 of file ShonanAveraging.cpp.

◆ tryOptimizingAt()

template<size_t d>

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

Try to optimize at SO(p)

Parameters

p	the dimensionality of the rotation manifold to optimize over
initial	initial SO(p) values

Returns: SO(p) values

Definition at line 190 of file ShonanAveraging.cpp.

Member Data Documentation

◆ D_

template<size_t d>

Sparse gtsam::ShonanAveraging< d >::D_

private

Definition at line 138 of file ShonanAveraging.h.

◆ L_

template<size_t d>

Sparse gtsam::ShonanAveraging< d >::L_

private

Definition at line 140 of file ShonanAveraging.h.

◆ measurements_

template<size_t d>

Measurements gtsam::ShonanAveraging< d >::measurements_

private

Definition at line 136 of file ShonanAveraging.h.

◆ nrUnknowns_

template<size_t d>

size_t gtsam::ShonanAveraging< d >::nrUnknowns_

private

Definition at line 137 of file ShonanAveraging.h.

◆ parameters_

template<size_t d>

Parameters gtsam::ShonanAveraging< d >::parameters_

private

Definition at line 135 of file ShonanAveraging.h.

◆ Q_

template<size_t d>

Sparse gtsam::ShonanAveraging< d >::Q_

private

Definition at line 139 of file ShonanAveraging.h.

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

Public Types

Public Member Functions

Private Member Functions

Private Attributes

Matrix API (advanced use, debugging)

Advanced API

Detailed Description

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

Member Typedef Documentation

◆ Measurements

◆ Parameters

◆ Rot

◆ Sparse

Constructor & Destructor Documentation

◆ ShonanAveraging()

Member Function Documentation

◆ buildD()

◆ buildGraphAt()

◆ buildQ()

◆ checkOptimality()

◆ computeA() [1/2]

◆ computeA() [2/2]

◆ computeA_()

◆ computeLambda() [1/2]

◆ computeLambda() [2/2]

◆ computeLambda_() [1/2]

◆ computeLambda_() [2/2]

◆ computeMinEigenValue()

◆ computeMinEigenValueAP()

◆ computeMinEigenVector()

◆ cost()

◆ costAt()

◆ createOptimizerAt()

◆ initializeRandomly() [1/2]

◆ initializeRandomly() [2/2]

◆ initializeRandomlyAt() [1/2]

◆ initializeRandomlyAt() [2/2]

◆ initializeWithDescent()

◆ keys()

◆ LiftTo()

◆ LiftwithDescent()

◆ makeNoiseModelRobust()

◆ maybeRobust()

◆ measured()

◆ measurement()

◆ nrUnknowns()

◆ numberMeasurements()

◆ projectFrom() [1/3]

◆ projectFrom() [2/3]

◆ projectFrom() [3/3]

◆ riemannianGradient()

◆ roundSolution()

◆ roundSolutionS() [1/3]

◆ roundSolutionS() [2/3]

◆ roundSolutionS() [3/3]

◆ run()

◆ StiefelElementMatrix()

◆ TangentVectorValues()

◆ tryOptimizingAt()

Member Data Documentation

◆ D_

◆ L_

◆ measurements_

◆ nrUnknowns_

◆ parameters_

◆ Q_

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