#include <GncOptimizer.h>
Public Types | |
typedef GncParameters::OptimizerType | BaseOptimizer |
For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer. More... | |
Public Member Functions | |
Vector | calculateWeights (const Values ¤tEstimate, const double mu) |
Calculate gnc weights. More... | |
bool | checkConvergence (const double mu, const Vector &weights, const double cost, const double prev_cost) const |
Check for convergence between consecutive GNC iterations. More... | |
bool | checkCostConvergence (const double cost, const double prev_cost) const |
Check convergence of relative cost differences. More... | |
bool | checkMuConvergence (const double mu) const |
Check if we have reached the value of mu for which the surrogate loss matches the original loss. More... | |
bool | checkWeightsConvergence (const Vector &weights) const |
Check convergence of weights to binary values. More... | |
bool | equals (const GncOptimizer &other, double tol=1e-9) const |
Equals. More... | |
const NonlinearFactorGraph & | getFactors () const |
Access a copy of the internal factor graph. More... | |
const Vector & | getInlierCostThresholds () const |
Get the inlier threshold. More... | |
const GncParameters & | getParams () const |
Access a copy of the parameters. More... | |
const Values & | getState () const |
Access a copy of the internal values. More... | |
const Vector & | getWeights () const |
Access a copy of the GNC weights. More... | |
GncOptimizer (const NonlinearFactorGraph &graph, const Values &initialValues, const GncParameters ¶ms=GncParameters()) | |
Constructor. More... | |
double | initializeMu () const |
Initialize the gnc parameter mu such that loss is approximately convex (remark 5 in GNC paper). More... | |
Vector | initializeWeightsFromKnownInliersAndOutliers () const |
NonlinearFactorGraph | makeWeightedGraph (const Vector &weights) const |
Create a graph where each factor is weighted by the gnc weights. More... | |
Values | optimize () |
Compute optimal solution using graduated non-convexity. More... | |
void | setInlierCostThresholds (const double inth) |
void | setInlierCostThresholds (const Vector &inthVec) |
void | setInlierCostThresholdsAtProbability (const double alpha) |
void | setWeights (const Vector w) |
double | updateMu (const double mu) const |
Update the gnc parameter mu to gradually increase nonconvexity. More... | |
Private Attributes | |
Vector | barcSq_ |
Inlier thresholds. A factor is considered an inlier if factor.error() < barcSq_[i] (where i is the position of the factor in the factor graph. Note that factor.error() whitens by the covariance. More... | |
NonlinearFactorGraph | nfg_ |
Original factor graph to be solved by GNC. More... | |
GncParameters | params_ |
GNC parameters. More... | |
Values | state_ |
Initial values to be used at each iteration by GNC. More... | |
Vector | weights_ |
Weights associated to each factor in GNC (this could be a local variable in optimize, but it is useful to make it accessible from outside). More... | |
Definition at line 44 of file GncOptimizer.h.
typedef GncParameters::OptimizerType gtsam::GncOptimizer< GncParameters >::BaseOptimizer |
For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer.
Definition at line 47 of file GncOptimizer.h.
|
inline |
Constructor.
Definition at line 58 of file GncOptimizer.h.
|
inline |
Calculate gnc weights.
Definition at line 419 of file GncOptimizer.h.
|
inline |
Check for convergence between consecutive GNC iterations.
Definition at line 389 of file GncOptimizer.h.
|
inline |
Check convergence of relative cost differences.
Definition at line 351 of file GncOptimizer.h.
|
inline |
Check if we have reached the value of mu for which the surrogate loss matches the original loss.
Definition at line 332 of file GncOptimizer.h.
|
inline |
Check convergence of weights to binary values.
Definition at line 362 of file GncOptimizer.h.
|
inline |
Equals.
Definition at line 167 of file GncOptimizer.h.
|
inline |
Access a copy of the internal factor graph.
Definition at line 152 of file GncOptimizer.h.
|
inline |
Get the inlier threshold.
Definition at line 164 of file GncOptimizer.h.
|
inline |
Access a copy of the parameters.
Definition at line 158 of file GncOptimizer.h.
|
inline |
Access a copy of the internal values.
Definition at line 155 of file GncOptimizer.h.
|
inline |
Access a copy of the GNC weights.
Definition at line 161 of file GncOptimizer.h.
|
inline |
Initialize the gnc parameter mu such that loss is approximately convex (remark 5 in GNC paper).
Definition at line 272 of file GncOptimizer.h.
|
inline |
Definition at line 174 of file GncOptimizer.h.
|
inline |
Create a graph where each factor is weighted by the gnc weights.
Definition at line 396 of file GncOptimizer.h.
|
inline |
Compute optimal solution using graduated non-convexity.
Definition at line 183 of file GncOptimizer.h.
|
inline |
Set the maximum weighted residual error for an inlier (same for all factors). For a factor in the form f(x) = 0.5 * || r(x) ||^2_Omega, the inlier threshold is the largest value of f(x) for the corresponding measurement to be considered an inlier. In other words, an inlier at x is such that 0.5 * || r(x) ||^2_Omega <= barcSq. Assuming an isotropic measurement covariance sigma^2 * Identity, the cost becomes: 0.5 * 1/sigma^2 || r(x) ||^2 <= barcSq. Hence || r(x) ||^2 <= 2 * barcSq * sigma^2.
Definition at line 115 of file GncOptimizer.h.
|
inline |
Set the maximum weighted residual error for an inlier (one for each factor). For a factor in the form f(x) = 0.5 * || r(x) ||^2_Omega, the inlier threshold is the largest value of f(x) for the corresponding measurement to be considered an inlier. In other words, an inlier at x is such that 0.5 * || r(x) ||^2_Omega <= barcSq.
Definition at line 123 of file GncOptimizer.h.
|
inline |
Set the maximum weighted residual error threshold by specifying the probability alpha that the inlier residuals are smaller than that threshold
Definition at line 130 of file GncOptimizer.h.
|
inline |
Set weights for each factor. This is typically not needed, but provides an extra interface for the user to initialize the weightst
Definition at line 142 of file GncOptimizer.h.
|
inline |
Update the gnc parameter mu to gradually increase nonconvexity.
Definition at line 317 of file GncOptimizer.h.
|
private |
Inlier thresholds. A factor is considered an inlier if factor.error() < barcSq_[i] (where i is the position of the factor in the factor graph. Note that factor.error() whitens by the covariance.
Definition at line 54 of file GncOptimizer.h.
|
private |
Original factor graph to be solved by GNC.
Definition at line 50 of file GncOptimizer.h.
|
private |
GNC parameters.
Definition at line 52 of file GncOptimizer.h.
|
private |
Initial values to be used at each iteration by GNC.
Definition at line 51 of file GncOptimizer.h.
|
private |
Weights associated to each factor in GNC (this could be a local variable in optimize, but it is useful to make it accessible from outside).
Definition at line 53 of file GncOptimizer.h.