SysSBA holds a set of nodes and points for sparse bundle adjustment. More...

#include <sba.h>

Public Member Functions
bool	addMonoProj (int ci, int pi, Eigen::Vector2d &q)
	Add a projection between point and camera, in setting up the system.
int	addNode (Eigen::Matrix< double, 4, 1 > &trans, Eigen::Quaternion< double > &qrot, const fc::CamParams &cp, bool isFixed=false)
	Adds a node to the system.
int	addPoint (Point p)
	Adds a point to the system.
void	addPointPlaneMatch (int ci0, int pi0, Eigen::Vector3d normal0, int ci1, int pi1, Eigen::Vector3d normal1)
	Adds a pair of point-plane projection matches. Assumes the points have already been added to the system with point-to-point projections; this just takes care of the forward and backward point-to-plane projections.
bool	addProj (int ci, int pi, Eigen::Vector3d &q, bool stereo=true)
	Add a projection between point and camera, in setting up the system.
bool	addStereoProj (int ci, int pi, Eigen::Vector3d &q)
	Add a projection between point and camera, in setting up the system.
double	calcAvgError ()
	Calculates average cost of the system.
double	calcCost ()
	Calculate the total cost of the system by adding the squared error over all projections.
double	calcCost (double dist)
	Calculates total cost of the system, not counting projections with errors higher than <dist>.
double	calcRMSCost (double dist=10000.0)
	Calculates total RMS cost of the system, not counting projections with errors higher than <dist>.
int	countBad (double dist)
	Find number of projections with errors over a certain value.
int	countProjs ()
	Return total number of projections.
int	doSBA (int niter, double lambda=1.0e-4, int useCSparse=0, double initTol=1.0e-8, int maxCGiters=100)
void	mergeTracks (std::vector< std::pair< int, int > > &prs)
	merge tracks based on identity pairs
int	mergeTracksSt (int tr0, int tr1)
	merge two tracks if possible (no conflicts)
int	numBadPoints ()
	Find number of points with Z < 0 (projected behind the camera).
void	printStats ()
	Print some system stats.
int	reduceLongTracks (double pct)
	get rid of long tracks
int	reduceTracks ()
	Reduce tracks by eliminating single tracks and invalid projections.
int	remExcessTracks (int minpts)
	removes tracks that aren't needed
int	removeBad (double dist)
	Remove projections with too high of an error.
void	setConnMat (int minpts)
void	setConnMatReduced (int maxconns)
	sets up connectivity matrix by greedy spanning tree
void	setProjCovariance (int ci, int pi, Eigen::Matrix3d &covar)
	Sets the covariance matrix of a projection.
void	setupSparseSys (double sLambda, int iter, int sparseType)
void	setupSys (double sLambda)
	SysSBA ()
	Default constructor.
void	updateNormals ()
	Update normals in point-plane matches, if any.
void	useCholmod (bool yes)
	use CHOLMOD or CSparse
Public Attributes
Eigen::MatrixXd	A
	linear system matrix and vector
Eigen::VectorXd	B
std::vector< std::vector< bool > >	connMat
CSparse	csp
double	huber
	Huber parameter; greater than 0.0 for Huber weighting of cost.
double	lambda
int	nFixed
	Number of fixed nodes (nodes with known poses) from the first node.
std::vector< Node, Eigen::aligned_allocator< Node > >	nodes
	Set of nodes (camera frames) for SBA system, indexed by node number.
double	sqMinDelta
	Convergence bound (square of minimum acceptable delta change)
long long	t0
long long	t1
long long	t2
long long	t3
long long	t4
std::vector< Track, Eigen::aligned_allocator < Track > >	tracks
	Set of tracks for each point P (projections onto camera frames).
bool	useLocalAngles
	local or global angle coordinates
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int	verbose
	How much information to print to console.
Protected Member Functions
vector< map< int, int > >	generateConns_ ()
void	tsplit (int tri, int len)
	Split a track into random tracks. (What is len?)
Protected Attributes
std::vector< JacobProds, Eigen::aligned_allocator < JacobProds > >	jps
	storage for Jacobian products
std::vector< Point, Eigen::aligned_allocator < Point > >	oldpoints
	Storage for old points, for checking LM step and reverting.
std::vector< Eigen::Vector3d, Eigen::aligned_allocator < Eigen::Vector3d > >	tps
	variables for each track

Detailed Description

SysSBA holds a set of nodes and points for sparse bundle adjustment.

Definition at line 75 of file sba.h.

Constructor & Destructor Documentation

sba::SysSBA::SysSBA ( ) [inline]

Default constructor.

Definition at line 85 of file sba.h.

Member Function Documentation

bool sba::SysSBA::addMonoProj	(	int	ci,
		int	pi,
		Eigen::Vector2d &	q
	)

Add a projection between point and camera, in setting up the system.

Parameters:

