ndt_cell.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, AASS Research Center, Orebro University.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of AASS Research Center nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  */
00035 #ifndef NDT_CELL_HH
00036 #define NDT_CELL_HH
00037 
00038 #include <spatial_index.h>
00039 #include <cell.h>
00040 #include<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 //#define ICRA_2013_NDT_OM_SIMPLE_MODE 
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 &center, 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     //void updateObservation();
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;               // = 0.1;                                                               // ???
00374     static double EVEC_INCLINED_THR;    // = cos(8*M_PI/18);//10 degree slope;  // ???
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<impl/ndt_cell.hpp>
00413 
00414 #endif


ndt_map
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Jan 6 2014 11:31:57