Detailed Description

Robust Incremental Smoothing and Mapping (riSAM) is a robust variant of iSAM2 for incremental factor-graph optimization.

riSAM solves each incremental update using an efficient form of Graduated Non-Convexity to reject outliers while maintaining robustness to initialization.

Citation: Robust Incremental Smoothing and Mapping (riSAM) D. McGann and J.G. Rogers III and M. Kaess, 2023, Proc. IEEE Intl. Conf. on Robotics and Automation (ICRA)

Public Member Functions
Public Interface
	RISAM (const Parameters &params)
	Constructs an instance of the riSAM algorithm with provided configuration.
UpdateResult	update (const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const std::optional< std::set< Key > > extraGncInvolvedKeys=std::nullopt, const FactorIndices &removeFactorIndices=FactorIndices(), const std::optional< FastMap< Key, int > > &constrainedKeys=std::nullopt, const std::optional< FastList< Key > > &noRelinKeys=std::nullopt, const std::optional< FastList< Key > > &extraReelimKeys=std::nullopt, bool force_relinearize=false)
	Update Interface.
UpdateResult	update (const NonlinearFactorGraph &newFactors, const Values &newTheta, const std::optional< std::set< Key > > extraGncInvolvedKeys, const ISAM2UpdateParams &updateParams)
	Update Interface.
Values	calculateEstimate ()
	Returns the current estimate from the solver.
const NonlinearFactorGraph &	getFactorsUnsafe () const
	Returns the underlying factors of the system.
std::set< size_t >	getOutliers (double chiSquaredOutlierThreshold)
	Returns the set of measurements (identified by their factor index) that have been deemed outliers.

Static Public Member Functions
Static Helpers
template<class FACTOR_TYPE>
static auto	MakeGraduated (GraduatedFactor::RobustLoss::shared_ptr loss, GraduationScheduler::shared_ptr scheduler)
	Returns a factory that constructs FACTOR_TYPE graduated factors for riSAM, identifying them as potential outlier measurements.

Public Types
Types
typedef RISAMParams	Parameters
	Configuration parameters for the riSAM algorithm.
typedef RISAMUpdateResult	UpdateResult
	Information about a riSAM update.

Protected Member Functions
Private Interface
UpdateResult	updateRobust (const NonlinearFactorGraph &newFactors, const Values &newTheta, const std::optional< std::set< Key > > &extraGncInvolvedKeys, const ISAM2UpdateParams &updateParams)
	Performs a robust update to the system.
FactorIndices	runRobustIteration (const FactorIndices &convexFactors, std::map< FactorIndex, size_t > &muUpdateCount)
	Performs a inner loop iteration of a robust update.
void	updateConvexFactorMu (const Values &currentEstimate, const size_t fidx, std::map< FactorIndex, size_t > &muUpdateCount, FastList< Key > &convexKeys)
	Compute the new value of $\mu$ for a given graduated factor.
FactorIndices	updateConvexFactors (const FactorIndices &convexFactors) const
	Compute the new set of convex factors after a robust iteration.
FactorIndices	convexifyInvolvedFactors (const NonlinearFactorGraph &newFactors, const Values &newTheta, const std::optional< std::set< Key > > &extraGncInvolvedKeys, ISAM2UpdateParams &internalUpdateParams, UpdateResult &updateResult)
	Convexifies factors involved in the update Finds all factors inside the total affected set, and involved in the update defined by new factors.
KeySet	accumulateInvolvedKeys (const NonlinearFactorGraph &newFactors, const std::optional< std::set< Key > > &extraGncInvolvedKeys, ISAM2UpdateParams &updateParams) const
	Accumulates the keys of all vars directly involved in an update.
void	convexifyFactorIfInvolved (const FactorIndex fidx, const KeySet &involvedKeys, const KeySet &affectedKeys, const bool isBatchUpdate, std::set< FactorIndex > &convexFactors)
	Convexifies a single factor if it is involved with the update.
void	updateHousekeeping (const NonlinearFactorGraph &newFactors, const ISAM2UpdateParams &updateParams)
	Update housekeeping information for riSAM this involves:
void	augmentMu (const NonlinearFactorGraph &newFactors, const FactorIndices &newFactorIndices)
	Extend or update mu and muInit for new factors.
void	incrementMuInits ()
	Increments muInits_ for any factor recently convexified.

Protected Attributes
Fields
Parameters	params_
	Configuration parameters for the riSAM algorithm.
std::unique_ptr< ISAM2 >	solver_
	The encapsulated iSAM2 algorithm.
FastVector< std::shared_ptr< double > >	mu_
	The current control parameter values for all factors.
FastVector< std::shared_ptr< double > >	muInits_
	The current initial control parameter values for all factors.
FastSet< size_t >	factorsToCheckStatus_
	The set of GraduatedFactors that have been convexified since the last muInit_ increment These are the factors that we can increment mu's for the next time variables converge.
VariableIndex	variableIndex_
	RISAM maintains its own variable index and factors.
NonlinearFactorGraph	factors_
	The Factors for the underlying system.

Constructor & Destructor Documentation

◆ RISAM()

