SysSPA2d holds a set of nodes and constraints for sparse pose adjustment.
More...
#include <spa2d.h>
|
bool | addConstraint (int nd0, int nd1, const Vector3d &mean, const Matrix3d &prec) |
|
int | addNode (const Vector3d &pos, int id) |
|
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) |
|
int | doSPAwindowed (int window, int niter, double sLambda, int useCSparse) |
|
void | getGraph (std::vector< float > &graph) |
|
std::vector< Node2d, Eigen::aligned_allocator< Node2d > > | getNodes () |
|
void | printStats () |
| print some system stats More...
|
|
bool | removeConstraint (int ndi0, int ndi1) |
|
void | removeNode (int id) |
|
void | setupSparseSys (double sLambda, int iter, int sparseType) |
|
void | setupSys (double sLambda) |
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | SysSPA2d () |
| constructor More...
|
|
void | useCholmod (bool yes) |
| use CHOLMOD or CSparse More...
|
|
void | writeSparseA (char *fname, bool useCSparse=false) |
|
SysSPA2d holds a set of nodes and constraints for sparse pose adjustment.
Definition at line 192 of file spa2d.h.
◆ SysSPA2d()
EIGEN_MAKE_ALIGNED_OPERATOR_NEW sba::SysSPA2d::SysSPA2d |
( |
| ) |
|
|
inline |
constructor
Definition at line 198 of file spa2d.h.
◆ addConstraint()
bool sba::SysSPA2d::addConstraint |
( |
int |
nd0, |
|
|
int |
nd1, |
|
|
const Vector3d & |
mean, |
|
|
const Matrix3d & |
prec |
|
) |
| |
◆ addNode()
int sba::SysSPA2d::addNode |
( |
const Vector3d & |
pos, |
|
|
int |
id |
|
) |
| |
add a node at a pose <pos> is x,y,th, with th in radians
Definition at line 207 of file spa2d.cpp.
◆ calcCost() [1/2]
double sba::SysSPA2d::calcCost |
( |
bool |
tcost = false | ) |
|
calculate the error in the system; if <tcost> is true, just the distance error without weighting
Definition at line 173 of file spa2d.cpp.
◆ calcCost() [2/2]
double sba::SysSPA2d::calcCost |
( |
double |
dist | ) |
|
calculate error assuming outliers > dist
◆ doSPA()
int sba::SysSPA2d::doSPA |
( |
int |
niter, |
|
|
double |
sLambda = 1.0e-4 , |
|
|
int |
useCSparse = SBA_SPARSE_CHOLESKY , |
|
|
double |
initTol = 1.0e-8 , |
|
|
int |
maxCGiters = 50 |
|
) |
| |
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> = 0 for dense Cholesky, 1 for sparse system, 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 425 of file spa2d.cpp.
◆ doSPAwindowed()
int sba::SysSPA2d::doSPAwindowed |
( |
int |
window, |
|
|
int |
niter, |
|
|
double |
sLambda, |
|
|
int |
useCSparse |
|
) |
| |
◆ getGraph()
void sba::SysSPA2d::getGraph |
( |
std::vector< float > & |
graph | ) |
|
return the graph of constraints x,y -> x',y' 4 floats per connection
Definition at line 972 of file spa2d.cpp.
◆ getNodes()
std::vector<Node2d,Eigen::aligned_allocator<Node2d> > sba::SysSPA2d::getNodes |
( |
| ) |
|
|
inline |
◆ printStats()
void sba::SysSPA2d::printStats |
( |
| ) |
|
◆ removeConstraint()
bool sba::SysSPA2d::removeConstraint |
( |
int |
ndi0, |
|
|
int |
ndi1 |
|
) |
| |
◆ removeNode()
void sba::SysSPA2d::removeNode |
( |
int |
id | ) |
|
◆ setupSparseSys()
void sba::SysSPA2d::setupSparseSys |
( |
double |
sLambda, |
|
|
int |
iter, |
|
|
int |
sparseType |
|
) |
| |
◆ setupSys()
void sba::SysSPA2d::setupSys |
( |
double |
sLambda | ) |
|
set up linear system, from RSS submission (konolige 2010) <sLambda> is the diagonal augmentation for the LM step
Definition at line 258 of file spa2d.cpp.
◆ useCholmod()
void sba::SysSPA2d::useCholmod |
( |
bool |
yes | ) |
|
|
inline |
◆ writeSparseA()
void sba::SysSPA2d::writeSparseA |
( |
char * |
fname, |
|
|
bool |
useCSparse = false |
|
) |
| |
Eigen::MatrixXd sba::SysSPA2d::A |
linear system matrix and vector
Definition at line 263 of file spa2d.h.
Eigen::VectorXd sba::SysSPA2d::B |
◆ csp
sparse matrix object
Definition at line 267 of file spa2d.h.
◆ errcost
double sba::SysSPA2d::errcost |
◆ lambda
double sba::SysSPA2d::lambda |
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. useCSParse = 0 for dense Cholesky, 1 for sparse Cholesky, 2 for BPCG
Definition at line 248 of file spa2d.h.
◆ nFixed
int sba::SysSPA2d::nFixed |
Number of fixed nodes.
Definition at line 222 of file spa2d.h.
◆ nodes
std::vector<Node2d,Eigen::aligned_allocator<Node2d> > sba::SysSPA2d::nodes |
set of nodes (camera frames) for SPA system, indexed by position;
Definition at line 214 of file spa2d.h.
◆ p2cons
std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> > sba::SysSPA2d::p2cons |
Set of P2 constraints.
Definition at line 225 of file spa2d.h.
◆ print_iros_stats
bool sba::SysSPA2d::print_iros_stats |
◆ scans
std::vector< std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > > sba::SysSPA2d::scans |
set of point scans, corresponding to nodes
Definition at line 219 of file spa2d.h.
◆ sqMinDelta
double sba::SysSPA2d::sqMinDelta |
Convergence bound (square of minimum acceptable delta change)
Definition at line 260 of file spa2d.h.
◆ verbose
bool sba::SysSPA2d::verbose |
if we want statistics
Definition at line 274 of file spa2d.h.
The documentation for this class was generated from the following files: