Public Member Functions | Private Attributes | List of all members
gtsam::ShonanGaugeFactor Class Reference

#include <ShonanGaugeFactor.h>

Inheritance diagram for gtsam::ShonanGaugeFactor:
Inheritance graph
[legend]

Public Member Functions

size_t dim () const override
 get the dimension of the factor (number of rows on linearization) More...
 
double error (const Values &c) const override
 Calculate the error of the factor: always zero. More...
 
std::shared_ptr< GaussianFactorlinearize (const Values &c) const override
 linearize to a GaussianFactor More...
 
 ShonanGaugeFactor (Key key, size_t p, size_t d=3, std::optional< double > gamma={})
 
 ~ShonanGaugeFactor () override
 Destructor. More...
 
- Public Member Functions inherited from gtsam::NonlinearFactor
 NonlinearFactor ()
 
template<typename CONTAINER >
 NonlinearFactor (const CONTAINER &keys)
 
void print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
 
virtual bool equals (const NonlinearFactor &f, double tol=1e-9) const
 
double error (const HybridValues &c) const override
 
virtual bool active (const Values &) const
 
virtual shared_ptr clone () const
 
virtual shared_ptr rekey (const std::map< Key, Key > &rekey_mapping) const
 
virtual shared_ptr rekey (const KeyVector &new_keys) const
 
virtual bool sendable () const
 
- Public Member Functions inherited from gtsam::Factor
virtual ~Factor ()=default
 Default destructor. More...
 
bool empty () const
 Whether the factor is empty (involves zero variables). More...
 
Key front () const
 First key. More...
 
Key back () const
 Last key. More...
 
const_iterator find (Key key) const
 find More...
 
const KeyVectorkeys () const
 Access the factor's involved variable keys. More...
 
const_iterator begin () const
 
const_iterator end () const
 
size_t size () const
 
virtual void printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
 print only keys More...
 
bool equals (const This &other, double tol=1e-9) const
 check equality More...
 
KeyVectorkeys ()
 
iterator begin ()
 
iterator end ()
 

Private Attributes

size_t rows_
 
std::shared_ptr< JacobianFactorwhitenedJacobian_
 Constant Jacobian. More...
 

Additional Inherited Members

- Public Types inherited from gtsam::NonlinearFactor
typedef std::shared_ptr< Thisshared_ptr
 
- Public Types inherited from gtsam::Factor
typedef KeyVector::const_iterator const_iterator
 Const iterator over keys. More...
 
typedef KeyVector::iterator iterator
 Iterator over keys. More...
 
- Protected Types inherited from gtsam::NonlinearFactor
typedef Factor Base
 
typedef NonlinearFactor This
 
- Protected Member Functions inherited from gtsam::Factor
 Factor ()
 
template<typename CONTAINER >
 Factor (const CONTAINER &keys)
 
template<typename ITERATOR >
 Factor (ITERATOR first, ITERATOR last)
 
- Static Protected Member Functions inherited from gtsam::Factor
template<typename CONTAINER >
static Factor FromKeys (const CONTAINER &keys)
 
template<typename ITERATOR >
static Factor FromIterators (ITERATOR first, ITERATOR last)
 
- Protected Attributes inherited from gtsam::Factor
KeyVector keys_
 The keys involved in this factor. More...
 

Detailed Description

The ShonanGaugeFactor creates a constraint on a single SO(n) to avoid moving in the stabilizer.

Details: SO(p) contains the p*3 Stiefel matrices of orthogonal frames: we take those to be the 3 columns on the left. The P*P skew-symmetric matrices associated with so(p) can be partitioned as (Appendix B in the ECCV paper): | [w] -K' | | K [g] | where w is the SO(3) space, K are the extra Stiefel diemnsions (wormhole!) and (g)amma are extra dimensions in SO(p) that do not modify the cost function. The latter corresponds to rotations SO(p-d), and so the first few values of p-d for d==3 with their corresponding dimensionality are {0:0, 1:0, 2:1, 3:3, ...}

The ShonanGaugeFactor adds a unit Jacobian to these extra dimensions, essentially restricting the optimization to the Stiefel manifold.

Definition at line 45 of file ShonanGaugeFactor.h.

Constructor & Destructor Documentation

◆ ShonanGaugeFactor()

gtsam::ShonanGaugeFactor::ShonanGaugeFactor ( Key  key,
size_t  p,
size_t  d = 3,
std::optional< double >  gamma = {} 
)
inline

Construct from key for an SO(p) matrix, for base dimension d (2 or 3) If parameter gamma is given, it acts as a precision = 1/sigma^2, and the Jacobian will be multiplied with 1/sigma = sqrt(gamma).

Definition at line 58 of file ShonanGaugeFactor.h.

◆ ~ShonanGaugeFactor()

gtsam::ShonanGaugeFactor::~ShonanGaugeFactor ( )
inlineoverride

Destructor.

Definition at line 91 of file ShonanGaugeFactor.h.

Member Function Documentation

◆ dim()

size_t gtsam::ShonanGaugeFactor::dim ( ) const
inlineoverridevirtual

get the dimension of the factor (number of rows on linearization)

Implements gtsam::NonlinearFactor.

Definition at line 97 of file ShonanGaugeFactor.h.

◆ error()

double gtsam::ShonanGaugeFactor::error ( const Values c) const
inlineoverridevirtual

Calculate the error of the factor: always zero.

Reimplemented from gtsam::NonlinearFactor.

Definition at line 94 of file ShonanGaugeFactor.h.

◆ linearize()

std::shared_ptr<GaussianFactor> gtsam::ShonanGaugeFactor::linearize ( const Values c) const
inlineoverridevirtual

linearize to a GaussianFactor

Implements gtsam::NonlinearFactor.

Definition at line 100 of file ShonanGaugeFactor.h.

Member Data Documentation

◆ rows_

size_t gtsam::ShonanGaugeFactor::rows_
private

Definition at line 47 of file ShonanGaugeFactor.h.

◆ whitenedJacobian_

std::shared_ptr<JacobianFactor> gtsam::ShonanGaugeFactor::whitenedJacobian_
private

Constant Jacobian.

Definition at line 50 of file ShonanGaugeFactor.h.


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


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:47:09