ci	camera/node index (same as in nodes structure).
pi	point index (same as in tracks structure).
q	the keypoint of the projection in image coordinates as u,v,u-d.

Returns:: whether the projection was added (true) or not (false). Should only fail if the projection is a duplicate of an existing one with a different keypoint (i.e., same point projected to 2 locations in an image).

Definition at line 145 of file sba.cpp.

int sba::SysSBA::addNode	(	Eigen::Matrix< double, 4, 1 > &	trans,
		Eigen::Quaternion< double > &	qrot,
		const fc::CamParams &	cp,
		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.
cp	Camera parameters containing the K parameters, including baseline and other camera parameters.
isFixed	Whether this camera is fixed in space or not, for sba.

Returns:: the index of the node added.

Definition at line 83 of file sba.cpp.

int sba::SysSBA::addPoint ( Point p )

Adds a point to the system.

Parameters:

p	A point (4x1 Eigen Vector).

Returns:: the index of the point added.

Definition at line 104 of file sba.cpp.

void sba::SysSBA::addPointPlaneMatch	(	int	ci0,
		int	pi0,
		Eigen::Vector3d	normal0,
		int	ci1,
		int	pi1,
		Eigen::Vector3d	normal1
	)

Adds a pair of point-plane projection matches. Assumes the points have already been added to the system with point-to-point projections; this just takes care of the forward and backward point-to-plane projections.

Parameters:

ci0	Camera index of the first point in the match.
pi0	Point index of the first point in the match.
normal0	3D normal for the first point in camera0's coordinate frame.
ci1	Camera index of the second point in the match.
pi1	Point index of the second point in the match.
normal1	3D normal for the second point in camera1's coordinate frame.

Definition at line 191 of file sba.cpp.

bool sba::SysSBA::addProj	(	int	ci,
		int	pi,
		Eigen::Vector3d &	q,
		bool	stereo = `true`
	)

Add a projection between point and camera, in setting up the system.

Parameters:

ci	camera/node index (same as in nodes structure).
pi	point index (same as in tracks structure).
q	the keypoint of the projection in image coordinates as u,v,u-d.
stereo	whether the point is stereo or not (true is stereo, false is monocular).

Returns:: whether the projection was added (true) or not (false). Should only fail if the projection is a duplicate of an existing one with a different keypoint (i.e., same point projected to 2 locations in an image).

Definition at line 115 of file sba.cpp.

bool sba::SysSBA::addStereoProj	(	int	ci,
		int	pi,
		Eigen::Vector3d &	q
	)

Add a projection between point and camera, in setting up the system.

Parameters:

ci	camera/node index (same as in nodes structure).
pi	point index (same as in tracks structure).
q	the keypoint of the projection in image coordinates as u,v,u-d.

Returns:: whether the projection was added (true) or not (false). Should only fail if the projection is a duplicate of an existing one with a different keypoint (i.e., same point projected to 2 locations in an image).

Definition at line 160 of file sba.cpp.

double sba::SysSBA::calcAvgError ( )

Calculates average cost of the system.

Returns:: Average error of the system over all projections, in pixels.

Definition at line 365 of file sba.cpp.

double sba::SysSBA::calcCost ( )

Calculate the total cost of the system by adding the squared error over all projections.

Returns:: Total error of the system, in pixels^2.

Definition at line 289 of file sba.cpp.

double sba::SysSBA::calcCost ( double dist )

Calculates total cost of the system, not counting projections with errors higher than <dist>.

Parameters:

dist	Distance, in pixels^2, above which projections are considered outliers.

Returns:: Total error of the system, in pixels^2.

Definition at line 310 of file sba.cpp.

double sba::SysSBA::calcRMSCost ( double dist = 10000.0 )

Calculates total RMS cost of the system, not counting projections with errors higher than <dist>.

Parameters:

dist	RMS distance, in pixels, above which projections are considered outliers.

Returns:: Total RMS error of the system, in pixels.

Definition at line 336 of file sba.cpp.

int sba::SysSBA::countBad ( double dist )

Find number of projections with errors over a certain value.

Definition at line 417 of file sba.cpp.

int sba::SysSBA::countProjs ( )

Return total number of projections.

Definition at line 275 of file sba.cpp.

int sba::SysSBA::doSBA	(	int	niter,
		double	sLambda = `1.0e-4`,
		int	useCSparse = `0`,
		double	initTol = `1.0e-8`,
		int	maxCGiter = `100`
	)

do LM solution for system; returns number of iterations on finish. Argument is max number of iterations to perform. <lambda> is the LM diagonal factor <useCSparse> is one of SBA_DENSE_CHOLESKY, SBA_SPARSE_CHOLESKY, SBA_GRADIENT, SBA_BLOCK_JACOBIAN_PCG initTol is the initial tolerance for CG iterations

Run the LM algorithm that computes a nonlinear SBA estimate. <niter> is the max number of iterations to perform; returns the number actually performed. <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

Definition at line 1312 of file sba.cpp.

vector< map< int, int > > sba::SysSBA::generateConns_ ( ) [protected]

Generate a connections map, used for setConnMat() and setConnMatReduced().

Definition at line 659 of file sba.cpp.

void sba::SysSBA::mergeTracks ( std::vector< std::pair< int, int > > & prs )

merge tracks based on identity pairs

merge tracks based on identity pairs this can be expensive

Definition at line 1592 of file sba.cpp.

int sba::SysSBA::mergeTracksSt	(	int	tri0,
		int	tri1
	)

merge two tracks if possible (no conflicts)

merge 2 tracks leave 2nd track null; eventually need to clean up null tracks returns merged track index if successful, -1 if tracks are redundant (same cam found on both with different keypts)

Definition at line 1660 of file sba.cpp.

int sba::SysSBA::numBadPoints ( )

Find number of points with Z < 0 (projected behind the camera).

Definition at line 389 of file sba.cpp.

void sba::SysSBA::printStats ( )

Print some system stats.

Definition at line 505 of file sba.cpp.

int sba::SysSBA::reduceLongTracks ( double pct )

get rid of long tracks

Definition at line 867 of file sba.cpp.

int sba::SysSBA::reduceTracks ( )

Reduce tracks by eliminating single tracks and invalid projections.

Definition at line 472 of file sba.cpp.

int sba::SysSBA::remExcessTracks ( int minpts )

removes tracks that aren't needed

Definition at line 950 of file sba.cpp.

int sba::SysSBA::removeBad ( double dist )

Remove projections with too high of an error.

Returns:: Number of projections removed.

Definition at line 446 of file sba.cpp.

void sba::SysSBA::setConnMat ( int minpts )

Sets up the connectivity matrix by clearing connections with less than minpts.

Definition at line 720 of file sba.cpp.

void sba::SysSBA::setConnMatReduced ( int maxconns )

sets up connectivity matrix by greedy spanning tree

Definition at line 768 of file sba.cpp.

void sba::SysSBA::setProjCovariance	(	int	ci,
		int	pi,
		Eigen::Matrix3d &	covar
	)

Sets the covariance matrix of a projection.

Parameters:

ci	camera/node index (same as in nodes structure).
pi	point index (same as in tracks structure).
covar	3x3 covariance matrix that affects the cost of the projection. Instead of the cost being \|\|err\|\|, the cost is now (err)Tcovar(err).

Definition at line 184 of file sba.cpp.

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

Definition at line 1165 of file sba.cpp.

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

Definition at line 1051 of file sba.cpp.

void sba::SysSBA::tsplit	(	int	tri,
		int	len
	)		`[protected]`

Split a track into random tracks. (What is len?)

Definition at line 815 of file sba.cpp.

void sba::SysSBA::updateNormals ( )

Update normals in point-plane matches, if any.

Definition at line 239 of file sba.cpp.

void sba::SysSBA::useCholmod ( bool yes ) [inline]

use CHOLMOD or CSparse

Definition at line 265 of file sba.h.

Member Data Documentation

Eigen::MatrixXd sba::SysSBA::A

linear system matrix and vector

Definition at line 240 of file sba.h.

Eigen::VectorXd sba::SysSBA::B

Definition at line 241 of file sba.h.

std::vector<std::vector<bool> > sba::SysSBA::connMat

sparse connectivity matrix for each node, holds vector of connecting nodes "true" for don't use connection

Definition at line 246 of file sba.h.

CSparse sba::SysSBA::csp

sparse matrix object putting this at the end gets rid of alignment errors when making SBA objects

Definition at line 270 of file sba.h.

double sba::SysSBA::huber

Huber parameter; greater than 0.0 for Huber weighting of cost.

Definition at line 113 of file sba.h.

std::vector<JacobProds, Eigen::aligned_allocator<JacobProds> > sba::SysSBA::jps [protected]

storage for Jacobian products

Definition at line 288 of file sba.h.

double sba::SysSBA::lambda

set up linear system, from Engels and Nister 2006; <sLambda> is the diagonal augmentation for the LM step

Definition at line 148 of file sba.h.

int sba::SysSBA::nFixed

Number of fixed nodes (nodes with known poses) from the first node.

Definition at line 92 of file sba.h.

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

Set of nodes (camera frames) for SBA system, indexed by node number.

Definition at line 89 of file sba.h.

std::vector<Point, Eigen::aligned_allocator<Point> > sba::SysSBA::oldpoints [protected]

Storage for old points, for checking LM step and reverting.

Definition at line 282 of file sba.h.

double sba::SysSBA::sqMinDelta

Convergence bound (square of minimum acceptable delta change)

Definition at line 162 of file sba.h.

long long sba::SysSBA::t0