#include <Optimizer.h>

Public Member Functions
void	augment_sparse_linear_system (SparseSystem &W, const Properties &prop)
void	batch_optimize (const Properties &prop, int *num_iterations)
	Optimizer (OptimizationInterface &fs)
void	relinearize (const Properties &prop)
void	update_estimate (const Properties &prop)
	~Optimizer ()
Private Member Functions
Eigen::VectorXd	compute_dog_leg (double alpha, const Eigen::VectorXd &h_sd, const Eigen::VectorXd &h_gn, double delta, double &gain_ratio_denominator)
Eigen::VectorXd	compute_gauss_newton_step (const SparseSystem &jacobian, SparseSystem *R=NULL, double lambda=0.)
void	gauss_newton (const Properties &prop, int *num_iterations=NULL)
void	levenberg_marquardt (const Properties &prop, int *num_iterations=NULL)
void	permute_vector (const Eigen::VectorXd &v, Eigen::VectorXd &p, const int *permutation)
void	powells_dog_leg (int *num_iterations=NULL, double delta0=1.0, int max_iterations=0, double epsilon1=1e-4, double epsilon2=1e-4, double epsilon3=1e-4)
bool	powells_dog_leg_update (double epsilon1, double epsilon3, SparseSystem &jacobian, Eigen::VectorXd &f_x, Eigen::VectorXd &gradient)
void	update_trust_radius (double rho, double hdl_norm)
Private Attributes
Cholesky *	_cholesky
double	current_SSE_at_linpoint
double	Delta
OptimizationInterface &	function_system
Eigen::VectorXd	gradient
Eigen::VectorXd	last_accepted_hdl

Detailed Description

Definition at line 41 of file Optimizer.h.

Constructor & Destructor Documentation

isam::Optimizer::Optimizer ( OptimizationInterface & fs ) [inline]

Definition at line 154 of file Optimizer.h.

isam::Optimizer::~Optimizer ( ) [inline]

Definition at line 218 of file Optimizer.h.

Member Function Documentation

void isam::Optimizer::augment_sparse_linear_system	(	SparseSystem &	W,
		const Properties &	prop
	)

Used to augment the sparse linear system by adding new measurements. Only useful in incremental mode.

Definition at line 192 of file Optimizer.cpp.

void isam::Optimizer::batch_optimize	(	const Properties &	prop,
		int *	num_iterations
	)

Perform batch optimization using the method set in prop

Definition at line 535 of file Optimizer.cpp.

VectorXd isam::Optimizer::compute_dog_leg	(	double	alpha,
		const Eigen::VectorXd &	h_sd,
		const Eigen::VectorXd &	h_gn,
		double	delta,
		double &	gain_ratio_denominator
	)		`[private]`

Computes and returns the dog-leg step given the parameter alpha, the trust region radius delta, and the steepest-descent and Gauss-Newton steps. Also computes and returns the value of the denominator that will be used to compute the gain ratio.

Definition at line 65 of file Optimizer.cpp.

VectorXd isam::Optimizer::compute_gauss_newton_step	(	const SparseSystem &	jacobian,
		SparseSystem *	R = `NULL`,
		double	lambda = `0.`
	)		`[private]`

Helper method for computing the Gauss-Newton step h_{gn} in Gauss-Newton, Levenberg-Marquardt, and Powell's dog-leg algorithms in batch mode. Specifically, this function can compute the Gauss-Newton step h_gn as part of the factorization of the relinearized SparseSystem computed at each iteration of batch processing algorithm.

Parameters:

jacobian	The SparseSystem representing the linearization
R
lambda

Returns:: h_gn

Definition at line 49 of file Optimizer.cpp.

void isam::Optimizer::gauss_newton	(	const Properties &	prop,
		int *	num_iterations = `NULL`
	)		`[private]`

Definition at line 283 of file Optimizer.cpp.

void isam::Optimizer::levenberg_marquardt	(	const Properties &	prop,
		int *	num_iterations = `NULL`
	)		`[private]`

Perform Levenberg-Marquardt

Parameters:

prop	Properties including stopping criteria max_iterations and epsilon, and lm_... parameters
num_iterations	Upon return, contains number of iterations performed.

Definition at line 368 of file Optimizer.cpp.

void isam::Optimizer::permute_vector	(	const Eigen::VectorXd &	v,
		Eigen::VectorXd &	p,
		const int *	permutation
	)		`[private]`

Given an input vector v and an array of ints representing a permutation, applies the permutation to the elements of v and returns the permuted vector