gtsam::RISAM::RISAM ( const Parameters & params )

inline

Constructs an instance of the riSAM algorithm with provided configuration.

Note this constructor will override some values in params.isam2Params.

Parameters

params The configuration parameters for this instance of riSAM.

Override user preferences for iSAM2 params To ensure that we update the current estimated theta_ after each intermediate GNC iteration we must use relinearizeSkip = 1 so that we allow relinearization at any step it is needed.

For each intermediate GNC iteration we update mu_ values for convex GraduatedFactors to ensure that GraduatedFactors are linearized according to these mu_ values we must turn off caching that could result in old mu_ being used.

Member Function Documentation

◆ accumulateInvolvedKeys()

KeySet gtsam::RISAM::accumulateInvolvedKeys	(	const NonlinearFactorGraph &	newFactors,
		const std::optional< std::set< Key > > &	extraGncInvolvedKeys,
		ISAM2UpdateParams &	updateParams ) const

protected

Accumulates the keys of all vars directly involved in an update.

Parameters

newFactors	The new factors for the update.
extraGncInvolvedKeys	User supplied involved keys.
updateParams	The parameters for the underlying iSAM2 update.

Returns: The set of involved factors.

◆ augmentMu()

void gtsam::RISAM::augmentMu	(	const NonlinearFactorGraph &	newFactors,
		const FactorIndices &	newFactorIndices )

protected

Extend or update mu and muInit for new factors.

Parameters

newFactors	The new factors for which to extend the containers.
newFactorIndices	the indices assigned to each new factor.

◆ convexifyFactorIfInvolved()

void gtsam::RISAM::convexifyFactorIfInvolved	(	const FactorIndex	fidx,
		const KeySet &	involvedKeys,
		const KeySet &	affectedKeys,
		const bool	isBatchUpdate,
		std::set< FactorIndex > &	convexFactors )

protected

Convexifies a single factor if it is involved with the update.

Parameters

fidx	The factor to convexify if involved.
involvedKeys	The set of keys involved with the update.
affectedKeys	The set of keys affected by the update.
isBatchUpdate	Flag indicating all factors are relinearized.
convexFactors	Accumulator for the set of all convex factors.

◆ convexifyInvolvedFactors()

FactorIndices gtsam::RISAM::convexifyInvolvedFactors	(	const NonlinearFactorGraph &	newFactors,
		const Values &	newTheta,
		const std::optional< std::set< Key > > &	extraGncInvolvedKeys,
		ISAM2UpdateParams &	internalUpdateParams,
		UpdateResult &	updateResult )

protected

Convexifies factors involved in the update Finds all factors inside the total affected set, and involved in the update defined by new factors.

Marks those factors as convex and resets mu_ to the factors current muInit_. See RISAM::update for adtl. parameter details.

Parameters

updateResult structure containing information about this update, modified by this function to fill in info about involved and affected variables as well as convexified factors.

Returns: The indices of all factors convexified in the update defined by the given parameters.

◆ getOutliers()

std::set< size_t > gtsam::RISAM::getOutliers ( double chiSquaredOutlierThreshold )

Returns the set of measurements (identified by their factor index) that have been deemed outliers.

riSAM defines a measurement as an outlier if its current residual is greater than the chi-squared threshold provided.

Parameters

chiSquaredOutlierThreshold - The chi-squared threshold used to define outliers. WARN: Potentially slow since we iterate over all factors.

Returns: The set of factors that are considered outliers by RISAM.

◆ incrementMuInits()

void gtsam::RISAM::incrementMuInits ( )

protected

Increments muInits_ for any factor recently convexified.

Used to mitigate the long-term effect of measurements that we are confident are outliers.

◆ MakeGraduated()

template<class FACTOR_TYPE>

auto gtsam::RISAM::MakeGraduated	(	GraduatedFactor::RobustLoss::shared_ptr	loss,
		GraduationScheduler::shared_ptr	scheduler )

inlinestatic

Returns a factory that constructs FACTOR_TYPE graduated factors for riSAM, identifying them as potential outlier measurements.

The graduation policy and the factor's own arguments are supplied separately, as MakeGraduated<FACTOR_TYPE>(graduation arguments)(factor arguments), so one factory can build many factors that share a graduation policy. Every constructed factor still carries its own graduation state.

Parameters

loss	The graduated robust loss applied to the factors
scheduler	The control param $\mu$ scheduler for the factors

◆ runRobustIteration()

FactorIndices gtsam::RISAM::runRobustIteration	(	const FactorIndices &	convexFactors,
		std::map< FactorIndex, size_t > &	muUpdateCount )

protected

Performs a inner loop iteration of a robust update.

Parameters

convexFactors	The set of factors that are convex for this update.
muUpdateCount	Accum. for # of updates applied to convex factors.

Returns: The set of factors that remain convex after this iteration.

◆ update() [1/2]

RISAM::UpdateResult gtsam::RISAM::update	(	const NonlinearFactorGraph &	newFactors,
		const Values &	newTheta,
		const std::optional< std::set< Key > >	extraGncInvolvedKeys,
		const ISAM2UpdateParams &	updateParams )

Update Interface.

See ISAM2 docs for details as parameters match (almost) exactly.

Parameters

extraGncInvolvedKeys - overrides internal RISAM logic and performs a robust update. All extra involved keys will be treated as being a part of the current update with respect to factor convexification.

◆ update() [2/2]

RISAM::UpdateResult gtsam::RISAM::update	(	const NonlinearFactorGraph &	newFactors = NonlinearFactorGraph(),
		const Values &	newTheta = Values(),
		const std::optional< std::set< Key > >	extraGncInvolvedKeys = std::nullopt,
		const FactorIndices &	removeFactorIndices = FactorIndices(),
		const std::optional< FastMap< Key, int > > &	constrainedKeys = std::nullopt,
		const std::optional< FastList< Key > > &	noRelinKeys = std::nullopt,
		const std::optional< FastList< Key > > &	extraReelimKeys = std::nullopt,
		bool	force_relinearize = false )

Update Interface.

See ISAM2 docs for details as parameters match (almost) exactly.

Parameters

extraGncInvolvedKeys - overrides internal RISAM logic and performs a robust update. All extra involved keys will be treated as being a part of the current update with respect to factor convexification.

◆ updateConvexFactorMu()

void gtsam::RISAM::updateConvexFactorMu	(	const Values &	currentEstimate,
		const size_t	fidx,
		std::map< FactorIndex, size_t > &	muUpdateCount,
		FastList< Key > &	convexKeys )

protected

Compute the new value of $\mu$ for a given graduated factor.

Parameters

currentEstimate	The current estimated solution.
fidx	The index of the graduated factor to update.
muUpdateCount	Accum. for # of updates applied to convex factors.
convexKeys	Accum. for all keys associated with convex factors.

Note: INVARIANT: fidx must index a Graduated Factor.

◆ updateConvexFactors()

FactorIndices gtsam::RISAM::updateConvexFactors ( const FactorIndices & convexFactors ) const

protected

Compute the new set of convex factors after a robust iteration.

Parameters

convexFactors The set of convex factors after the prev. iter.

Returns: The set of factors that are still convex after the current iter.

◆ updateHousekeeping()

void gtsam::RISAM::updateHousekeeping	(	const NonlinearFactorGraph &	newFactors,
		const ISAM2UpdateParams &	updateParams )

protected

Update housekeeping information for riSAM this involves:

Determining the indices for newFactors.
Update the riSAM fields: factors_ and variableIndex_. NOTE: they will be ahead of those in solver_ until solver.update() is called.
Update mu_ and muInit_ for the new factors.
Parameters

newFactors The new factors for the current update.

updateParams The update parameters for the current update.

◆ updateRobust()

RISAM::UpdateResult gtsam::RISAM::updateRobust	(	const NonlinearFactorGraph &	newFactors,
		const Values &	newTheta,
		const std::optional< std::set< Key > > &	extraGncInvolvedKeys,
		const ISAM2UpdateParams &	updateParams )

protected

Performs a robust update to the system.

A robust update is required any time newFactors contains GraduatedFactors (i.e. there are potentially new inlier/outlier measurements).

See also: RISAM::update for details on params.

Update until all convex factors have converged

Member Data Documentation

◆ factors_

NonlinearFactorGraph gtsam::RISAM::factors_

protected

The Factors for the underlying system.

INVARIANT: matches that of solver_ after every update.

◆ mu_

FastVector<std::shared_ptr<double> > gtsam::RISAM::mu_

protected

The current control parameter values for all factors.

If factor i is a GraduatedFactor mu_[i] is a reference to that factors internal state. If factor i is not Graduated mu_[i] is a pointer to a value of zero.

◆ muInits_

FastVector<std::shared_ptr<double> > gtsam::RISAM::muInits_

protected

The current initial control parameter values for all factors.

If factor i is not Graduated muInits_[i] is a pointer to a value of zero.

◆ variableIndex_

VariableIndex gtsam::RISAM::variableIndex_

protected

RISAM maintains its own variable index and factors.

These match exactly that of the underlying iSAM2 solver, but lead ahead of it since we update RISAM housekeeping before updating the underlying solver.

VariableIndex for the underlying system. INVARIANT: matches that of solver_ after every update.

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

/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/sam/RISAM.h
/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/sam/RISAM.cpp

newFactors	The new factors for the current update.
updateParams	The update parameters for the current update.

Detailed Description

Public Member Functions

Static Public Member Functions

Public Types

Protected Member Functions

Protected Attributes

Constructor & Destructor Documentation

◆ RISAM()

Member Function Documentation

◆ accumulateInvolvedKeys()

◆ augmentMu()

◆ convexifyFactorIfInvolved()

◆ convexifyInvolvedFactors()

◆ getOutliers()

◆ incrementMuInits()

◆ MakeGraduated()

◆ runRobustIteration()

◆ update() [1/2]

◆ update() [2/2]

◆ updateConvexFactorMu()

◆ updateConvexFactors()

◆ updateHousekeeping()

◆ updateRobust()

Member Data Documentation

◆ factors_

◆ mu_

◆ muInits_

◆ variableIndex_