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

#include <sba.h>

Public Member Functions
bool	addConstraint (int nd0, int nd1, Eigen::Vector3d &tmean, Eigen::Quaterniond &qpmean, Eigen::Matrix< double, 6, 6 > &prec)
	Adds a pose constraint to the system. More...

void	addConstraint (int pr, int p0, int p1, Eigen::Matrix< double, 12, 1 > &mean, Eigen::Matrix< double, 12, 12 > &prec)

int	addNode (Eigen::Matrix< double, 4, 1 > &trans, Eigen::Quaternion< double > &qrot, bool isFixed=false)
	Adds a node to the system. More...

double	calcCost (bool tcost=false)

double	calcCost (double dist)
	calculate error assuming outliers > dist More...

int	doSPA (int niter, double sLambda=1.0e-4, int useCSparse=SBA_SPARSE_CHOLESKY, double initTol=1.0e-8, int CGiters=50)

void	printStats ()
	print some system stats More...

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

void	setupSys (double sLambda)

void	spanningTree (int node=0)
	Spanning tree initialization. More...

EIGEN_MAKE_ALIGNED_OPERATOR_NEW	SysSPA ()
	constructor More...

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

Public Attributes
Eigen::MatrixXd	A
	linear system matrix and vector More...

Eigen::VectorXd	B

CSparse	csp
	sparse matrix object More...

double	lambda

int	nFixed
	Number of fixed nodes. More...

std::vector< Node, Eigen::aligned_allocator< Node > >	nodes
	set of nodes (camera frames) for SPA system, indexed by position; More...

Eigen::Matrix< double, 4, 1 >	oldqrot

Eigen::Matrix< double, 4, 1 >	oldtrans

std::vector< ConP2, Eigen::aligned_allocator< ConP2 > >	p2cons
	Set of P2 constraints. More...

std::vector< double >	scales
	set of scale for SPA system, indexed by position; More...

std::vector< ConScale, Eigen::aligned_allocator< ConScale > >	scons
	Set of scale constraints. More...

double	sqMinDelta
	Convergence bound (square of minimum acceptable delta change) More...

bool	useLocalAngles
	local or global angle coordinates More...

bool	verbose
	print info More...

Detailed Description

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

Definition at line 440 of file sba.h.

Constructor & Destructor Documentation

◆ SysSPA()

EIGEN_MAKE_ALIGNED_OPERATOR_NEW sba::SysSPA::SysSPA ( )

inline

constructor

Definition at line 446 of file sba.h.

Member Function Documentation

◆ addConstraint() [1/2]

bool sba::SysSPA::addConstraint	(	int	nd0,
		int	nd1,
		Eigen::Vector3d &	tmean,
		Eigen::Quaterniond &	qpmean,
		Eigen::Matrix< double, 6, 6 > &	prec
	)

Adds a pose constraint to the system.

Parameters

n1	Index of first node of the constraint
n2	Index of second node of the constraint
tmean	A 3x1 vector, local translation from n1 to n2
qpmean	A Quaternion, local rotation from n1 to n2
prec	A 6x6 matrix, precision matrix for this link

Returns: true if constraint added, false if nd0 or nd1 not found

Definition at line 662 of file spa.cpp.

◆ addConstraint() [2/2]

void sba::SysSPA::addConstraint	(	int	pr,
		int	p0,
		int	p1,
		Eigen::Matrix< double, 12, 1 > &	mean,
		Eigen::Matrix< double, 12, 12 > &	prec
	)

Add a constraint between two poses, in a given pose frame. <pr> is reference pose, <p0> and <p1> are the pose constraints.

◆ addNode()

int sba::SysSPA::addNode	(	Eigen::Matrix< double, 4, 1 > &	trans,
		Eigen::Quaternion< double > &	qrot,
		bool	isFixed = `false`
	)

Adds a node to the system.

Parameters

trans	A 4x1 vector of translation of the camera.
qrot	A Quaternion containing the rotatin of the camera.
isFixed	Whether this camera is fixed in space or not, for spa.

Returns: the index of the node added.

Definition at line 640 of file spa.cpp.

◆ calcCost() [1/2]

double sba::SysSPA::calcCost ( bool tcost = false )

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

Definition at line 689 of file spa.cpp.

◆ calcCost() [2/2]

double sba::SysSPA::calcCost ( double dist )

calculate error assuming outliers > dist

◆ doSPA()

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

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.

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> is true for sparse Cholesky. 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 947 of file spa.cpp.

◆ printStats()

void sba::SysSPA::printStats ( )

print some system stats

◆ setupSparseSys()

void sba::SysSPA::setupSparseSys	(	double	sLambda,
		int	iter,
		int	sparseType
	)

Definition at line 852 of file spa.cpp.

◆ setupSys()

void sba::SysSPA::setupSys ( double sLambda )

Definition at line 730 of file spa.cpp.

◆ spanningTree()

void sba::SysSPA::spanningTree ( int node = 0 )

Spanning tree initialization.

Definition at line 1168 of file spa.cpp.

◆ writeSparseA()

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

Definition at line 1124 of file spa.cpp.

Member Data Documentation

◆ A

Eigen::MatrixXd sba::SysSPA::A

linear system matrix and vector

Definition at line 525 of file sba.h.

◆ B

Eigen::VectorXd sba::SysSPA::B

Definition at line 526 of file sba.h.

◆ csp

CSparse sba::SysSPA::csp

sparse matrix object

Definition at line 529 of file sba.h.

◆ lambda

double sba::SysSPA::lambda

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

Definition at line 504 of file sba.h.

◆ nFixed

int sba::SysSPA::nFixed

Number of fixed nodes.

Definition at line 480 of file sba.h.

◆ nodes

std::vector<Node,Eigen::aligned_allocator<Node> > sba::SysSPA::nodes

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

Definition at line 474 of file sba.h.

◆ oldqrot

Eigen::Matrix<double,4,1> sba::SysSPA::oldqrot

Definition at line 534 of file sba.h.

◆ oldtrans

Eigen::Matrix<double,4,1> sba::SysSPA::oldtrans

6DOF pose as a unit quaternion and translation vector, saving for LM step

Definition at line 533 of file sba.h.

◆ p2cons

std::vector<ConP2,Eigen::aligned_allocator<ConP2> > sba::SysSPA::p2cons

Set of P2 constraints.

Definition at line 483 of file sba.h.

◆ scales

std::vector<double> sba::SysSPA::scales

set of scale for SPA system, indexed by position;

Definition at line 477 of file sba.h.

◆ scons

std::vector<ConScale,Eigen::aligned_allocator<ConScale> > sba::SysSPA::scons

Set of scale constraints.

Definition at line 486 of file sba.h.

◆ sqMinDelta

double sba::SysSPA::sqMinDelta

Convergence bound (square of minimum acceptable delta change)

Definition at line 515 of file sba.h.

◆ useLocalAngles

bool sba::SysSPA::useLocalAngles

local or global angle coordinates

Definition at line 489 of file sba.h.

◆ verbose

bool sba::SysSPA::verbose

print info

Definition at line 450 of file sba.h.

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

Public Member Functions

Public Attributes