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... | |
SysSBA holds a set of nodes and points for sparse bundle adjustment.
bool sba::SysSBA::addMonoProj | ( | int | ci, |
int | pi, | ||
Eigen::Vector2d & | q | ||
) |
Add a projection between point and camera, in setting up the system.
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. |
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.
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. |
int sba::SysSBA::addPoint | ( | Point | p | ) |
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.
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. |
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.
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). |
bool sba::SysSBA::addStereoProj | ( | int | ci, |
int | pi, | ||
Eigen::Vector3d & | q | ||
) |
Add a projection between point and camera, in setting up the system.
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. |
double sba::SysSBA::calcAvgError | ( | ) |
double sba::SysSBA::calcCost | ( | ) |
double sba::SysSBA::calcCost | ( | double | dist | ) |
double sba::SysSBA::calcRMSCost | ( | double | dist = 10000.0 | ) |
int sba::SysSBA::countBad | ( | double | dist | ) |
int sba::SysSBA::countProjs | ( | ) |
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
|
protected |
Generate a connections map, used for setConnMat() and setConnMatReduced().
void sba::SysSBA::mergeTracks | ( | std::vector< std::pair< int, int > > & | prs | ) |
int sba::SysSBA::mergeTracksSt | ( | int | tri0, |
int | tri1 | ||
) |
int sba::SysSBA::numBadPoints | ( | ) |
int sba::SysSBA::reduceLongTracks | ( | double | pct | ) |
int sba::SysSBA::reduceTracks | ( | ) |
int sba::SysSBA::remExcessTracks | ( | int | minpts | ) |
int sba::SysSBA::removeBad | ( | double | dist | ) |
void sba::SysSBA::setConnMat | ( | int | minpts | ) |
void sba::SysSBA::setConnMatReduced | ( | int | maxconns | ) |
void sba::SysSBA::setProjCovariance | ( | int | ci, |
int | pi, | ||
Eigen::Matrix3d & | covar | ||
) |
Sets the covariance matrix of a projection.
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)T*covar*(err). |
void sba::SysSBA::setupSparseSys | ( | double | sLambda, |
int | iter, | ||
int | sparseType | ||
) |
|
protected |
void sba::SysSBA::updateNormals | ( | ) |
|
inline |
Eigen::MatrixXd sba::SysSBA::A |
std::vector<std::vector<bool> > sba::SysSBA::connMat |
CSparse sba::SysSBA::csp |
double sba::SysSBA::huber |
|
protected |
double sba::SysSBA::lambda |
int sba::SysSBA::nFixed |
double sba::SysSBA::sqMinDelta |
|
protected |
bool sba::SysSBA::useLocalAngles |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int sba::SysSBA::verbose |