SysSPA2d holds a set of nodes and constraints for sparse pose adjustment. More...

#include <spa2d.h>

Public Member Functions
bool	addConstraint (int nd0, int nd1, const Vector3d &mean, const Matrix3d &prec)
int	addNode (const Vector3d &pos, int id)
double	calcCost (bool tcost=false)
double	calcCost (double dist)
	calculate error assuming outliers > dist
int	doSPA (int niter, double sLambda=1.0e-4, int useCSparse=SBA_SPARSE_CHOLESKY, double initTol=1.0e-8, int CGiters=50)
int	doSPAwindowed (int window, int niter, double sLambda, int useCSparse)
void	getGraph (std::vector< float > &graph)
std::vector< Node2d, Eigen::aligned_allocator < Node2d > >	getNodes ()
void	printStats ()
	print some system stats
bool	removeConstraint (int ndi0, int ndi1)
void	removeNode (int id)
void	setupSparseSys (double sLambda, int iter, int sparseType)
void	setupSys (double sLambda)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	SysSPA2d ()
	constructor
void	useCholmod (bool yes)
	use CHOLMOD or CSparse
void	writeSparseA (char *fname, bool useCSparse=false)
Public Attributes
Eigen::MatrixXd	A
	linear system matrix and vector
Eigen::VectorXd	B
CSparse2d	csp
	sparse matrix object
double	errcost
double	lambda
int	nFixed
	Number of fixed nodes.
std::vector< Node2d, Eigen::aligned_allocator < Node2d > >	nodes
	set of nodes (camera frames) for SPA system, indexed by position;
std::vector< Con2dP2, Eigen::aligned_allocator < Con2dP2 > >	p2cons
	Set of P2 constraints.
bool	print_iros_stats
std::vector< std::vector < Eigen::Vector2d, Eigen::aligned_allocator < Eigen::Vector2d > > >	scans
	set of point scans, corresponding to nodes
double	sqMinDelta
	Convergence bound (square of minimum acceptable delta change)
bool	verbose
	if we want statistics

Detailed Description

SysSPA2d holds a set of nodes and constraints for sparse pose adjustment.

Definition at line 187 of file spa2d.h.

Constructor & Destructor Documentation

EIGEN_MAKE_ALIGNED_OPERATOR_NEW SysSPA2d::SysSPA2d ( ) [inline]

constructor

Definition at line 193 of file spa2d.h.

Member Function Documentation

bool SysSPA2d::addConstraint	(	int	nd0,
		int	nd1,
		const Vector3d &	mean,
		const Matrix3d &	prec
	)

Definition at line 231 of file spa2d.cpp.

int SysSPA2d::addNode	(	const Vector3d &	pos,
		int	id
	)

add a node at a pose <pos> is x,y,th, with th in radians

Definition at line 208 of file spa2d.cpp.

double SysSPA2d::calcCost ( bool tcost = false )

calculate the error in the system; if <tcost> is true, just the distance error without weighting

Definition at line 174 of file spa2d.cpp.

double SysSPA2d::calcCost ( double dist )

calculate error assuming outliers > dist

int SysSPA2d::doSPA	(	int	niter,
		double	sLambda = `1.0e-4`,
		int	useCSparse = `SBA_SPARSE_CHOLESKY`,
		double	initTol = `1.0e-8`,
		int	maxCGiters = `50`
	)

Run the LM algorithm that computes a nonlinear SPA estimate. <niter> is the max number of iterations to perform; returns the number actually performed. <lambda> is the diagonal augmentation for LM. <useCSparse> = 0 for dense Cholesky, 1 for sparse system, 2 for gradient system, 3 for block jacobian PCG <initTol> is the initial tolerance for CG <maxCGiters> is max # of iterations in BPCG

Definition at line 434 of file spa2d.cpp.

int SysSPA2d::doSPAwindowed	(	int	window,
		int	niter,
		double	sLambda,
		int	useCSparse
	)

Definition at line 650 of file spa2d.cpp.

void SysSPA2d::getGraph ( std::vector< float > & graph )

return the graph of constraints x,y -> x',y' 4 floats per connection

Definition at line 993 of file spa2d.cpp.

std::vector<Node2d,Eigen::aligned_allocator<Node2d> > SysSPA2d::getNodes ( ) [inline]

Definition at line 210 of file spa2d.h.

void SysSPA2d::printStats ( )

print some system stats

bool SysSPA2d::removeConstraint	(	int	ndi0,
		int	ndi1
	)

void SysSPA2d::removeNode ( int id )

void SysSPA2d::setupSparseSys	(	double	sLambda,
		int	iter,
		int	sparseType
	)

Definition at line 329 of file spa2d.cpp.

void SysSPA2d::setupSys ( double sLambda )

set up linear system, from RSS submission (konolige 2010) <sLambda> is the diagonal augmentation for the LM step

Definition at line 259 of file spa2d.cpp.

void SysSPA2d::useCholmod ( bool yes ) [inline]

use CHOLMOD or CSparse

Definition at line 265 of file spa2d.h.

void SysSPA2d::writeSparseA	(	char *	fname,
		bool	useCSparse = `false`
	)

Member Data Documentation

Eigen::MatrixXd SysSPA2d::A

linear system matrix and vector

Definition at line 258 of file spa2d.h.

Eigen::VectorXd SysSPA2d::B

Definition at line 259 of file spa2d.h.

CSparse2d SysSPA2d::csp

sparse matrix object

Definition at line 262 of file spa2d.h.

double SysSPA2d::errcost

Definition at line 227 of file spa2d.h.

double SysSPA2d::lambda

do LM solution for system; returns number of iterations on finish. Argument is max number of iterations to perform, initial diagonal augmentation, and sparse form of Cholesky. useCSParse = 0 for dense Cholesky, 1 for sparse Cholesky, 2 for BPCG

Definition at line 243 of file spa2d.h.

int SysSPA2d::nFixed

Number of fixed nodes.

Definition at line 217 of file spa2d.h.

std::vector<Node2d,Eigen::aligned_allocator<Node2d> > SysSPA2d::nodes

set of nodes (camera frames) for SPA system, indexed by position;

Definition at line 209 of file spa2d.h.

std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> > SysSPA2d::p2cons

Set of P2 constraints.

Definition at line 220 of file spa2d.h.

bool SysSPA2d::print_iros_stats

Definition at line 270 of file spa2d.h.

std::vector< std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > > SysSPA2d::scans

set of point scans, corresponding to nodes

Definition at line 214 of file spa2d.h.

double SysSPA2d::sqMinDelta

Convergence bound (square of minimum acceptable delta change)

Definition at line 255 of file spa2d.h.

bool SysSPA2d::verbose

if we want statistics

Definition at line 269 of file spa2d.h.

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

Public Member Functions

Public Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation