ndt_cell.hpp
Go to the documentation of this file.
00001 #include <cstring>
00002 #include <cstdio>
00003 
00004 #define JFFERR(x) std::cerr << x << std::endl; return -1;
00005 
00006 namespace lslgeneric
00007 {
00008 
00009 
00010 template<typename PointT>
00011 bool NDTCell<PointT>::parametersSet_ = false;
00012 template<typename PointT>
00013 double NDTCell<PointT>::EVAL_ROUGH_THR;
00014 template<typename PointT>
00015 double NDTCell<PointT>::EVEC_INCLINED_THR;
00016 template<typename PointT>
00017 double NDTCell<PointT>::EVAL_FACTOR;
00018 
00019 template<typename PointT>
00020 void NDTCell<PointT>::setParameters(double _EVAL_ROUGH_THR   ,
00021                                     double _EVEC_INCLINED_THR,
00022                                     double _EVAL_FACTOR
00023                                    )
00024 {
00025 
00026     NDTCell<PointT>::EVAL_ROUGH_THR    = _EVAL_ROUGH_THR   ;
00027     NDTCell<PointT>::EVEC_INCLINED_THR = cos(_EVEC_INCLINED_THR);
00028     NDTCell<PointT>::EVAL_FACTOR       = _EVAL_FACTOR      ;
00029     parametersSet_ = true;
00030 }
00031 
00033 
00036 template<typename PointT>
00037 Cell<PointT>* NDTCell<PointT>::clone() const
00038 {
00039     NDTCell<PointT> *ret = new NDTCell<PointT>();
00040     ret->setDimensions(this->xsize_,this->ysize_,this->zsize_);
00041     ret->setCenter(this->center_);
00042     ret->setRGB(this->R,this->G,this->B);
00043     ret->setOccupancy(occ);
00044     ret->setEmptyval(emptyval);
00045     ret->setEventData(edata);
00046     ret->setN(this->N);
00047     ret->isEmpty = this->isEmpty;
00048     ret->hasGaussian_ = this->hasGaussian_;
00049     return ret;
00050 }
00054 template<typename PointT>
00055 Cell<PointT>* NDTCell<PointT>::copy() const
00056 {
00057     NDTCell<PointT> *ret = new NDTCell<PointT>();
00058 
00059     ret->setDimensions(this->xsize_,this->ysize_,this->zsize_);
00060     ret->setCenter(this->center_);
00061 
00062     for(unsigned int i=0; i<this->points_.size(); i++)
00063     {
00064         PointT pt = this->points_[i];
00065         ret->points_.push_back(pt);
00066     }
00067 
00068     ret->setMean(this->getMean());
00069     ret->setCov(this->getCov());
00070     ret->setRGB(this->R,this->G,this->B);
00071     ret->setOccupancy(this->occ);
00072     ret->setEmptyval(emptyval);
00073     ret->setEventData(edata);
00074     ret->setN(this->N);
00075     ret->isEmpty = this->isEmpty;
00076     ret->hasGaussian_ = this->hasGaussian_;
00077     ret->consistency_score = this->consistency_score;
00078     ret->cost = this->cost;
00079     return ret;
00080 }
00086 template<typename PointT>
00087 inline
00088 void NDTCell<PointT>::updateSampleVariance(const Eigen::Matrix3d &cov2,const Eigen::Vector3d &m2, unsigned int numpointsindistribution, 
00089                                                                                                                                                                         bool updateOccupancyFlag, float max_occu, unsigned int maxnumpoints)
00090 {
00091 
00092 
00093     if(numpointsindistribution<=2){
00094         fprintf(stderr,"updateSampleVariance:: INVALID NUMBER OF POINTS\n");
00095         return;
00096     }
00097     if(this->hasGaussian_)
00098     {
00099         Eigen::Vector3d msum1 = mean_ * (double) N;
00100         Eigen::Vector3d msum2 = m2 * (double) numpointsindistribution;
00101 
00102         Eigen::Matrix3d csum1 = cov_ * (double) (N-1);
00103         Eigen::Matrix3d csum2 = cov2 * (double) (numpointsindistribution-1);
00104 
00105         if( fabsf(N) < 1e-5) {
00106             fprintf(stderr,"Divider error (%u %u)!\n",N,numpointsindistribution);
00107             hasGaussian_ = false;
00108             return;
00109         }
00110         double divider = (double)numpointsindistribution+(double)N;
00111         if(fabs(divider) < 1e-5)
00112         {
00113             fprintf(stderr,"Divider error (%u %u)!\n",N,numpointsindistribution);
00114             return;
00115         }
00116         mean_ = (msum1 + msum2) / (divider);
00117 
00118         double w1 =  ((double) N / (double)(numpointsindistribution*(N+numpointsindistribution)));
00119         double w2 = (double) (numpointsindistribution)/(double) N;
00120         Eigen::Matrix3d csum3 = csum1 + csum2 + w1 * (w2 * msum1 - msum2) * ( w2 * msum1 - msum2).transpose();
00121         N = N + numpointsindistribution;
00122         cov_ = 1.0/((double)N-1.0)  * csum3;
00123         if(updateOccupancyFlag){
00124             double likoccval = 0.6;
00125             double logoddlikoccu = numpointsindistribution * log((likoccval)/(1.0-likoccval));
00126             updateOccupancy(logoddlikoccu, max_occu); 
00127         }
00128     }
00129     else
00130     {
00131         mean_ = m2;
00132         cov_ = cov2;
00133         N = numpointsindistribution;
00134         hasGaussian_ = true;
00135         if(updateOccupancyFlag){
00136             double likoccval = 0.6;
00137             double logoddlikoccu = numpointsindistribution * log((likoccval)/(1.0-likoccval));
00138             updateOccupancy(logoddlikoccu,max_occu); 
00139         }
00140     }
00141     if(N>maxnumpoints) N=maxnumpoints;
00142     if(this->occ<0){
00143         this->hasGaussian_ = false;
00144         return;
00145     }
00146     rescaleCovariance();
00147 }
00148 
00150 template<typename PointT>
00151 inline
00152 void NDTCell<PointT>::computeGaussianSimple(){
00153         Eigen::Vector3d meanSum_;
00154         Eigen::Matrix3d covSum_;
00155                         
00156                                 if(points_.size()<6){
00157                                         points_.clear();
00158                                         return;
00159                                 }
00160                                 
00161         mean_<<0,0,0;
00162         for(unsigned int i=0; i< points_.size(); i++)
00163         {
00164             Eigen::Vector3d tmp;
00165             tmp<<points_[i].x,points_[i].y,points_[i].z;
00166             mean_ += tmp;
00167         }
00168         meanSum_ = mean_;
00169         mean_ /= (points_.size());
00170         Eigen::MatrixXd mp;
00171         mp.resize(points_.size(),3);
00172         for(unsigned int i=0; i< points_.size(); i++)
00173         {
00174             mp(i,0) = points_[i].x - mean_(0);
00175             mp(i,1) = points_[i].y - mean_(1);
00176             mp(i,2) = points_[i].z - mean_(2);
00177         }
00178         covSum_ = mp.transpose()*mp;
00179         cov_ = covSum_/(points_.size()-1);
00180         this->rescaleCovariance();
00181         N = points_.size();     
00182                                 R = 0; G = 0; B = 0;
00183                                 updateColorInformation();
00184 }
00190 template<typename PointT>
00191 inline void NDTCell<PointT>::studentT(){
00192         Eigen::Vector3d meanSum_, meantmp_;
00193         Eigen::Matrix3d covSum_, covTmp_;
00194 
00195         double nu = 5; // degrees of freedom of the t-distribution
00196         unsigned int maxIter = 10; // maximum number of iterations
00197         //mean_<<0,0,0;
00198         // weights for each sample point. Initialize each weight to 1
00199         std::vector<double> lambda;
00200         unsigned int pnts = points_.size();
00201         lambda.reserve(pnts);
00202         for(unsigned int i=0;i<pnts;i++) lambda[i] = 1.0; 
00203         
00204                 
00205         for (unsigned int j=0; j<maxIter; j++)
00206         {
00207                         // update mean
00208                         double lambdaSum=0;
00209                         meantmp_<<0,0,0;
00210                         for(unsigned int i=0; i< pnts; i++)
00211                         {
00212                                         Eigen::Vector3d tmp;
00213                                         tmp<<points_[i].x,points_[i].y,points_[i].z;
00214                                         meantmp_ += lambda[i]*tmp;
00215                                         lambdaSum += lambda[i];
00216                         
00217                         }
00218                         
00219                         meanSum_ = meantmp_;
00220                         meantmp_ /= lambdaSum;
00221                         
00222                         // update scalematrix
00223                         Eigen::MatrixXd mp;
00224                         mp.resize(points_.size(),3);
00225                         for(unsigned int i=0; i< pnts; i++)
00226                         {
00227                                         double sqrtLambda = sqrt(lambda[i]);
00228                                         mp(i,0) = sqrtLambda*(points_[i].x - meantmp_(0));
00229                                         mp(i,1) = sqrtLambda*(points_[i].y - meantmp_(1));
00230                                         mp(i,2) = sqrtLambda*(points_[i].z - meantmp_(2));
00231                         }
00232                         covSum_ = mp.transpose()*mp;
00233                         
00234                         covTmp_ = covSum_/(points_.size());
00235                         
00236                         // compute inverse scalematrix
00237                         Eigen::Matrix3d invCov;
00238                         double det=0;
00239                         bool exists=false;
00240                         covTmp_.computeInverseAndDetWithCheck(invCov,det,exists);
00241                         if(!exists){
00243                                 return;
00244                         }
00245                         
00246                         Eigen::Vector3d tempVec;
00247                         // update the weights
00248                         for (unsigned int i=0; i< points_.size(); i++){
00249                                         tempVec(0) = points_[i].x - meantmp_(0);
00250                                         tempVec(1) = points_[i].y - meantmp_(1);
00251                                         tempVec(2) = points_[i].z - meantmp_(2);
00252                                         double temp = nu;
00253                                         temp += squareSum(invCov, tempVec);
00254                                         lambda[i] = (nu+3)/(temp);
00255                         }
00256         }
00257         double temp;
00258         temp = nu/(nu-2.0);
00259         covTmp_ = temp*covTmp_;
00260         
00261         if(!hasGaussian_){
00262                 mean_ = meantmp_;
00263                 cov_ = covTmp_;
00264                 N = pnts;
00265                 this->rescaleCovariance();
00266         }else{
00267                 updateSampleVariance(covTmp_, meantmp_, pnts, false);
00268         }
00269         
00270         points_.clear();
00271 }
00272 
00277 template<typename PointT>
00278 inline
00279 void NDTCell<PointT>::computeGaussian(int mode, unsigned int maxnumpoints, float occupancy_limit, Eigen::Vector3d origin, double sensor_noise)
00280 {
00284         
00286     double likoccval = 0.6;
00287     
00288     
00289     double logoddlikoccu = points_.size() * log((likoccval)/(1.0-likoccval));
00290 //              if(mode != CELL_UPDATE_MODE_ERROR_REFINEMENT){
00291                         if(logoddlikoccu > 0.4)  
00292                         {
00293                                         updateOccupancy(logoddlikoccu, occupancy_limit);
00294                         }
00295                         else
00296                         {
00297         #ifdef ICRA_2013_NDT_OM_SIMPLE_MODE
00298                                 double logoddlikempty = emptyval * log((0.49)/(1.0-0.49));
00299                                 updateOccupancy(logoddlikempty, occupancy_limit);       
00300         #else
00301                                 //updateOccupancy(logoddlikoccu+emptylik, occupancy_limit); << MUST BE ENABLED FOR OMG-NR
00302                                 updateOccupancy(logoddlikoccu, occupancy_limit);
00303                                 
00304         #endif
00305                         }
00306 //              }
00307     //occ++;
00308                 isEmpty = -1;
00309                 emptyval = 0;
00310                 emptylik = 0;
00311                 emptydist = 0;
00312                 
00313                 if(occ<=0){
00314       hasGaussian_ = false;
00315       return;
00316                 }
00317                 
00360                 if((hasGaussian_==false && points_.size()< 3) || points_.size()==0){
00361                         points_.clear();
00362                         return; 
00363                 }
00364                 
00365                 if(mode==CELL_UPDATE_MODE_STUDENT_T){
00366                         studentT();
00367                         return;
00368                 }
00369 
00370 
00371     if(!hasGaussian_)
00372     {
00373         Eigen::Vector3d meanSum_;
00374         Eigen::Matrix3d covSum_;
00375 
00376         mean_<<0,0,0;
00377         for(unsigned int i=0; i< points_.size(); i++)
00378         {
00379             Eigen::Vector3d tmp;
00380             tmp<<points_[i].x,points_[i].y,points_[i].z;
00381             mean_ += tmp;
00382         }
00383         meanSum_ = mean_;
00384         mean_ /= (points_.size());
00385         Eigen::MatrixXd mp;
00386         mp.resize(points_.size(),3);
00387         for(unsigned int i=0; i< points_.size(); i++)
00388         {
00389             mp(i,0) = points_[i].x - mean_(0);
00390             mp(i,1) = points_[i].y - mean_(1);
00391             mp(i,2) = points_[i].z - mean_(2);
00392         }
00393         covSum_ = mp.transpose()*mp;
00394         cov_ = covSum_/(points_.size()-1);
00395         this->rescaleCovariance();
00396         N = points_.size();
00397         
00398 
00401     }
00402     else if(mode == CELL_UPDATE_MODE_COVARIANCE_INTERSECTION)   
00403     {
00406         Eigen::Vector3d m2;
00407         m2<<0,0,0;
00408         for(unsigned int i=0; i< points_.size(); i++)
00409         {
00410             Eigen::Vector3d tmp;
00411             tmp<<points_[i].x,points_[i].y,points_[i].z;
00412             m2 += tmp;
00413         }
00414         m2 /= (points_.size());
00415 
00416 
00417         Eigen::MatrixXd mp;
00418         mp.resize(points_.size(),3);
00419         for(unsigned int i=0; i< points_.size(); i++)
00420         {
00421             mp(i,0) = points_[i].x - m2(0);
00422             mp(i,1) = points_[i].y - m2(1);
00423             mp(i,2) = points_[i].z - m2(2);
00424         }
00425         Eigen::Matrix3d c2;
00426         c2 = mp.transpose()*mp/(points_.size()-1);
00427         double w1 = 0.98;
00428         Eigen::Matrix3d c3,icov2,icov3;
00429         bool exists = false;
00430         double det=0;
00431         exists = rescaleCovariance(c2, icov2);
00432 
00433         if(exists)
00434         {
00435             c3 = w1 * icov_ + (1.0-w1) * icov2;
00436             c3.computeInverseAndDetWithCheck(icov3,det,exists);
00437             if(exists)
00438             {
00439                 cov_ = icov3;
00440                 mean_ = icov3 * (w1*icov_*mean_ + (1.0-w1)*icov2*m2);
00441 
00442                 this->rescaleCovariance();
00443                 N += points_.size();
00444             }
00445             else
00446             {
00447                 fprintf(stderr,"Covariance Intersection failed - Inverse does not exist (2)\n");
00448             }
00449         }
00450         else
00451         {
00452             points_.clear();
00453             fprintf(stderr,"Covariance Intersection failed - Inverse does not exist (1)\n");
00454         }
00457     }
00458     else if(mode == CELL_UPDATE_MODE_SAMPLE_VARIANCE)
00459     {
00462         Eigen::Vector3d meanSum_ = mean_ * N;
00463         Eigen::Matrix3d covSum_ = cov_ * (N-1);
00464 
00465         Eigen::Vector3d m2;
00466         m2<<0,0,0;
00467         for(unsigned int i=0; i< points_.size(); i++)
00468         {
00469             Eigen::Vector3d tmp;
00470             tmp<<points_[i].x,points_[i].y,points_[i].z;
00471             m2 += tmp;
00472         }
00473         Eigen::Vector3d T2 = m2;
00474 
00475         m2 /= (points_.size());
00476 
00477         Eigen::MatrixXd mp;
00478         mp.resize(points_.size(),3);
00479         for(unsigned int i=0; i< points_.size(); i++)
00480         {
00481             mp(i,0) = points_[i].x - m2(0);
00482             mp(i,1) = points_[i].y - m2(1);
00483             mp(i,2) = points_[i].z - m2(2);
00484         }
00485 
00486         Eigen::Matrix3d c2;
00487         c2 = mp.transpose()*mp;
00488         Eigen::Matrix3d c3;
00489 
00490         double w1 =  ((double) N / (double)(points_.size()*(N+points_.size())));
00491         double w2 = (double) (points_.size())/(double) N;
00492 
00493         c3 = covSum_ + c2 + w1 * (w2 * meanSum_ - (T2)) * ( w2 * meanSum_ - (T2)).transpose();
00494 
00495         meanSum_ = meanSum_ + T2;
00496         covSum_ = c3;
00497         N = N + points_.size();
00498 
00503         if(maxnumpoints > 0)
00504         {
00505             if(maxnumpoints < N)
00506             {
00507                 meanSum_ = meanSum_ * ((double)maxnumpoints / (double)N);
00508                 covSum_ = covSum_ * ((double)(maxnumpoints-1) / (double)(N-1));
00509                 N = maxnumpoints;
00510             }
00511         }
00512         mean_ = meanSum_ / (double) N;
00513         cov_ = covSum_ / (double) (N-1);
00514         this->rescaleCovariance();
00515 
00518     }
00519     else if(mode == CELL_UPDATE_MODE_ERROR_REFINEMENT)
00520     {
00523         
00524         double e_min = 5.0e-4;
00525         double e_max = 5.0e-2;
00526         double o_max = 10.0;
00527         if(occ>o_max) occ=o_max;
00528         if(occ<-o_max) occ = -o_max;
00529 
00530         double epsilon = ((e_min - e_max) / (2.0*o_max)) * (occ+o_max)+e_max;
00531 
00532         for(unsigned int i=0; i< points_.size(); i++)
00533         {
00534             Eigen::Vector3d tmp;
00535             tmp<<points_[i].x,points_[i].y,points_[i].z;
00536             mean_ = mean_ + epsilon * (tmp - mean_);
00537 
00538             cov_ = cov_+epsilon * ((tmp-mean_) * (tmp-mean_).transpose() - cov_);
00539             //occ += 1.0;
00540             //if(occ>o_max) occ=o_max;
00541                                                 //if(occ<-o_max) occ = -o_max;
00542         }
00543         hasGaussian_ = true;
00544         this->rescaleCovariance();
00545 
00548     }
00549     else if(mode == CELL_UPDATE_MODE_SAMPLE_VARIANCE_SURFACE_ESTIMATION)
00550     {
00553         Eigen::Vector3d meanSum_ = mean_ * N;
00554         Eigen::Matrix3d covSum_ = cov_ * (N-1);
00555 
00556         Eigen::Vector3d m2;
00557         m2<<0,0,0;
00558         for(unsigned int i=0; i< points_.size(); i++)
00559         {
00560             Eigen::Vector3d tmp;
00561             tmp<<points_[i].x,points_[i].y,points_[i].z;
00562             m2 += tmp;
00563         }
00564         Eigen::Vector3d T2 = m2;
00565 
00566         m2 /= (points_.size());
00567 
00568         PointT orig;
00569         orig.x = origin[0];
00570         orig.y=origin[1];
00571         orig.z = origin[2];
00572 
00573         Eigen::Vector3d Xm;
00574         Eigen::Matrix3d c2=Eigen::Matrix3d::Zero();
00575 
00576 
00577 
00578         for(unsigned int i=0; i< points_.size(); i++)
00579         {
00580             Eigen::Vector3d X;
00581             X<<points_[i].x,points_[i].y,points_[i].z;
00582 
00583             computeMaximumLikelihoodAlongLine(orig, points_[i], Xm);
00584             double dist = (origin-X).norm();
00585             //double sigma_dist = 0.5 * ((dist*dist)/12.0); ///test for distance based sensor noise
00586             double sigma_dist = 0.5 * ((dist)/20.0); 
00587             double snoise = sigma_dist + sensor_noise;
00588             double model_trust_given_meas = 0.49 + 0.5*exp(-0.5 * ((Xm-X).norm()*(Xm-X).norm())/(snoise*snoise));
00589 
00590             //double loglik_m = log(model_trust_given_meas / (1-model_trust_given_meas));
00591             double oval = occ/10.0;
00592             if(oval > 5.0) oval = 5.0;
00593 
00594             double likweight = oval;
00595 
00596             double weight = (1.0 - 1.0/(1.0+exp(likweight)))*model_trust_given_meas;
00597             if(N<100) weight = 0;
00598             c2 = c2 + (1.0-weight) * (X-m2)*(X-m2).transpose() + weight * (Xm-mean_)*(Xm-mean_).transpose();
00599         }
00600 
00601 
00602         Eigen::Matrix3d c3;
00603 
00604         double w1 =  ((double) N / (double)(points_.size()*(N+points_.size())));
00605         double w2 = (double) (points_.size())/(double) N;
00606 
00607         c3 = covSum_ + c2 + w1 * (w2 * meanSum_ - (T2)) * ( w2 * meanSum_ - (T2)).transpose();
00608 
00609         meanSum_ = meanSum_ + T2;
00610         covSum_ = c3;
00611         N = N + points_.size();
00612 
00617         if(maxnumpoints > 0)
00618         {
00619             if(maxnumpoints < N)
00620             {
00621                 meanSum_ = meanSum_ * ((double)maxnumpoints / (double)N);
00622                 covSum_ = covSum_ * ((double)(maxnumpoints-1) / (double)(N-1));
00623                 N = maxnumpoints;
00624             }
00625         }
00626         mean_ = meanSum_ / (double) N;
00627         cov_ = covSum_ / (double) (N-1);
00628         this->rescaleCovariance();
00631     }
00632 
00633 
00634     updateColorInformation();
00635                 
00636                 points_.clear();
00637                 /*
00638     if( !(hasGaussian_ && occ < 0))
00639     {
00640         points_.clear();
00641     }*/
00642 
00643 
00644 }
00645 
00652 template<typename PointT>
00653 inline void NDTCell<PointT>::updateColorInformation()
00654 {
00655 }
00656 
00658 template<>
00659 inline void NDTCell<pcl::PointXYZRGB>::updateColorInformation()
00660 {
00661     double r=0,g=0,b=0;
00662     for(unsigned int i=0; i< points_.size(); i++)
00663     {
00664         r+= ((double)points_[i].r)/255.0;
00665         g+= ((double)points_[i].g)/255.0;
00666         b+= ((double)points_[i].b)/255.0;
00667     }
00668     if(R == 0 && G == 0 && B == 0)
00669     {
00670         this->setRGB(r/(double)points_.size(), g/(double)points_.size(),b/(double)points_.size());
00671     }
00672     else
00673     {
00674         r /=  points_.size();
00675         g /=  points_.size();
00676         b /=  points_.size();
00677 
00678 
00679         R = 0.9*R + 0.1*r;
00680         G = 0.9*G + 0.1*g;
00681         B = 0.9*B + 0.1*b;
00682     }
00683 
00684 }
00686 template<>
00687 inline void NDTCell<pcl::PointXYZI>::updateColorInformation()
00688 {
00689     double intensity=0;
00690     for(unsigned int i=0; i< points_.size(); i++)
00691     {
00692         intensity+= ((double)points_[i].intensity)/255.0;
00693     }
00694     this->setRGB(intensity/(double)points_.size(), intensity/(double)points_.size(),intensity/(double)points_.size());
00695 }
00696 
00697 
00698 
00700 
00704 template<typename PointT>
00705 bool NDTCell<PointT>::rescaleCovariance(Eigen::Matrix3d &cov, Eigen::Matrix3d &invCov)
00706 {
00707     Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> Sol (cov);
00708 
00709     Eigen::Matrix3d evecs;
00710     Eigen::Vector3d evals;
00711 
00712     evecs = Sol.eigenvectors().real();
00713     evals = Sol.eigenvalues().real();
00714 
00715     if(evals(0) <= 0 || evals(1) <= 0 || evals(2) <= 0)
00716     {
00717         //fprintf(stderr,"Negative Eigenvalues!\n");
00718         hasGaussian_ = false;
00719         return false;
00720     }
00721     else
00722     {
00723         bool recalc = false;
00724         //guard against near singular matrices::
00725         int idMax;
00726         //double minEval = evals.minCoeff(&idMin);
00727         double maxEval = evals.maxCoeff(&idMax);
00728         if(maxEval > evals(0)*EVAL_FACTOR)
00729         {
00730             evals(0) = evals(idMax)/EVAL_FACTOR;
00731             recalc = true;
00732         }
00733         if(maxEval > evals(1)*EVAL_FACTOR)
00734         {
00735             evals(1) = evals(idMax)/EVAL_FACTOR;
00736             recalc = true;
00737         }
00738         if(maxEval > evals(2)*EVAL_FACTOR)
00739         {
00740             evals(2) = evals(idMax)/EVAL_FACTOR;
00741             recalc = true;
00742         }
00743 
00744         if(recalc)
00745         {
00746             Eigen::Matrix3d Lam;
00747             Lam = evals.asDiagonal();
00748             cov = evecs*Lam*(evecs.transpose());
00749         }
00750 
00751         //compute inverse covariance
00752         Eigen::Matrix3d Lam;
00753         Lam = evals.asDiagonal();
00754         invCov = evecs*(Lam.inverse())*(evecs.transpose());
00755     }
00756     return true;
00757 }
00758 
00759 
00760 
00761 
00763 template<typename PointT>
00764 void NDTCell<PointT>::rescaleCovariance()
00765 {
00766     Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> Sol (cov_);
00767 
00768     evecs_ = Sol.eigenvectors().real();
00769     evals_ = Sol.eigenvalues().real();
00770 
00771     //std::cout<<"evals = [ "<<evals_.transpose()<<"]';\n";
00772     if(evals_(0) <= 0 || evals_(1) <= 0|| evals_(2) <= 0)
00773     {
00774         //fprintf(stderr,"evals <=0\n");
00775         hasGaussian_ = false;
00776     }
00777     else
00778     {
00779         hasGaussian_ = true;
00780         
00781         bool recalc = false;
00782         //guard against near singular matrices::
00783         int idMax;
00784         //double minEval = evals.minCoeff(&idMin);
00785         double maxEval = evals_.maxCoeff(&idMax);
00786         if(maxEval > evals_(0)*EVAL_FACTOR)
00787         {
00788             evals_(0) = evals_(idMax)/EVAL_FACTOR;
00789             recalc = true;
00790         }
00791         if(maxEval > evals_(1)*EVAL_FACTOR)
00792         {
00793             evals_(1) = evals_(idMax)/EVAL_FACTOR;
00794             recalc = true;
00795         }
00796         if(maxEval > evals_(2)*EVAL_FACTOR)
00797         {
00798             evals_(2) = evals_(idMax)/EVAL_FACTOR;
00799             recalc = true;
00800         }
00801 
00802         if(recalc)
00803         {
00804             Eigen::Matrix3d Lam;
00805             Lam = evals_.asDiagonal();
00806             cov_ = evecs_*Lam*(evecs_.transpose());
00807         }
00808         classify();
00809         //compute inverse covariance
00810         Eigen::Matrix3d Lam;
00811         Lam = (evals_).asDiagonal();
00812         icov_ = evecs_*(Lam.inverse())*(evecs_.transpose());
00813     }
00814 }
00815 
00816 
00819 template<typename PointT>
00820 void NDTCell<PointT>::writeToVRML(FILE *fout, Eigen::Vector3d color)
00821 {
00822     if(hasGaussian_)
00823     {
00824         Eigen::Vector3d col;
00825 
00826         switch (cl_)
00827         {
00828         case ROUGH:
00829             col<<1,0,0;
00830             break;
00831         case HORIZONTAL:
00832             col<<0,1,0;
00833             break;
00834         case VERTICAL:
00835             col<<0,1,1;
00836             break;
00837         case INCLINED:
00838             col<<0,0,1;
00839             break;
00840         default:
00841             col<<0,0,0;
00842             break;
00843         }
00844 
00845         if(fout == NULL)
00846         {
00847             printf("problem outputing to vrml, wrong file pointer\n");
00848             return;
00849         }
00850 
00851         // ####### added for debugging #######
00852 
00853         // std::cerr << "evals:" << std::endl;
00854         // std::cerr << evals_ << std::endl;
00855 
00856         // std::cerr << "evecs:" << std::endl;
00857         // std::cerr << evecs_ << std::endl;
00858 
00859         // std::cerr << "cov:" << std::endl;
00860         // std::cerr << cov_ << std::endl;
00861 
00862         // ###################################
00863 
00864         Eigen::Vector3d ori;
00865         //opposite order to get local transforms
00866         ori = evecs_.eulerAngles(2,1,0);
00867         double F_ZERO = 10e-5;
00868 
00869         if (sqrt(evals_(0)) < F_ZERO || sqrt(evals_(1)) < F_ZERO ||
00870                 sqrt(evals_(2)) < F_ZERO)
00871         {
00872             return;
00873         }
00874         if(isnan(ori(0)) || isnan(ori(1)) || isnan(ori(2)))
00875         {
00876             return;
00877         }
00878         if(isinf(ori(0)) || isinf(ori(1)) || isinf(ori(2)))
00879         {
00880             return;
00881         }
00882 
00883 
00884         fprintf(fout,"Transform {\n\t");
00885         //VRML transforms are applied like this:
00886         //P_new = Trans x Rot x Scale x P_old
00887         //translation is mean location
00888         //scale is evals x |evec|%lf %lf %lf\n\t
00889         //orientation is from rotation matrix
00890         //NOTE: scaled 3x for display reasons!
00891         if(color != Eigen::Vector3d(0,0,0))
00892         {
00893             col = color;
00894         }
00895         fprintf(fout,"\
00896                 translation %lf %lf %lf \n\t \
00897                 Transform {\n\
00898                 rotation 0 0 1 %lf\n\t \
00899                 Transform {\n\
00900                 rotation 0 1 0 %lf\n\t\t \
00901                 Transform { \n\
00902                 rotation 1 0 0 %lf\n\t\t \
00903                 Transform { \n\
00904                 scale %lf %lf %lf \
00905                 \n\t\tchildren [\n\t\tShape{\n\t\t\tgeometry Sphere {radius 1}\n",
00906                 //\n\t\tchildren [\n\t\tShape{\n\t\t\tgeometry Box {size 1 1 1}\n",
00907 
00908                 mean_(0),mean_(1),mean_(2),
00909                 ori(0),ori(1),-ori(2),
00910                 //3*sqrt(evals(0)),3*sqrt(evals(1)),3*sqrt(evals(2))
00911                 sqrt(evals_(0)),sqrt(evals_(1)),sqrt(evals_(2))
00912                );
00913         fprintf(fout,"\t\t\tappearance Appearance {\n\t\t\tmaterial Material \
00914                 { diffuseColor %lf %lf %lf }\n}\n}\n]\n}}}}}\n",
00915                 col(0),col(1),col(2)
00916                );
00917     }
00918     else
00919     {
00920         /*
00921         fprintf(fout,"Shape {\n\tgeometry IndexedFaceSet {\n\t\tcoord \
00922                 Coordinate {\n\t\tpoint [\n");
00923         fprintf(fout,"%lf %lf %lf\n", this->center_.x-this->xsize_/2, this->center_.y-this->ysize_/2, this->center_.z-this->zsize_/2);
00924         fprintf(fout,"%lf %lf %lf\n", this->center_.x+this->xsize_/2, this->center_.y-this->ysize_/2, this->center_.z-this->zsize_/2);
00925         fprintf(fout,"%lf %lf %lf\n", this->center_.x+this->xsize_/2, this->center_.y+this->ysize_/2, this->center_.z-this->zsize_/2);
00926         fprintf(fout,"%lf %lf %lf\n", this->center_.x-this->xsize_/2, this->center_.y+this->ysize_/2, this->center_.z-this->zsize_/2);
00927 
00928         fprintf(fout,"%lf %lf %lf\n", this->center_.x-this->xsize_/2, this->center_.y-this->ysize_/2, this->center_.z+this->zsize_/2);
00929         fprintf(fout,"%lf %lf %lf\n", this->center_.x+this->xsize_/2, this->center_.y-this->ysize_/2, this->center_.z+this->zsize_/2);
00930         fprintf(fout,"%lf %lf %lf\n", this->center_.x+this->xsize_/2, this->center_.y+this->ysize_/2, this->center_.z+this->zsize_/2);
00931         fprintf(fout,"%lf %lf %lf\n", this->center_.x-this->xsize_/2, this->center_.y+this->ysize_/2, this->center_.z+this->zsize_/2);
00932 
00933         fprintf(fout,"]\n}\ncolor Color {\n\t color [\n");
00934         for(int i=0; i<8; i++) {
00935             fprintf(fout,"%lf %lf %lf\n", color(0),color(1),color(2) );
00936         }
00937 
00938         fprintf(fout,"]\n}\n coordIndex [\n");
00939         fprintf(fout,"0 1 2 3 -1\n\
00940                 4 5 6 7 -1\n\
00941                 0 1 5 4 -1\n\
00942                 1 2 6 5 -1\n\
00943                 2 3 7 6 -1\n\
00944                 3 0 4 7 -1\n\
00945                 ]\n}\n}");
00946         */
00947     }
00948 }
00949 
00952 template<typename PointT>
00953 void NDTCell<PointT>::writeJFFMatrix(FILE * jffout, Eigen::Matrix3d &mat)
00954 {
00955 
00956     double dtemp[6];
00957 
00958     dtemp[0] = mat.coeff(0,0);
00959     dtemp[1] = mat.coeff(1,0);
00960     dtemp[2] = mat.coeff(2,0);
00961     dtemp[3] = mat.coeff(1,1);
00962     dtemp[4] = mat.coeff(2,1);
00963     dtemp[5] = mat.coeff(2,2);
00964 
00965     fwrite(dtemp, sizeof(double), 6, jffout);
00966 
00967 }
00968 
00971 template<typename PointT>
00972 void NDTCell<PointT>::writeJFFVector(FILE * jffout, Eigen::Vector3d &vec)
00973 {
00974 
00975     double dtemp[3];
00976 
00977     for(int i=0; i<3; i++)
00978     {
00979         dtemp[i] = vec.coeff(i);
00980     }
00981 
00982     fwrite(dtemp, sizeof(double), 3, jffout);
00983 
00984 }
00985 
00988 template<typename PointT>
00989 void NDTCell<PointT>::writeJFFEventData(FILE * jffout, TEventData &evdata)
00990 {
00991 
00992     float    ftemp[4];
00993     uint8_t  ocval[1] = {evdata.occval};
00994     uint64_t evnts[1] = {evdata.events};
00995 
00996     ftemp[0] = evdata.a_exit_event;
00997     ftemp[1] = evdata.b_exit_event;
00998     ftemp[2] = evdata.a_entry_event;
00999     ftemp[3] = evdata.b_entry_event;
01000 
01001     fwrite(ocval, sizeof(uint8_t),  1, jffout);
01002     fwrite(ftemp, sizeof( float ),  4, jffout);
01003     fwrite(evnts, sizeof(uint64_t), 1, jffout);
01004 
01005 }
01006 
01009 template<typename PointT>
01010 int NDTCell<PointT>::writeToJFF(FILE * jffout)
01011 {
01012 
01013     PointT * center = &(this->center_);
01014     fwrite(center, sizeof(PointT), 1, jffout);
01015     double cell_size[3] = {this->xsize_, this->ysize_, this->zsize_};
01016     fwrite(cell_size, sizeof(double), 3, jffout);
01017 
01018     writeJFFMatrix(jffout, cov_);
01019     //writeJFFMatrix(jffout, covSum_);
01020     writeJFFVector(jffout, mean_);
01021     //writeJFFVector(jffout, meanSum_);
01022 
01023     // Temporary arrays to write all cell data
01024     double dtemp[2] = {d1_, d2_};
01025     //int    itemp[2] = {N, emptyval};
01026     int    itemp[3] = {N, emptyval,(int) hasGaussian_};
01027     float  ftemp[5] = {R, G, B, occ};
01028 
01029     fwrite(dtemp, sizeof(double), 2, jffout);
01030     fwrite(itemp, sizeof( int ),  3, jffout);
01031     fwrite(ftemp, sizeof(float),  4, jffout);
01032 
01033     writeJFFEventData(jffout, edata);
01034 
01035     return 0;
01036 
01037 }
01038 
01041 template<typename PointT>
01042 int NDTCell<PointT>::loadJFFMatrix(FILE * jffin, Eigen::Matrix3d &mat)
01043 {
01044 
01045     double dtemp[6];
01046 
01047     if(fread(&dtemp, sizeof(double), 6, jffin) <= 0)
01048         return -1;
01049 
01050     mat(0,0) = dtemp[0];
01051     mat(1,0) = dtemp[1];
01052     mat(2,0) = dtemp[2];
01053     mat(1,1) = dtemp[3];
01054     mat(2,1) = dtemp[4];
01055     mat(2,2) = dtemp[5];
01056     mat(0,1) = dtemp[1];
01057     mat(0,2) = dtemp[2];
01058     mat(1,2) = dtemp[4];
01059 
01060     return 0;
01061 
01062 }
01063 
01066 template<typename PointT>
01067 int NDTCell<PointT>::loadJFFVector(FILE * jffin, Eigen::Vector3d &vec)
01068 {
01069 
01070     double dtemp[3];
01071 
01072     if(fread(&dtemp, sizeof(double), 3, jffin) <= 0)
01073         return -1;
01074 
01075     vec << dtemp[0], dtemp[1], dtemp[2];
01076 
01077     return 0;
01078 
01079 }
01080 
01083 template<typename PointT>
01084 int NDTCell<PointT>::loadJFFEventData(FILE * jffin, TEventData &evdata)
01085 {
01086 
01087     float    ftemp[4];
01088     uint8_t  ocval;
01089     uint64_t evnts;
01090 
01091     if(fread(&ocval, sizeof(uint8_t),  1, jffin) <= 0)
01092         return -1;
01093     if(fread(&ftemp, sizeof( float ),  4, jffin) <= 0)
01094         return -1;
01095     if(fread(&evnts, sizeof(uint64_t), 1, jffin) <= 0)
01096         return -1;
01097 
01098     evdata.a_exit_event  = ftemp[0];
01099     evdata.b_exit_event  = ftemp[1];
01100     evdata.a_entry_event = ftemp[2];
01101     evdata.b_entry_event = ftemp[3];
01102     evdata.occval        = ocval;
01103     evdata.events        = evnts;
01104 
01105     return 0;
01106 }
01107 
01110 template<typename PointT>
01111 int NDTCell<PointT>::loadFromJFF(FILE * jffin)
01112 {
01113 
01114     PointT center;
01115     if(fread(&center, sizeof(PointT), 1, jffin) <= 0)
01116         return -1;
01117     this->setCenter(center);
01118 
01119     double dimensions[3];
01120     if(fread(&dimensions, sizeof(double), 3, jffin) <= 0)
01121         return -1;
01122     this->setDimensions(dimensions[0], dimensions[1], dimensions[2]);
01123 
01124     Eigen::Matrix3d temp_matrix;
01125     Eigen::Vector3d temp_vector;
01126     if(loadJFFMatrix(jffin, temp_matrix) < 0)
01127         return -1;
01128     this->setCov(temp_matrix);
01129 
01130     if(loadJFFVector(jffin, temp_vector) < 0)
01131         return -1;
01132     this->setMean(temp_vector);
01133 
01134     // Temporary arrays to load all cell data to
01135     double dtemp[2];// = {d1_, d2_};
01136     int    itemp[3];// = {N, emptyval, hasGaussian_};
01137     float  ftemp[5];// = {R, G, B, occ, cellConfidence};
01138 
01139     if(fread(&dtemp, sizeof(double), 2, jffin) <= 0)
01140         return -1;
01141     if(fread(&itemp, sizeof( int ),  3, jffin) <= 0)
01142         return -1;
01143     if(fread(&ftemp, sizeof(float),  4, jffin) <= 0)
01144         return -1;
01145 
01146     this->d1_ = dtemp[0];
01147     this->d2_ = dtemp[1];
01148 
01149     this->setN(itemp[0]);
01150     this->setEmptyval(itemp[1]);
01151     this->hasGaussian_ = (bool) itemp[2];
01152 
01153     this->setRGB(ftemp[0], ftemp[1], ftemp[2]);
01154     this->setOccupancy(ftemp[3]);
01155 
01156     TEventData edata;
01157     loadJFFEventData(jffin, edata);
01158     this->setEventData(edata);
01159 
01160     return 0;
01161 
01162 }
01163 
01164 
01169 template<typename PointT>
01170 void NDTCell<PointT>::classify()
01171 {
01172 
01173     cl_ = UNKNOWN;
01174 
01175     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> tr;
01176     tr = tr.rotate(evecs_);
01177 
01178     int index=-1;
01179     double minEval = evals_.minCoeff(&index);
01180     if(index<0 || index > 2) return;
01181 
01182     if(minEval > EVAL_ROUGH_THR )
01183     {
01184         cl_ = ROUGH;
01185     }
01186     //if 1 eval << other 2 -> planar distr
01187     else
01188     {
01189         //default case -> sloping surface
01190         cl_ = INCLINED;
01191 
01192         //check the orientation of the vanishing axis
01193         Eigen::Vector3d e3;
01194         e3<<0,0,1;
01195 
01196         Eigen::Vector3d minorAxis = evecs_.col(index); //tr*e3;
01197 
01198         //dot product with vertical axis gives us the angle
01199         double d = minorAxis.dot(e3);
01200         double l = minorAxis.norm();
01201         double ac = d/l;
01202         if(fabsf(ac) < EVEC_INCLINED_THR)
01203         {
01204             //angle is nearly perpendicular => vertical surface
01205             cl_ = VERTICAL;
01206         }
01207 
01208         if(fabsf(ac) > 1-EVEC_INCLINED_THR)
01209         {
01210             //angle is nearly 0 => horizontal surface
01211             cl_ = HORIZONTAL;
01212         }
01213     }
01214 }
01215 
01216 template<typename PointT>
01217 double NDTCell<PointT>::getLikelihood(const PointT &pt) const
01218 {
01219     //compute likelihood
01220     if(!hasGaussian_) return -1;
01221     Eigen::Vector3d vec (pt.x,pt.y,pt.z);
01222     vec = vec-mean_;
01223     double likelihood = vec.dot(icov_*vec);
01224     if(std::isnan(likelihood)) return -1;
01225 
01226     return exp(-likelihood/2);
01227     //return -d1*exp(-d2*likelihood/2);
01228 }
01229 
01230 
01233 template<typename PointT>
01234 void NDTCell<PointT>::setCov(const Eigen::Matrix3d &_cov)
01235 {
01236     cov_ = _cov;
01237     this->rescaleCovariance();
01238 }
01249 template<typename PointT>
01250 inline
01251 double NDTCell<PointT>::computeMaximumLikelihoodAlongLine(const PointT &p1, const PointT &p2, Eigen::Vector3d &out)
01252 {
01253     Eigen::Vector3d v1,v2;
01254     v1 << p1.x,p1.y,p1.z;
01255     v2 << p2.x,p2.y,p2.z;
01256 
01257     Eigen::Vector3d L = (v2-v1)/(v2-v1).norm();
01258     Eigen::Vector3d A = icov_ * L;
01259     Eigen::Vector3d B = (v2 - mean_);
01260 
01261     double sigma = A(0)*L(0)+A(1)*L(1)+A(2)*L(2);
01262     if(sigma == 0) return 1.0;
01263 
01264     double t = -(A(0)*B(0)+A(1)*B(1)+A(2)*B(2))/sigma;
01265 
01266     Eigen::Vector3d X = L*t+v2; 
01267 
01268     PointT p;
01269     p.x = X(0);
01270     p.y = X(1);
01271     p.z = X(2);
01272     out = X;
01273     return getLikelihood(p);
01274 
01275 }
01276 
01277 
01278 
01279 
01280 }; // end namespace


ndt_map
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Oct 6 2014 03:18:54