Loading...
Searching...
No Matches
GncParams.h
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, 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/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
namespace gtsam {
/* ************************************************************************* */
enum GncLossType {
GM /*Geman McClure*/,
TLS /*Truncated least squares*/
};
template<class BaseOptimizerParameters>
class GncParams {
public:
typedef typename BaseOptimizerParameters::OptimizerType OptimizerType;
enum Verbosity {
SILENT = 0,
SUMMARY,
MU,
WEIGHTS,
VALUES
};
GncParams(const BaseOptimizerParameters& baseOptimizerParams)
: baseOptimizerParams(baseOptimizerParams) {
}
GncParams()
: baseOptimizerParams() {
}
BaseOptimizerParameters baseOptimizerParams;
GncLossType lossType = TLS;
size_t maxIterations = 100;
double muStep = 1.4;
double relativeCostTol = 1e-5;
double weightsTol = 1e-4;
Verbosity verbosity = SILENT;
//TODO(Varun) replace IndexVector with vector<size_t> once pybind11/stl.h is globally enabled.
using IndexVector = FastVector<uint64_t>;
IndexVector knownInliers = IndexVector();
IndexVector knownOutliers = IndexVector();
void setLossType(const GncLossType type) {
lossType = type;
}
void setMaxIterations(const size_t maxIter) {
std::cout
<< "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! "
<< std::endl;
maxIterations = maxIter;
}
void setMuStep(const double step) {
muStep = step;
}
void setRelativeCostTol(double value) {
relativeCostTol = value;
}
void setWeightsTol(double value) {
weightsTol = value;
}
void setVerbosityGNC(const Verbosity value) {
verbosity = value;
}
void setKnownInliers(const IndexVector& knownIn) {
for (size_t i = 0; i < knownIn.size(); i++){
knownInliers.push_back(knownIn[i]);
}
std::sort(knownInliers.begin(), knownInliers.end());
}
void setKnownOutliers(const IndexVector& knownOut) {
for (size_t i = 0; i < knownOut.size(); i++){
knownOutliers.push_back(knownOut[i]);
}
std::sort(knownOutliers.begin(), knownOutliers.end());
}
bool equals(const GncParams& other, double tol = 1e-9) const {
return baseOptimizerParams.equals(other.baseOptimizerParams)
&& lossType == other.lossType && maxIterations == other.maxIterations
&& std::fabs(muStep - other.muStep) <= tol
&& verbosity == other.verbosity && knownInliers == other.knownInliers
&& knownOutliers == other.knownOutliers;
}
void print(const std::string& str) const {
std::cout << str << "\n";
switch (lossType) {
case GM:
std::cout << "lossType: Geman McClure" << "\n";
break;
case TLS:
std::cout << "lossType: Truncated Least-squares" << "\n";
break;
default:
throw std::runtime_error("GncParams::print: unknown loss type.");
}
std::cout << "maxIterations: " << maxIterations << "\n";
std::cout << "muStep: " << muStep << "\n";
std::cout << "relativeCostTol: " << relativeCostTol << "\n";
std::cout << "weightsTol: " << weightsTol << "\n";
std::cout << "verbosity: " << verbosity << "\n";
for (size_t i = 0; i < knownInliers.size(); i++)
std::cout << "knownInliers: " << knownInliers[i] << "\n";
for (size_t i = 0; i < knownOutliers.size(); i++)
std::cout << "knownOutliers: " << knownOutliers[i] << "\n";
baseOptimizerParams.print("Base optimizer params: ");
}
};
}
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
GaussNewtonOptimizer.h
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:33
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::GncLossType
GncLossType
Choice of robust loss function for GNC.
Definition GncParams.h:36
gtsam::GncParams::baseOptimizerParams
BaseOptimizerParameters baseOptimizerParams
GNC parameters.
Definition GncParams.h:67
gtsam::GncParams::knownInliers
IndexVector knownInliers
Slots in the factor graph corresponding to measurements that we know are outliers.
Definition GncParams.h:80
gtsam::GncParams::GncParams
GncParams()
Default constructor.
Definition GncParams.h:62
gtsam::GncParams::OptimizerType
BaseOptimizerParameters::OptimizerType OptimizerType
For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimi...
Definition GncParams.h:45
gtsam::GncParams::setKnownInliers
void setKnownInliers(const IndexVector &knownIn)
(Optional) Provide a vector of measurements that must be considered inliers.
Definition GncParams.h:123
gtsam::GncParams::print
void print(const std::string &str) const
Print.
Definition GncParams.h:151
gtsam::GncParams::IndexVector
FastVector< uint64_t > IndexVector
Use IndexVector for inliers and outliers since it is fast + wrapping.
Definition GncParams.h:78
gtsam::GncParams::GncParams
GncParams(const BaseOptimizerParameters &baseOptimizerParams)
Constructor.
Definition GncParams.h:57
gtsam::GncParams::muStep
double muStep
Multiplicative factor to reduce/increase the mu in gnc.
Definition GncParams.h:71
gtsam::GncParams::weightsTol
double weightsTol
If the weights are within weightsTol from being binary, stop iterating (only for TLS).
Definition GncParams.h:73
gtsam::GncParams::setKnownOutliers
void setKnownOutliers(const IndexVector &knownOut)
(Optional) Provide a vector of measurements that must be considered outliers.
Definition GncParams.h:134
gtsam::GncParams::verbosity
Verbosity verbosity
Verbosity level.
Definition GncParams.h:74
gtsam::GncParams::equals
bool equals(const GncParams &other, double tol=1e-9) const
Equals.
Definition GncParams.h:142
gtsam::GncParams::setRelativeCostTol
void setRelativeCostTol(double value)
Set the maximum relative difference in mu values to stop iterating.
Definition GncParams.h:103
gtsam::GncParams::lossType
GncLossType lossType
any other specific GNC parameters:
Definition GncParams.h:69
gtsam::GncParams::maxIterations
size_t maxIterations
Maximum number of iterations.
Definition GncParams.h:70
gtsam::GncParams::setLossType
void setLossType(const GncLossType type)
Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
Definition GncParams.h:85
gtsam::GncParams::setMuStep
void setMuStep(const double step)
Set the graduated non-convexity step: at each GNC iteration, mu is updated as mu <- mu * muStep.
Definition GncParams.h:98
gtsam::GncParams::relativeCostTol
double relativeCostTol
If relative cost change is below this threshold, stop iterating.
Definition GncParams.h:72
gtsam::GncParams::setVerbosityGNC
void setVerbosityGNC(const Verbosity value)
Set the verbosity level.
Definition GncParams.h:113
gtsam::GncParams::setMaxIterations
void setMaxIterations(const size_t maxIter)
Set the maximum number of iterations in GNC (changing the max nr of iters might lead to less accurate...
Definition GncParams.h:90
gtsam::GncParams::Verbosity
Verbosity
Verbosity levels.
Definition GncParams.h:48
gtsam::GncParams::setWeightsTol
void setWeightsTol(double value)
Set the maximum difference between the weights and their rounding in {0,1} to stop iterating.
Definition GncParams.h:108