SysSPA holds a set of nodes and constraints for sparse pose adjustment.
More...
#include <sba.h>
|
| 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) |
| |
SysSPA holds a set of nodes and constraints for sparse pose adjustment.
Definition at line 440 of file sba.h.
| EIGEN_MAKE_ALIGNED_OPERATOR_NEW sba::SysSPA::SysSPA |
( |
| ) |
|
|
inline |
constructor
Definition at line 446 of file sba.h.
| 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 666 of file spa.cpp.
| 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.
| 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 644 of file spa.cpp.
| 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 693 of file spa.cpp.
| double sba::SysSPA::calcCost |
( |
double |
dist | ) |
|
calculate error assuming outliers > dist
| 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 951 of file spa.cpp.
| void sba::SysSPA::printStats |
( |
| ) |
|
| void sba::SysSPA::setupSparseSys |
( |
double |
sLambda, |
|
|
int |
iter, |
|
|
int |
sparseType |
|
) |
| |
| void sba::SysSPA::setupSys |
( |
double |
sLambda | ) |
|
| void sba::SysSPA::spanningTree |
( |
int |
node = 0 | ) |
|
Spanning tree initialization.
Definition at line 1172 of file spa.cpp.
| void sba::SysSPA::writeSparseA |
( |
char * |
fname, |
|
|
bool |
useCSparse = false |
|
) |
| |
| Eigen::MatrixXd sba::SysSPA::A |
linear system matrix and vector
Definition at line 525 of file sba.h.
| Eigen::VectorXd sba::SysSPA::B |
sparse matrix object
Definition at line 529 of file sba.h.
| 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.
Number of fixed nodes.
Definition at line 480 of file sba.h.
| 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.
| Eigen::Matrix<double,4,1> sba::SysSPA::oldqrot |
| 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.
| std::vector<ConP2,Eigen::aligned_allocator<ConP2> > sba::SysSPA::p2cons |
Set of P2 constraints.
Definition at line 483 of file sba.h.
| std::vector<double> sba::SysSPA::scales |
set of scale for SPA system, indexed by position;
Definition at line 477 of file sba.h.
Set of scale constraints.
Definition at line 486 of file sba.h.
| double sba::SysSPA::sqMinDelta |
Convergence bound (square of minimum acceptable delta change)
Definition at line 515 of file sba.h.
| bool sba::SysSPA::useLocalAngles |
local or global angle coordinates
Definition at line 489 of file sba.h.
| 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: