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 #ifndef NDT_CELL_HH
00036 #define NDT_CELL_HH
00037
00038 #include <ndt_map/spatial_index.h>
00039 #include <ndt_map/cell.h>
00040 #include <ndt_map/impl/EventCounterData.hpp>
00041 #include <pcl/point_cloud.h>
00042 #include <pcl/point_types.h>
00043 #include <vector>
00044 #include <cstdio>
00045 #include <Eigen/Eigen>
00046
00047 #include <fstream>
00048
00052 #define CELL_UPDATE_MODE_COVARIANCE_INTERSECTION 0
00053
00054 #define CELL_UPDATE_MODE_SAMPLE_VARIANCE 1
00055
00056 #define CELL_UPDATE_MODE_ERROR_REFINEMENT 2
00057
00058 #define CELL_UPDATE_MODE_SAMPLE_VARIANCE_SURFACE_ESTIMATION 3
00059
00060 #define CELL_UPDATE_MODE_STUDENT_T 4
00061
00062 #define REFACTORED
00063
00064 namespace lslgeneric
00065 {
00066
00075 template<typename PointT>
00076 class NDTCell : public Cell<PointT>
00077 {
00078 public:
00079 bool hasGaussian_;
00080 double cost;
00081 char isEmpty;
00082 double consistency_score;
00083 enum CellClass {HORIZONTAL=0, VERTICAL, INCLINED, ROUGH, UNKNOWN};
00084 std::vector<PointT> points_;
00085
00086 NDTCell()
00087 {
00088 hasGaussian_ = false;
00089 if(!parametersSet_)
00090 {
00091 setParameters();
00092 }
00093 N = 0;
00094 R = 0;
00095 G = 0;
00096 B = 0;
00097 occ = 0;
00098 emptyval = 0;
00099 isEmpty=0;
00100 emptylik = 0;
00101 emptydist = 0;
00102 max_occu_ = 1;
00103 consistency_score=0;
00104 cost=INT_MAX;
00105 }
00106
00107 virtual ~NDTCell()
00108 {
00109 points_.clear();
00110 }
00111
00112 NDTCell(PointT ¢er, double &xsize, double &ysize, double &zsize):
00113 Cell<PointT>(center,xsize,ysize,zsize)
00114 {
00115 hasGaussian_ = false;
00116 N = 0;
00117 R = 0;
00118 G = 0;
00119 B = 0;
00120 occ = 0;
00121 emptyval = 0;
00122 isEmpty = 0;
00123 emptylik = 0;
00124 emptydist = 0;
00125 if(!parametersSet_)
00126 {
00127 setParameters();
00128 }
00129 consistency_score=0;
00130 cost=INT_MAX;
00131 }
00132
00133 NDTCell(const NDTCell& other):Cell<PointT>()
00134 {
00135 this->center_ = other.center_;
00136 this->xsize_ = other.xsize_;
00137 this->ysize_ = other.ysize_;
00138 this->zsize_ = other.zsize_;
00139 this->hasGaussian_ = other.hasGaussian_;
00140 this->R = other.R;
00141 this->G = other.G;
00142 this->B = other.B;
00143 this->N = other.N;
00144 this->occ = other.occ;
00145 this->emptyval = other.emptyval;
00146 this->edata = other.edata;
00147 this->consistency_score=other.consistency_score;
00148 this->isEmpty = other.isEmpty;
00149 if(this->hasGaussian_) {
00150 this->setMean(other.getMean());
00151 this->setCov(other.getCov());
00152 }
00153 this->cost=other.cost;
00154 }
00155
00156 virtual Cell<PointT>* clone() const;
00157 virtual Cell<PointT>* copy() const;
00158
00165 inline void updateSampleVariance(const Eigen::Matrix3d &cov2,const Eigen::Vector3d &m2, unsigned int numpointsindistribution,
00166 bool updateOccupancyFlag=true, float max_occu=1024, unsigned int maxnumpoints=1e9);
00167
00181 inline void computeGaussian(int mode=CELL_UPDATE_MODE_SAMPLE_VARIANCE, unsigned int maxnumpoints=1e9, float occupancy_limit=255, Eigen::Vector3d origin = Eigen::Vector3d(0,0,0), double sensor_noise=0.1);
00182
00186 void computeGaussianSimple();
00190 inline void updateColorInformation();
00191
00192
00193 void rescaleCovariance();
00199 bool rescaleCovariance(Eigen::Matrix3d &cov, Eigen::Matrix3d &invCov);
00200
00201
00202 void classify();
00203
00204 void writeToVRML(FILE *fout, Eigen::Vector3d col = Eigen::Vector3d(0,0,0));
00205 int writeToJFF(FILE * jffout);
00206 int loadFromJFF(FILE * jffin);
00207
00208 inline CellClass getClass() const
00209 {
00210 return cl_;
00211 }
00212 inline Eigen::Matrix3d getCov() const
00213 {
00214 return cov_;
00215 }
00216 inline Eigen::Matrix3d getInverseCov() const
00217 {
00218 return icov_;
00219 }
00220 inline Eigen::Vector3d getMean() const
00221 {
00222 return mean_;
00223 }
00224 inline Eigen::Matrix3d getEvecs() const
00225 {
00226 return evecs_;
00227 }
00228 inline Eigen::Vector3d getEvals() const
00229 {
00230 return evals_;
00231 }
00232
00233
00234 void setCov(const Eigen::Matrix3d &cov);
00235
00236 inline void setMean(const Eigen::Vector3d &mean)
00237 {
00238 mean_ = mean;
00239 }
00240 inline void setEvals(const Eigen::Vector3d &ev)
00241 {
00242 evals_ = ev;
00243 }
00244
00245
00246
00248 static void setParameters(double _EVAL_ROUGH_THR =0.1,
00249 double _EVEC_INCLINED_THR=8*M_PI/18,
00250 double _EVAL_FACTOR =100);
00254 double getLikelihood(const PointT &pt) const;
00255
00260 virtual void addPoint(PointT &pt)
00261 {
00262 points_.push_back(pt);
00263 }
00264
00265
00266 virtual void addPoints(pcl::PointCloud<PointT> &pt)
00267 {
00268 points_.insert(points_.begin(),pt.points.begin(),pt.points.end());
00269 }
00270
00272 void setRGB(float r, float g, float b)
00273 {
00274 R=r;
00275 G = g;
00276 B = b;
00277 }
00278 void getRGB(float &r, float &g, float &b)
00279 {
00280 r = R;
00281 g = G;
00282 b = B;
00283 }
00284
00289 void updateOccupancy(float occ_val, float max_occu=255.0)
00290 {
00291 occ+=occ_val;
00292 if(occ>max_occu) occ = max_occu;
00293 if(occ<-max_occu) occ = -max_occu;
00294 max_occu_= max_occu;
00295 }
00296
00300 float getOccupancy()
00301 {
00302 return occ;
00303 }
00307 float getOccupancyRescaled()
00308 {
00309 float occupancy = 1 - 1/(1+exp(occ));
00310 return occupancy > 1 ? 1 : (occupancy < 0 ? 0 : occupancy);
00311 }
00312
00313 void updateEmpty(double elik, double dist)
00314 {
00315 emptyval++;
00316 emptylik+=elik;
00317 emptydist+=dist;
00318 }
00319
00320 float getDynamicLikelihood(unsigned int N)
00321 {
00322 return edata.computeSemiStaticLikelihood(N);
00323 }
00324 void setOccupancy(float occ_)
00325 {
00326 occ = occ_;
00327 }
00328 void setEmptyval(int emptyval_)
00329 {
00330 emptyval=emptyval_;
00331 }
00332 void setEventData(TEventData _ed)
00333 {
00334 edata = _ed;
00335 }
00336 int getEmptyval()
00337 {
00338 return emptyval;
00339 }
00340 TEventData getEventData()
00341 {
00342 return edata;
00343 }
00344 void setN(int N_)
00345 {
00346 N = N_;
00347 }
00348 int getN()
00349 {
00350 return N;
00351 }
00363 inline double computeMaximumLikelihoodAlongLine(const PointT &p1,const PointT &p2, Eigen::Vector3d &out);
00364
00365 private:
00366 Eigen::Matrix3d cov_;
00367 Eigen::Matrix3d icov_;
00368 Eigen::Matrix3d evecs_;
00369 Eigen::Vector3d mean_;
00370 Eigen::Vector3d evals_;
00371 CellClass cl_;
00372 static bool parametersSet_;
00373 static double EVAL_ROUGH_THR;
00374 static double EVEC_INCLINED_THR;
00375 static double EVAL_FACTOR;
00376 double d1_,d2_;
00377 unsigned int N;
00378 int emptyval;
00379 double emptylik;
00380 double emptydist;
00381 float R,G,B;
00382 float occ;
00383 float max_occu_;
00384 TEventData edata;
00385
00389 inline void studentT();
00390
00391 double squareSum(const Eigen::Matrix3d &C,const Eigen::Vector3d &x){
00392 double sum;
00393 sum = C(0,0)*x(0)*x(0) + C(1,1)*x(1)*x(1) + C(2,2)*x(2)*x(2);
00394 sum += 2.0*C(0,1)*x(0)*x(1) + 2.0*C(0,2)*x(0)*x(2) + 2.0*C(1,2)*x(1)*x(2);
00395 return sum;
00396 }
00397
00398
00399 void writeJFFMatrix(FILE * jffout, Eigen::Matrix3d &mat);
00400 void writeJFFVector(FILE * jffout, Eigen::Vector3d &vec);
00401 void writeJFFEventData(FILE * jffout, TEventData &evdata);
00402 int loadJFFMatrix(FILE * jffin, Eigen::Matrix3d &mat);
00403 int loadJFFVector(FILE * jffin, Eigen::Vector3d &vec);
00404 int loadJFFEventData(FILE * jffin, TEventData &evdata);
00405
00406 public:
00407 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00408
00409 };
00410 };
00411
00412 #include<ndt_map/impl/ndt_cell.hpp>
00413
00414 #endif