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;
00196 unsigned int maxIter = 10;
00197
00198
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
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
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
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
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
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
00302 updateOccupancy(logoddlikoccu, occupancy_limit);
00303
00304 #endif
00305 }
00306
00307
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
00540
00541
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
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
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
00639
00640
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
00718 hasGaussian_ = false;
00719 return false;
00720 }
00721 else
00722 {
00723 bool recalc = false;
00724
00725 int idMax;
00726
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
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
00772 if(evals_(0) <= 0 || evals_(1) <= 0|| evals_(2) <= 0)
00773 {
00774
00775 hasGaussian_ = false;
00776 }
00777 else
00778 {
00779 hasGaussian_ = true;
00780
00781 bool recalc = false;
00782
00783 int idMax;
00784
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
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
00852
00853
00854
00855
00856
00857
00858
00859
00860
00861
00862
00863
00864 Eigen::Vector3d ori;
00865
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
00886
00887
00888
00889
00890
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
00907
00908 mean_(0),mean_(1),mean_(2),
00909 ori(0),ori(1),-ori(2),
00910
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
00922
00923
00924
00925
00926
00927
00928
00929
00930
00931
00932
00933
00934
00935
00936
00937
00938
00939
00940
00941
00942
00943
00944
00945
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
01020 writeJFFVector(jffout, mean_);
01021
01022
01023
01024 double dtemp[2] = {d1_, d2_};
01025
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(¢er, 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
01135 double dtemp[2];
01136 int itemp[3];
01137 float ftemp[5];
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
01187 else
01188 {
01189
01190 cl_ = INCLINED;
01191
01192
01193 Eigen::Vector3d e3;
01194 e3<<0,0,1;
01195
01196 Eigen::Vector3d minorAxis = evecs_.col(index);
01197
01198
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
01205 cl_ = VERTICAL;
01206 }
01207
01208 if(fabsf(ac) > 1-EVEC_INCLINED_THR)
01209 {
01210
01211 cl_ = HORIZONTAL;
01212 }
01213 }
01214 }
01215
01216 template<typename PointT>
01217 double NDTCell<PointT>::getLikelihood(const PointT &pt) const
01218 {
01219
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
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 };