Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #ifndef _SPA2D_H_
00040 #define _SPA2D_H_
00041
00042 #ifndef EIGEN_USE_NEW_STDVECTOR
00043 #define EIGEN_USE_NEW_STDVECTOR
00044 #endif // EIGEN_USE_NEW_STDVECTOR
00045
00046 #include <stdio.h>
00047 #include <iostream>
00048
00049 #include <Eigen/Core>
00050 #include <Eigen/Geometry>
00051 #include <Eigen/LU>
00052 #include <Eigen/StdVector>
00053
00054 #include <vector>
00055
00056
00057 #include <nav2d_karto/csparse.h>
00058
00059
00060
00061
00062 #define SBA_DENSE_CHOLESKY 0
00063 #define SBA_SPARSE_CHOLESKY 1
00064 #define SBA_GRADIENT 2
00065 #define SBA_BLOCK_JACOBIAN_PCG 3
00066
00067
00070
00088
00089 class Node2d
00090 {
00091 public:
00092 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00093
00095 int nodeId;
00096
00098 Eigen::Matrix<double,3,1> trans;
00099 double arot;
00101 inline void normArot()
00102 {
00103 if (arot > M_PI) arot -= 2.0*M_PI;
00104 if (arot < -M_PI) arot += 2.0*M_PI;
00105 }
00106
00108 Eigen::Matrix<double,2,3> w2n;
00109 void setTransform();
00110
00114
00115
00118 Eigen::Matrix2d dRdx;
00119
00120 void setDr();
00121
00123 bool isFixed;
00124
00127 Eigen::Matrix<double,3,1> oldtrans;
00128 double oldarot;
00129 };
00130
00131
00135
00136 class Con2dP2
00137 {
00138 public:
00139 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00140
00142 int ndr;
00143
00145 int nd1;
00146
00148 Eigen::Vector2d tmean;
00149 double amean;
00150 Eigen::Matrix<double,3,3> prec;
00151
00153 Eigen::Matrix<double,3,1> err;
00155 inline double calcErr(const Node2d &nd0, const Node2d &nd1);
00156
00158 double calcErrDist(const Node2d &nd0, const Node2d &nd1);
00159
00160
00162 Eigen::Matrix<double,3,3> J0,J0t,J1,J1t;
00163
00166 const static double qScale = 1.0;
00167
00170
00173
00177 void setJacobians(std::vector<Node2d,Eigen::aligned_allocator<Node2d> > &nodes);
00178
00180 bool isValid;
00181 };
00182
00183
00184
00186
00187 class SysSPA2d
00188 {
00189 public:
00190 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00191
00193 SysSPA2d() { nFixed = 1; verbose = false; lambda = 1.0e-4, print_iros_stats=false; }
00194
00197 int addNode(const Vector3d &pos, int id);
00198 void removeNode(int id);
00199
00200
00201
00202
00203
00204 bool addConstraint(int nd0, int nd1, const Vector3d &mean, const Matrix3d &prec);
00205 bool removeConstraint(int ndi0, int ndi1);
00206
00207
00209 std::vector<Node2d,Eigen::aligned_allocator<Node2d> > nodes;
00210 std::vector<Node2d,Eigen::aligned_allocator<Node2d> > getNodes()
00211 { return nodes; }
00212
00214 std::vector< std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > > scans;
00215
00217 int nFixed;
00218
00220 std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> > p2cons;
00221
00224 double calcCost(bool tcost = false);
00226 double calcCost(double dist);
00227 double errcost;
00228
00229
00231 void printStats();
00232 void writeSparseA(char *fname, bool useCSparse = false);
00233
00236 void setupSys(double sLambda);
00237 void setupSparseSys(double sLambda, int iter, int sparseType);
00238
00243 double lambda;
00244 int doSPA(int niter, double sLambda = 1.0e-4, int useCSparse = SBA_SPARSE_CHOLESKY, double initTol = 1.0e-8, int CGiters = 50);
00245 int doSPAwindowed(int window, int niter, double sLambda, int useCSparse);
00246
00247
00248 #ifdef SBA_DSIF
00249
00250 void doDSIF(int newnode);
00251 void setupSparseDSIF(int newnode);
00252 #endif
00253
00255 double sqMinDelta;
00256
00258 Eigen::MatrixXd A;
00259 Eigen::VectorXd B;
00260
00262 CSparse2d csp;
00263
00265 void useCholmod(bool yes)
00266 { csp.useCholmod = yes; }
00267
00269 bool verbose;
00270 bool print_iros_stats;
00271
00274 void getGraph(std::vector<float> &graph);
00275 };
00276
00277
00278
00279
00281
00283 bool read2dP2File(char *fname, SysSPA2d spa);
00284
00285
00286 #endif // _SPA2d_H_