RISAM - Robust Incremental Smoothing and Mapping

Overview¶

The RISAM class in GTSAM is designed to perform online robust optimization using an incrementalized version of Graduated Non-Convexity built on the ISAM2 algorithm. This method is intended for scenarios where the incremental optimization problem is affected by outliers. In cases where all measurements are known to be inliers RISAM functions identically to ISAM2, however, when potential outliers are incorporated RISAM applies an incremental GNC step to the effected problem improving robustness over standard ISAM2 but preventing sensitivity to initialization found in M-Estimation approaches.

Like the GNCOptimizer, RISAM leverages a robust cost function $\rho(e)$ , where $e$ is the error term. The goal is to minimize the sum of these robust costs over all measurements:

\min_x \sum_i \rho(e_i(x))

(1)

Unlike the batch setting that GNCOptimzier is used for, RISAM targets the incremental problem where we incrementally incorporate measurements online.

\min_{x^t} \sum_i \rho(e_i(x^t))

(2)

Where we re-solve only a small subproblem at each step that is affected by the new measurements. RISAM solves this sub-problem robustly by solving a continuation of problems defined by graduated robust kernel $\rho(e, \mu)$ where the control parameter $\mu$ smoothly transitions the kernel from quadratic ( $x^2$ ) to a robust loss.

\rho(e^t, \mu)

(3)

By starting with non-robust error RISAM better handles poor-initialization, and by transitioning to the final robust loss RISAM removes the influence of outliers.

Authors: Frank Dellaert, et al. (see THANKS for the full author list)

See LICENSE for the license information

try:
    import google.colab
    %pip install --quiet gtsam-develop
except ImportError:
    pass

Key features:¶

Online Robust Optimization: RISAM is designed to support incremental optimization problems with outliers, using a robust cost function that can mitigate their effects.
Incremental Graduated Non-Convexity: This technique allows the optimizer to solve each incremental subproblem with a convex problem and gradually transform it into the original non-convex problem, which helps in avoiding local minima.

Key Methods + Classes¶

RISAM is designed to be a drop-in replacement for ISAM2. To see details on its key methods see ISAM2.

Additional Key Methods:

getOutliers: Returns the set of measurements currently classified as outliers.

Additional Key Helpers + Classes

MakeGraduated: Constructs any factor as a GraduatedFactor which identifies factors as possible outliers to RISAM. All other factors are treated as known inliers. It is curried as MakeGraduated<FactorType>(loss, scheduler)(factor arguments...), which keeps the graduation arguments separate from the factor’s.
noiseModel::mEstimator::GemanMcClure: The suggested robust loss function to use with riSAM.
- The authors further suggest to use noiseModel::mEstimator::GemanMcClure::GradScheme::SCALE_INVARIANT with riSAM.
GraduationScheduler: Class that defines the graduation schedule for riSAM.
- The default configuration is setup to work with GemanMcClure loss with SCALE_INVARIANT graduation.

Parameters¶

The RISAM::Parameters class defines parameters specific to RISAM:

Parameter	Type	Default Value	Description
isam2Params	ISAM2Params	ISAM2Params()	The parameters for the encapsulated ISAM2 optimizer. It is recommended to use DogLegLineSearch for optimization.
incrementOutlierMu	bool	true	Whether to increment the initial value of $\mu$ used for each incremental GNC update over time as certainty of their inlier/outlier status increases.
outlierMuChiSquaredUpperBound	double	0.95	The $\chi^2$ threshold for factor residual to consider it an outlier for $\mu_{init}$ updates..
outlierMuChiSquaredLowerBound	double	0.25	The $\chi^2$ threshold for factor residual to consider it strong inlier for $\mu_{init}$ updates.
outlierMuAverageVariableConvergenceThreshold	double	0.01	The threshold average variable delta to initiate $\mu_{init}$ updates.
numberExtraIterations	size_t	1	The number of extra `ISAM2::updates` called internally at each iteration after converging to the fully robust problem.

Usage Considerations¶

Outlier Rejection: RISAM is particularly effective in online scenarios with significant outlier presence, such as online SLAM.
Trust Region Optimization: While RISAM can use any underlying optimization step methods supported by ISAM2 it is strongly recommended to use DoglegLineSearch as it accounts for the changes in problem structure (convexity changes) and prevents divergence through trust region steps.