|
|
| ShonanAveraging3 (const Measurements &measurements, const Parameters ¶meters=Parameters()) |
|
| ShonanAveraging3 (std::string g2oFile, const Parameters ¶meters=Parameters()) |
|
| ShonanAveraging3 (const BetweenFactorPose3s &factors, const Parameters ¶meters=Parameters()) |
|
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 |
| 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.
|
|
| ShonanAveraging (const Measurements &measurements, const Parameters ¶meters=Parameters()) |
| | Construct from set of relative measurements (given as BetweenFactor<Rot3> for now) NoiseModel must be isotropic.
|
|
size_t | nrUnknowns () const |
| | Return number of unknowns.
|
|
size_t | numberMeasurements () 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.
|
|
const Rot & | measured (size_t k) const |
| | k^th measurement, as a Rot.
|
|
const KeyVector & | keys (size_t k) const |
| | Keys for k^th measurement, as a vector of Key values.
|
|
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).
|
| 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.
|
|
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.
|
|
Sparse | computeA (const Values &values) const |
| | Compute A matrix whose Eigenvalues we will examine.
|
|
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.
|
| double | computeMinEigenValueAP (const Values &values, Vector *minEigenVector=nullptr) const |
| | Compute minimum eigenvalue with accelerated power method.
|
|
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.
|
| NonlinearFactorGraph | buildGraphAt (size_t p) const |
| | Build graph for SO(p).
|
| Values | initializeRandomlyAt (size_t p, std::mt19937 &rng) const |
| | Create initial Values of type SO(p).
|
|
double | costAt (size_t p, const Values &values) const |
| | Calculate cost for SO(p) Values should be of type SO(p).
|
| std::pair< double, Vector > | computeMinEigenVector (const Values &values) const |
| | Compute minimum eigenvalue for optimality check.
|
| bool | checkOptimality (const Values &values) const |
| | Check optimality.
|
| boost::shared_ptr< LevenbergMarquardtOptimizer > | createOptimizerAt (size_t p, const Values &initial) const |
| | Try to create optimizer at SO(p).
|
| Values | tryOptimizingAt (size_t p, const Values &initial) const |
| | Try to optimize at 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).
|