Parameters:

v	Input vector.
permutation	An array of ints representing the permutation to be applied to the elements of v.

Returns:: The returned permuted vector p satisfies p(permutation[i]) = v(i) i.e., p is formed by mapping the ith element of v to the permutation[i]-th element of p.

Definition at line 42 of file Optimizer.cpp.

void isam::Optimizer::powells_dog_leg	(	int *	num_iterations = `NULL`,
		double	delta0 = `1.0`,
		int	max_iterations = `0`,
		double	epsilon1 = `1e-4`,
		double	epsilon2 = `1e-4`,
		double	epsilon3 = `1e-4`
	)		`[private]`

Powell's dog leg algorithm, a trust region method that combines Gauss-Newton and steepest descent, similar to Levenberg-Marquardt, but requiring less iterations (matrix factorizations). See Manolis05iccv for a comparison to LM. Implemented according to Madsen, Nielson, Tingleff, "Methods for Non-Linear Least Squares Problems", lecture notes, Denmark, 2004 (http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf).

Parameters:

num_iterations	Contains number of iterations on return if not NULL.
delta0	Initial trust region.
max_iterations	Maximum number of iterations (0 means unlimited).
epsilon1
epsilon2
epsilon3

Definition at line 466 of file Optimizer.cpp.

bool isam::Optimizer::powells_dog_leg_update	(	double	epsilon1,
		double	epsilon3,
		SparseSystem &	jacobian,
		Eigen::VectorXd &	f_x,
		Eigen::VectorXd &	gradient
	)		`[private]`

Definition at line 183 of file Optimizer.cpp.

void isam::Optimizer::relinearize ( const Properties & prop )

Computes the Jacobian J(x_est) of the residual error function about the current estimate x_est, computes the thin QR factorization of J(x_est), and then stores (R,d) as a SparseSystem, where

Q^t f(x_est) = | d | | e |

and || e ||^2 is the squared residual error.

NOTA BENE: (Deeply) Internally, this algorithm uses the SuiteSparse library to perform the matrix decomposition shown above. The SuiteSparse library utilizes variable reordering in order to reduce fill-in and boost efficiency. Consequently, the ordering (i.e., naming) of variables in the SparseSystem computed by this function differs from the ordering (naming) of the variables in the Graph object contained in the Slam class.

The mappings between the two orderings can be obtained using OrderedSparseMatrix::a_to_r() and OrderedSparseMatrix::r_to_a(). These functions return const int*'s that point to internal arrays encoding the permutation between the naming of variables passed in to the factorization routine, and the naming of variables in the factored matrices returned by the relinearization.

More precisely, if

const int* order = function_system._R.a_to_r();

then the variable x_i from the Graph object stored the Slam class is mapped to the variable x_{order[i]} in the SparseSystem obtained after linearization.

The call

const int* inverse_order = function_system._R.r_to_a();

retrieves the inverse permutation.

Definition at line 110 of file Optimizer.cpp.

void isam::Optimizer::update_estimate ( const Properties & prop )

Updates the current estimated solution

Definition at line 225 of file Optimizer.cpp.

void isam::Optimizer::update_trust_radius	(	double	rho,
		double	hdl_norm
	)		`[private]`

Definition at line 101 of file Optimizer.cpp.

Member Data Documentation

Cholesky* isam::Optimizer::_cholesky [private]

This data member is used to compute the thin QR decomposition of the linear system during the relinearization steps.

Definition at line 55 of file Optimizer.h.

double isam::Optimizer::current_SSE_at_linpoint [private]

This keeps a running count of the sum of squared errors at the linearization point; only used with Powell's Dog-Leg in incremental mode.

Definition at line 77 of file Optimizer.h.

double isam::Optimizer::Delta [private]

Current radius of trust region; only used with Powell's Dog-Leg.

Definition at line 65 of file Optimizer.h.

OptimizationInterface& isam::Optimizer::function_system [private]

Provides an interface for manipulating the linearized system in the Slam class.

Definition at line 49 of file Optimizer.h.

Eigen::VectorXd isam::Optimizer::gradient [private]

Cached gradient vector; only used with increment Powell's Dog-Leg.

Definition at line 60 of file Optimizer.h.

Eigen::VectorXd isam::Optimizer::last_accepted_hdl [private]

Used to restore the previous estimate for cases in which the proposed step is rejected; only used with Powell's Dog-Leg in incremental mode.

Definition at line 71 of file Optimizer.h.

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

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation