gtsam::DCSAM Class Reference

Detailed Description

Class which implements Discrete-Continuous Smoothing And Mapping, as detailed in Doherty22ral (https://arxiv.org/abs/2204.11936).

Performs hybrid optimization by alternatively optimizing for continous and discrete variables.

Public Member Functions
	DCSAM (const ISAM2Params &isam_params)
void	update (const HybridNonlinearFactorGraph &graph, const HybridValues &initialGuess=HybridValues())
	For this solver, runs an iteration of alternating minimization between discrete and continuous variables, adding any user-supplied factors (with initial guess) first.
void	update (const HybridNonlinearFactorGraph &graph, const DiscreteValues &initialGuessDiscrete)
	Inline convenience function to allow "skipping" the initial guess for continuous variables while adding an initial guess for discrete variables.
void	update ()
	Simply used to call update without any new factors.
HybridValues	calculateEstimate () const
	This is the primary function used to extract an estimate from the solver.
const VectorValues &	getDelta () const
	Access the current delta, computed during the last call to update.
double	error (const VectorValues &x) const
	Compute the linear error using the underlying solver.
const DiscreteFactorGraph &	getDiscreteFactorGraph () const
	Used to obtain the marginals from the solver.
const NonlinearFactorGraph &	getNonlinearFactorGraph () const

Protected Member Functions
void	updateDiscrete (const DiscreteFactorGraph &dfg=DiscreteFactorGraph(), const DiscreteValues &discreteVals=DiscreteValues())
	Add factors in dfg to member discrete factor graph dfg_, then optimize for most probable explanation and update the current discrete estimate.
void	updateContinuous (const NonlinearFactorGraph &newFactors, const Values &initialGuess)
	Given the latest discrete values (currDiscrete_), a set of new factors (newFactors), and an initial guess for any new keys (initialGuess), this method.
DiscreteValues	solveDiscrete () const
	Solve for discrete variables given continuous variables.

Member Function Documentation

calculateEstimate()

HybridValues gtsam::DCSAM::calculateEstimate

(

)

const

This is the primary function used to extract an estimate from the solver.

Internally, calls isam_.calculateEstimate() and dfg_.optimize() to obtain an estimate for the continuous (resp. discrete) variables and packages them into a HybridValues.

Returns

a HybridValues object containing an estimate of the most probable assignment to the continuous (HybridValues.nonlinear) and discrete (HybridValues.discrete) variables.

getDiscreteFactorGraph()

const DiscreteFactorGraph & gtsam::DCSAM::getDiscreteFactorGraph ( ) const

inline

Used to obtain the marginals from the solver.

NOTE: not obviously correct (see DCSAM.cpp implementation) at the moment. Should perhaps retrieve the marginals for the factor graph obtained as isam_.getFactorsUnsafe() and dfg_ rather than taking as a parameter? I think this was originally intended to mimic the gtsam Marginals class.

Parameters

graph
continuousEst
dfg

solveDiscrete()

DiscreteValues gtsam::DCSAM::solveDiscrete ( ) const

protected

Solve for discrete variables given continuous variables.

Internally, this method computes DiscreteBoundaryFactors from the hybrid factors using the current continuous estimate currContinuous_, and then calls dfg_.optimize() to get the most probable estimate.

Returns

An assignment (DiscreteValues) to the discrete variables in the graph.

update() [1/2]

void gtsam::DCSAM::update

(

)

Simply used to call update without any new factors.

Runs an iteration of optimization.

update() [2/2]

void gtsam::DCSAM::update	(	const HybridNonlinearFactorGraph &	graph,
		const HybridValues &	initialGuess = HybridValues() )

For this solver, runs an iteration of alternating minimization between discrete and continuous variables, adding any user-supplied factors (with initial guess) first.

1. Adds new discrete factors (if any) as supplied by a user to the discrete factor graph, then adds any discrete-continuous factors to the discrete factor graph, appropriately initializing their continuous variables to those from the last solve and any supplied by the initial guess.
2. Update the solution for the discrete variables.
3. For all new discrete-continuous factors to be passed to the continuous solver, update/set the latest discrete variables (prior to adding).
4. In one step: add new factors, new values, and earmarked old factor keys to iSAM. Specifically, loop over DC factors already in iSAM, updating their discrete information, then call isam_.update() with the (initialized) new DC factors, any new continuous factors, and the initial guess to be supplied.
5. Calculate the latest continuous variables from iSAM.
6. Update the discrete factors in the discrete factor graph dfg_ with the latest information from the continuous solve.

Parameters

graph	- a HybridNonlinearFactorGraph containing all factors to add.
initialGuessContinuous	- an initial guess for any new continuous keys that appear in the updated factors (or if one wants to force override previously obtained continuous values).
initialGuessDiscrete	- Initial guess for discrete variables.

updateContinuous()

void gtsam::DCSAM::updateContinuous ( const NonlinearFactorGraph & newFactors, const Values & initialGuess )

protected

Given the latest discrete values (currDiscrete_), a set of new factors (newFactors), and an initial guess for any new keys (initialGuess), this method.

1. Selects the appropriate continuous factors from the hybridfactors
2. Marks any affected keys as such
3. Calls isam_.update with the new factors and initial guess.

See implementation for more detail.

updateDiscrete()

void gtsam::DCSAM::updateDiscrete ( const DiscreteFactorGraph & dfg = DiscreteFactorGraph(), const DiscreteValues & discreteVals = DiscreteValues() )

protected

Add factors in dfg to member discrete factor graph dfg_, then optimize for most probable explanation and update the current discrete estimate.

Parameters

dfg	- A discrete factor graph containing the factors to add
discreteValues	- An assignment to the continuous variables (or subset thereof).

---

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

• /tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/hybrid/DCSAM.h
• /tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/hybrid/DCSAM.cpp