Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes | List of all members
sba::SysSBA Class Reference

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. More...
 
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. More...
 
int addPoint (Point p)
 Adds a point to the system. More...
 
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. More...
 
bool addProj (int ci, int pi, Eigen::Vector3d &q, bool stereo=true)
 Add a projection between point and camera, in setting up the system. More...
 
bool addStereoProj (int ci, int pi, Eigen::Vector3d &q)
 Add a projection between point and camera, in setting up the system. More...
 
double calcAvgError ()
 Calculates average cost of the system. More...
 
double calcCost ()
 Calculate the total cost of the system by adding the squared error over all projections. More...
 
double calcCost (double dist)
 Calculates total cost of the system, not counting projections with errors higher than <dist>. More...
 
double calcRMSCost (double dist=10000.0)
 Calculates total RMS cost of the system, not counting projections with errors higher than <dist>. More...
 
int countBad (double dist)
 Find number of projections with errors over a certain value. More...
 
int countProjs ()
 Return total number of projections. More...
 
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 More...
 
int mergeTracksSt (int tr0, int tr1)
 merge two tracks if possible (no conflicts) More...
 
int numBadPoints ()
 Find number of points with Z < 0 (projected behind the camera). More...
 
void printStats ()
 Print some system stats. More...
 
int reduceLongTracks (double pct)
 get rid of long tracks More...
 
int reduceTracks ()
 Reduce tracks by eliminating single tracks and invalid projections. More...
 
int remExcessTracks (int minpts)
 removes tracks that aren't needed More...
 
int removeBad (double dist)
 Remove projections with too high of an error. More...
 
void setConnMat (int minpts)
 
void setConnMatReduced (int maxconns)
 sets up connectivity matrix by greedy spanning tree More...
 
void setProjCovariance (int ci, int pi, Eigen::Matrix3d &covar)
 Sets the covariance matrix of a projection. More...
 
void setupSparseSys (double sLambda, int iter, int sparseType)
 
void setupSys (double sLambda)
 
 SysSBA ()
 Default constructor. More...
 
void updateNormals ()
 Update normals in point-plane matches, if any. More...
 
void useCholmod (bool yes)
 use CHOLMOD or CSparse More...
 

Public Attributes

Eigen::MatrixXd A
 linear system matrix and vector More...
 
Eigen::VectorXd B
 
std::vector< std::vector< bool > > connMat
 
CSparse csp
 
double huber
 Huber parameter; greater than 0.0 for Huber weighting of cost. More...
 
double lambda
 
int nFixed
 Number of fixed nodes (nodes with known poses) from the first node. More...
 
std::vector< Node, Eigen::aligned_allocator< Node > > nodes
 Set of nodes (camera frames) for SBA system, indexed by node number. More...
 
double sqMinDelta
 Convergence bound (square of minimum acceptable delta change) More...
 
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). More...
 
bool useLocalAngles
 local or global angle coordinates More...
 
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int verbose
 How much information to print to console. More...
 

Protected Member Functions

vector< map< int, int > > generateConns_ ()
 
void tsplit (int tri, int len)
 Split a track into random tracks. (What is len?) More...
 

Protected Attributes

std::vector< JacobProds, Eigen::aligned_allocator< JacobProds > > jps
 storage for Jacobian products More...
 
std::vector< Point, Eigen::aligned_allocator< Point > > oldpoints
 Storage for old points, for checking LM step and reverting. More...
 
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > tps
 variables for each track More...
 

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

◆ SysSBA()

sba::SysSBA::SysSBA ( )
inline

Default constructor.

Definition at line 85 of file sba.h.

Member Function Documentation

◆ addMonoProj()

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

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

Parameters
cicamera/node index (same as in nodes structure).
pipoint index (same as in tracks structure).
qthe 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 141 of file sba.cpp.

◆ addNode()

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
transA 4x1 vector of translation of the camera.
qrotA Quaternion containing the rotatin of the camera.
cpCamera parameters containing the K parameters, including baseline and other camera parameters.
isFixedWhether this camera is fixed in space or not, for sba.
Returns
the index of the node added.

Definition at line 79 of file sba.cpp.

◆ addPoint()

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

Adds a point to the system.

Parameters
pA point (4x1 Eigen Vector).
Returns
the index of the point added.

Definition at line 100 of file sba.cpp.

◆ addPointPlaneMatch()

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
ci0Camera index of the first point in the match.
pi0Point index of the first point in the match.
normal03D normal for the first point in camera0's coordinate frame.
ci1Camera index of the second point in the match.
pi1Point index of the second point in the match.
normal13D normal for the second point in camera1's coordinate frame.

Definition at line 187 of file sba.cpp.

◆ addProj()

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
cicamera/node index (same as in nodes structure).
pipoint index (same as in tracks structure).
qthe keypoint of the projection in image coordinates as u,v,u-d.
stereowhether 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 111 of file sba.cpp.

◆ addStereoProj()

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

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

Parameters
cicamera/node index (same as in nodes structure).
pipoint index (same as in tracks structure).
qthe 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 156 of file sba.cpp.

◆ calcAvgError()

double sba::SysSBA::calcAvgError ( )

Calculates average cost of the system.

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

Definition at line 361 of file sba.cpp.

◆ calcCost() [1/2]

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 285 of file sba.cpp.

◆ calcCost() [2/2]

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

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

Parameters
distDistance, in pixels^2, above which projections are considered outliers.
Returns
Total error of the system, in pixels^2.

Definition at line 306 of file sba.cpp.

◆ calcRMSCost()

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

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

Parameters
distRMS distance, in pixels, above which projections are considered outliers.
Returns
Total RMS error of the system, in pixels.

Definition at line 332 of file sba.cpp.

◆ countBad()

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

Find number of projections with errors over a certain value.

Definition at line 413 of file sba.cpp.

◆ countProjs()

int sba::SysSBA::countProjs ( )

Return total number of projections.

Definition at line 271 of file sba.cpp.

◆ doSBA()

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 1308 of file sba.cpp.

◆ generateConns_()

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

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

Definition at line 655 of file sba.cpp.

◆ mergeTracks()

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 1588 of file sba.cpp.

◆ mergeTracksSt()

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 1656 of file sba.cpp.

◆ numBadPoints()

int sba::SysSBA::numBadPoints ( )

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

Definition at line 385 of file sba.cpp.

◆ printStats()

void sba::SysSBA::printStats ( )

Print some system stats.

Definition at line 501 of file sba.cpp.

◆ reduceLongTracks()

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

get rid of long tracks

Definition at line 863 of file sba.cpp.

◆ reduceTracks()

int sba::SysSBA::reduceTracks ( )

Reduce tracks by eliminating single tracks and invalid projections.

Definition at line 468 of file sba.cpp.

◆ remExcessTracks()

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

removes tracks that aren't needed

Definition at line 946 of file sba.cpp.

◆ removeBad()

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

Remove projections with too high of an error.

Returns
Number of projections removed.

Definition at line 442 of file sba.cpp.

◆ setConnMat()

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

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

Definition at line 716 of file sba.cpp.

◆ setConnMatReduced()

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

sets up connectivity matrix by greedy spanning tree

Definition at line 764 of file sba.cpp.

◆ setProjCovariance()

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

Sets the covariance matrix of a projection.

Parameters
cicamera/node index (same as in nodes structure).
pipoint index (same as in tracks structure).
covar3x3 covariance matrix that affects the cost of the projection. Instead of the cost being ||err||, the cost is now (err)T*covar*(err).

Definition at line 180 of file sba.cpp.

◆ setupSparseSys()

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

Definition at line 1161 of file sba.cpp.

◆ setupSys()

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

Definition at line 1047 of file sba.cpp.

◆ tsplit()

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

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

Definition at line 811 of file sba.cpp.

◆ updateNormals()

void sba::SysSBA::updateNormals ( )

Update normals in point-plane matches, if any.

Definition at line 235 of file sba.cpp.

◆ useCholmod()

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

use CHOLMOD or CSparse

Definition at line 265 of file sba.h.

Member Data Documentation

◆ A

Eigen::MatrixXd sba::SysSBA::A

linear system matrix and vector

Definition at line 240 of file sba.h.

◆ B

Eigen::VectorXd sba::SysSBA::B

Definition at line 241 of file sba.h.

◆ connMat

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.

◆ csp

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.

◆ huber

double sba::SysSBA::huber

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

Definition at line 113 of file sba.h.

◆ jps

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

storage for Jacobian products

Definition at line 288 of file sba.h.

◆ lambda

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.

◆ nFixed

int sba::SysSBA::nFixed

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

Definition at line 92 of file sba.h.

◆ nodes

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.

◆ oldpoints

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.

◆ sqMinDelta

double sba::SysSBA::sqMinDelta

Convergence bound (square of minimum acceptable delta change)

Definition at line 162 of file sba.h.

◆ t0

long long sba::SysSBA::t0

Definition at line 82 of file sba.h.

◆ t1

long long sba::SysSBA::t1

Definition at line 82 of file sba.h.

◆ t2

long long sba::SysSBA::t2

Definition at line 82 of file sba.h.

◆ t3

long long sba::SysSBA::t3

Definition at line 82 of file sba.h.

◆ t4

long long sba::SysSBA::t4

Definition at line 82 of file sba.h.

◆ tps

std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > sba::SysSBA::tps
protected

variables for each track

Definition at line 285 of file sba.h.

◆ tracks

std::vector<Track, Eigen::aligned_allocator<Track> > sba::SysSBA::tracks

Set of tracks for each point P (projections onto camera frames).

Definition at line 95 of file sba.h.

◆ useLocalAngles

bool sba::SysSBA::useLocalAngles

local or global angle coordinates

Definition at line 144 of file sba.h.

◆ verbose

EIGEN_MAKE_ALIGNED_OPERATOR_NEW int sba::SysSBA::verbose

How much information to print to console.

Definition at line 81 of file sba.h.


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


sparse_bundle_adjustment
Author(s):
autogenerated on Wed Mar 2 2022 01:03:04