42 #ifndef EIGEN_USE_NEW_STDVECTOR 43 #define EIGEN_USE_NEW_STDVECTOR 44 #endif // EIGEN_USE_NEW_STDVECTOR 49 #include <Eigen/Geometry> 51 #ifndef EIGEN_USE_NEW_STDVECTOR 52 #define EIGEN_USE_NEW_STDVECTOR 54 #include <Eigen/StdVector> 63 #define SBA_DENSE_CHOLESKY 0 64 #define SBA_SPARSE_CHOLESKY 1 65 #define SBA_GRADIENT 2 66 #define SBA_BLOCK_JACOBIAN_PCG 3 97 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
108 if (arot > M_PI) arot -= 2.0*M_PI;
109 if (arot < -M_PI) arot += 2.0*M_PI;
113 Eigen::Matrix<double,2,3>
w2n;
144 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
155 Eigen::Matrix<double,3,3>
prec;
158 Eigen::Matrix<double,3,1>
err;
160 inline double calcErr(
const Node2d &nd0,
const Node2d &nd1);
163 double calcErrDist(
const Node2d &nd0,
const Node2d &nd1);
167 Eigen::Matrix<double,3,3> J0,J0t,J1,
J1t;
171 static constexpr
double qScale = 1.0;
182 void setJacobians(std::vector<
Node2d,Eigen::aligned_allocator<Node2d> > &nodes);
195 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
198 SysSPA2d() { nFixed = 1; verbose =
false; lambda = 1.0e-4, print_iros_stats=
false; }
202 int addNode(
const Vector3d &pos,
int id);
203 void removeNode(
int id);
209 bool addConstraint(
int nd0,
int nd1,
const Vector3d &mean,
const Matrix3d &prec);
210 bool removeConstraint(
int ndi0,
int ndi1);
214 std::vector<Node2d,Eigen::aligned_allocator<Node2d> >
nodes;
215 std::vector<Node2d,Eigen::aligned_allocator<Node2d> >
getNodes()
219 std::vector< std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > >
scans;
225 std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> >
p2cons;
229 double calcCost(
bool tcost =
false);
231 double calcCost(
double dist);
237 void writeSparseA(
char *fname,
bool useCSparse =
false);
241 void setupSys(
double sLambda);
242 void setupSparseSys(
double sLambda,
int iter,
int sparseType);
249 int doSPA(
int niter,
double sLambda = 1.0e-4,
int useCSparse =
SBA_SPARSE_CHOLESKY,
double initTol = 1.0e-8,
int CGiters = 50);
250 int doSPAwindowed(
int window,
int niter,
double sLambda,
int useCSparse);
254 void doDSIF(
int newnode);
256 void setupSparseDSIF(
int newnode);
279 void getGraph(std::vector<float> &graph);
double sqMinDelta
Convergence bound (square of minimum acceptable delta change)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SysSPA2d()
constructor
Eigen::Vector2d tmean
Mean vector, quaternion (inverse) and precision matrix for this constraint.
std::vector< std::vector< Eigen::Vector2d, Eigen::aligned_allocator< Eigen::Vector2d > > > scans
set of point scans, corresponding to nodes
std::vector< Node2d, Eigen::aligned_allocator< Node2d > > getNodes()
bool isFixed
For SPA, is this camera fixed or free?
void useCholmod(bool yes)
use CHOLMOD or CSparse
int nd1
Node2d index for the second node.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int nodeId
node id - somewhat redundant, but can be useful, e.g., in KARTO links
Eigen::Matrix< double, 3, 3 > J1t
Eigen::Matrix< double, 3, 3 > prec
#define SBA_SPARSE_CHOLESKY
Eigen::Matrix< double, 3, 1 > trans
6DOF pose as a unit quaternion and translation vector
Eigen::Matrix< double, 3, 1 > err
error
void writeSparseA(const char *fname, SysSBA &sba)
void normArot()
Normalize to [-pi,+pi].
bool verbose
if we want statistics
Eigen::Matrix< double, 3, 1 > oldtrans
bool isValid
valid or not (could be out of bounds)
Eigen::Matrix< double, 2, 3 > w2n
Resultant transform from world to node coordinates;.
std::vector< Con2dP2, Eigen::aligned_allocator< Con2dP2 > > p2cons
Set of P2 constraints.
std::vector< Node2d, Eigen::aligned_allocator< Node2d > > nodes
set of nodes (camera frames) for SPA system, indexed by position;
CSparse2d csp
sparse matrix object
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int ndr
Reference pose index.
SysSPA2d holds a set of nodes and constraints for sparse pose adjustment.
bool read2dP2File(char *fname, SysSPA2d spa)
constraint files
Eigen::MatrixXd A
linear system matrix and vector
int nFixed
Number of fixed nodes.