adaptive_oc_tree.cpp
Go to the documentation of this file.
00001 #include <boost/math/distributions/chi_squared.hpp>
00002 #include <Eigen/Eigen>
00003 //#include <ros/ros.h>
00004 
00005 //using boost::math::chi_squared;
00006 //using boost::math::cdf;
00007 
00008 namespace lslgeneric
00009 {
00010 
00011 template <typename PointT>
00012 void AdaptiveOctTree<PointT>::setParameters(bool _useDornikHansen,
00013         bool _useFlatness,
00014         double _RSS_THRESHOLD,
00015         double _DH_SIGNIFICANCE_LVL,
00016         double _MIN_CELL_SIZE,
00017         double _FLAT_FACTOR,
00018         double _BIG_CELL_SIZE,
00019         double _SMALL_CELL_SIZE
00020                                            )
00021 {
00022 
00023     this->BIG_CELL_SIZE = _BIG_CELL_SIZE;
00024     this->SMALL_CELL_SIZE = _SMALL_CELL_SIZE;
00025 
00026     useDornikHansen      = _useDornikHansen;
00027     useFlatness          = _useFlatness;
00028     RSS_THRESHOLD        = _RSS_THRESHOLD;
00029     DH_SIGNIFICANCE_LVL  = _DH_SIGNIFICANCE_LVL;
00030     MIN_CELL_SIZE        = _MIN_CELL_SIZE;
00031     FLAT_FACTOR          = _FLAT_FACTOR;
00032 
00033     parametersSet = true;
00034 }
00037 template <typename PointT>
00038 AdaptiveOctTree<PointT>::AdaptiveOctTree() : OctTree<PointT>()
00039 {
00040     if(!parametersSet)
00041     {
00042         setParameters();
00043     }
00044 }
00045 
00048 template <typename PointT>
00049 AdaptiveOctTree<PointT>::AdaptiveOctTree(pcl::PointXYZ center, double xsize, double ysize,
00050         double zsize, NDTCell<PointT>* type, OctTree<PointT> *_parent, unsigned int _depth) :
00051     OctTree<PointT>(center,xsize,ysize,zsize,type,_parent,_depth)
00052 {
00053     if(!parametersSet)
00054     {
00055         setParameters();
00056     }
00057 }
00058 
00061 template <typename PointT>
00062 AdaptiveOctTree<PointT>::~AdaptiveOctTree()
00063 {
00064 }
00065 
00069 template <typename PointT>
00070 void AdaptiveOctTree<PointT>::computeTreeLeafs()
00071 {
00072     if(this->isLeaf())
00073     {
00074         myTreeLeafs.push_back(this);
00075         return;
00076     }
00077 
00078     myTreeLeafs.clear();
00079     std::vector<OctTree<PointT>*> next;
00080     next.push_back(this);
00081 
00082     while(next.size()>0)
00083     {
00084         OctTree<PointT> *cur = next.front();
00085         if(cur!=NULL)
00086         {
00087             if(cur->isLeaf())
00088             {
00089                 myTreeLeafs.push_back(cur);
00090             }
00091             else
00092             {
00093                 for(int i=0; i<8; i++)
00094                 {
00095                     OctTree<PointT>* tmp = cur->getChild(i);
00096                     if(tmp!=NULL)
00097                     {
00098                         next.push_back(tmp);
00099                     }
00100                 }
00101             }
00102         }
00103         next.erase(next.begin());
00104     }
00105 }
00106 
00110 template <typename PointT>
00111 void AdaptiveOctTree<PointT>::postProcessPoints()
00112 {
00113 
00114     //compute leafs as OctTree*
00115     computeTreeLeafs();
00116     //cout<<"leafs :"<<myTreeLeafs.size()<<endl;
00117 
00118     for(unsigned int i=0; i<myTreeLeafs.size(); i++)
00119     {
00120         NDTCell<PointT> * nd = dynamic_cast<NDTCell<PointT>*>((myTreeLeafs[i])->myCell_);
00121         if(nd == NULL) continue;
00122         if(useDornikHansen)
00123         {
00124             double significance = computeDornikHansen(nd);
00125             if(significance < 0)
00126             {
00127                 //there were not enough points, let's clear the cell!
00128                 //(myTreeLeafs[i])->myCell->points.clear();
00129                 continue;
00130             }
00131             if(significance < DH_SIGNIFICANCE_LVL)
00132             {
00133                 //split the leafs
00134                 std::vector<OctTree<PointT>*> newLeafs = splitTree(myTreeLeafs[i]);
00135                 myTreeLeafs.insert(myTreeLeafs.end(),newLeafs.begin(),newLeafs.end());
00136             }
00137         }
00138         else if(useFlatness)
00139         {
00140             nd->computeGaussian();
00141             if(!nd->hasGaussian_)
00142             {
00143                 continue;
00144             }
00145 
00146             Eigen::Vector3d evals = nd->getEvals();
00147             int idMin, idMax;
00148             double minEval = evals.minCoeff(&idMin);
00149             double maxEval = evals.maxCoeff(&idMax);
00150             int idMiddle = -1;
00151             for(int j=0; j<3; j++)
00152             {
00153                 if(j!=idMin && j!=idMax)
00154                 {
00155                     idMiddle =  j;
00156                 }
00157             }
00158             if(idMiddle < 0) continue;
00159 
00160             if(minEval*FLAT_FACTOR > evals(idMiddle))
00161             {
00162                 std::vector<OctTree<PointT>*> newLeafs = splitTree(myTreeLeafs[i]);
00163                 myTreeLeafs.insert(myTreeLeafs.end(),newLeafs.begin(),newLeafs.end());
00164             }
00165 
00166 
00167         }
00168         else
00169         {
00170             double rss = computeResidualSquare(nd);
00171             if(rss > RSS_THRESHOLD)
00172             {
00173                 //split the leafs
00174                 std::vector<OctTree<PointT>*> newLeafs = splitTree(myTreeLeafs[i]);
00175                 myTreeLeafs.insert(myTreeLeafs.end(),newLeafs.begin(),newLeafs.end());
00176             }
00177         }
00178     }
00179 
00180     this->leafsCached_ = false;
00181 }
00182 
00186 template <typename PointT>
00187 double AdaptiveOctTree<PointT>::computeDornikHansen(NDTCell<PointT> *cell)
00188 {
00189     double pval = 1;
00190     double Ep = 0;
00191     //test statistics breaks down for n<=7
00192     if(cell->points_.size() <= 7) return -1;
00193     cell->computeGaussian();
00194 
00195     //degree is 2*number_of_dimensions
00196     boost::math::chi_squared dist(6);
00197 
00198     Eigen::Vector3d mean = cell->getMean();
00199     Eigen::Matrix3d C = cell->getCov();
00200 
00201     Eigen::MatrixXd Xhat(cell->points_.size(),3); //, tmpMat;
00202 
00203     for(unsigned int i=0; i< cell->points_.size(); i++)
00204     {
00205         Xhat(i,0) = cell->points_[i].x - mean(0);
00206         Xhat(i,1) = cell->points_[i].y - mean(1);
00207         Xhat(i,2) = cell->points_[i].z - mean(2);
00208     }
00209 
00210     //Compute transform for observed points
00211     Eigen::Matrix3d V;
00212     Eigen::Matrix3d H;
00213     Eigen::Vector3d lambda;
00214     Eigen::Matrix3d L;
00215     Eigen::Matrix3d R1;
00216 
00217     Eigen::MatrixXd R;
00218 
00219     V(0,0) = 1/sqrt(C(0,0));
00220     V(1,1) = 1/sqrt(C(1,1));
00221     V(2,2) = 1/sqrt(C(2,2));
00222 
00223     C = V*C*V;
00224     Eigen::EigenSolver<Eigen::Matrix3d> eig(C);
00225     H = eig.eigenvectors().real();
00226     lambda = eig.eigenvalues().real();
00227 
00228     //covariance is not positive semidefinate
00229     if(lambda.minCoeff() <= 0) return -1;
00230 
00231     L(0,0) = 1/sqrt(lambda(0));
00232     L(1,1) = 1/sqrt(lambda(1));
00233     L(2,2) = 1/sqrt(lambda(2));
00234 
00235     //transform observations
00236     R1 = H*L*H.transpose()*V;
00237     R = R1*Xhat.transpose();
00238 
00239     //compute skewness and kurtois of new observations (in each dimension)
00240     //samples are zero mean, compute for each dimension standard deviation
00241     Ep = 0.0;
00242     double n = R.cols();
00243     for(unsigned int dim = 0; dim < 3; dim ++)
00244     {
00245         double m2 = 0, m3 = 0, m4 =0;
00246         double b1 = 0, b2;
00247         double beta, omega2, gamma, y;
00248         double gamma2, a,c, k, alpha, chi;
00249         double z1,z2;
00250 
00251         for(unsigned int i=0; i<R.cols(); i++)
00252         {
00253             m2 += pow(R(dim,i),2);
00254             m3 += pow(R(dim,i),3);
00255             m4 += pow(R(dim,i),4);
00256         }
00257         m2 /= R.cols();
00258         m3 /= R.cols();
00259         m4 /= R.cols();
00260 //      cout <<"dim "<<dim<<" m2 "<<m2<<" m3 "<<m3<<" m4 "<<m4<<endl;
00261         b1 = m3/(pow(m2,1.5));
00262         b2 = m4/(pow(m2,2));
00263 
00264 //      cout<<"b1 "<<b1<<" b2 "<<b2<<endl;
00265 
00266         //compute Z1 and Z2
00267         beta = 3*(n*n + 27*n -70)*(n+1)*(n+3)/((n-2)*(n+5)*(n+7)*(n+9));
00268         omega2 = -1+sqrt(2*(beta-1));
00269         gamma = 1/sqrt(log(sqrt(omega2)));
00270         y = b1*sqrt((omega2-1)*(n+1)*(n+3)/(12*(n-2)));
00271         z1 = gamma*log(y+sqrt(y*y+1));
00272 
00273         gamma2 = (n-3)*(n+1)*(n*n+15*n-4);
00274         a = (n-2)*(n+5)*(n+7)*(n*n+27*n-70)/(6*gamma2);
00275         c = (n-7)*(n+5)*(n+7)*(n*n+2*n-5)/(6*gamma2);
00276         k = (n+5)*(n+7)*(pow(n,3)+37*n*n+11*n-313)/(12*gamma2);
00277         alpha = a + b1*b1*c;
00278         chi = (b2-1-b1*b1)*2*k;
00279         z2 = (pow((chi/(2*alpha)),(1./3.))-1+(1./(9*alpha)))*sqrt(9*alpha);
00280 
00281 //      cout<<"z1: "<<z1<<" z2: "<<z2<<endl;
00282 
00283         //compute Ep
00284         Ep += z1*z1 + z2*z2;
00285     }
00286 
00287     //compute probability from chi square cdf
00288     pval = 1-boost::math::cdf(dist,Ep);
00289 
00290 //    cout<<"Ep "<<Ep<<endl;
00291 //    cout<<"P: "<<pval<<endl;
00292     if(pval>DH_SIGNIFICANCE_LVL)
00293     {
00294         double dx,dy,dz;
00295         cell->getDimensions(dx,dy,dz);
00296         std::cout<<"final split was at ("<<dx<<","<<dy<<","<<dz<<"); pval is "<<pval<<std::endl;
00297     }
00298     return 0;
00299 
00300 }
00304 template <typename PointT>
00305 double AdaptiveOctTree<PointT>::computeResidualSquare(NDTCell<PointT> *cell)
00306 {
00307     double rss = 0; //residual sum squared
00308     double meanResidual = 0;
00309     double residualVar = 0;
00310     if(cell->points_.size() <= 3) return 0;
00311 
00312     cell->computeGaussian();
00313 
00314     Eigen::Vector3d cur, curProd;
00315     Eigen::Matrix3d cov = cell->getCov();
00316     Eigen::Vector3d mean = cell->getMean();
00317     //Eigen::LLT<Eigen::Matrix3d> lltOfCov = cov.llt();
00318 
00319     for(unsigned int i=0; i< cell->points_.size(); i++)
00320     {
00321 
00322         cur(0) = cell->points_[i].x - mean(0);
00323         cur(1) = cell->points_[i].y - mean(1);
00324         cur(2) = cell->points_[i].z - mean(2);
00325 
00326         //lltOfCov.solve(cur,&curProd);
00327         //rss += cur.dot(curProd);
00328         rss += cur.dot(cur);
00329         meanResidual += cur.norm()/cell->points_.size();
00330     }
00331 
00332     for(unsigned int i=0; i< cell->points_.size(); i++)
00333     {
00334         cur(0) = cell->points_[i].x - mean(0);
00335         cur(1) = cell->points_[i].y - mean(1);
00336         cur(2) = cell->points_[i].z - mean(2);
00337 
00338         residualVar += pow(cur.norm()-meanResidual,2)/(cell->points_.size()-1);
00339     }
00340     double bic = rss/residualVar + log(cell->points_.size());
00341 //    ROS_INFO("rss %lf mean %lf var %lf bic %lf",rss,meanResidual,residualVar,bic);
00342     return bic;
00343 }
00344 
00349 template <typename PointT>
00350 std::vector<OctTree<PointT>*> AdaptiveOctTree<PointT>::splitTree(OctTree<PointT> *octLeaf)
00351 {
00352     std::vector<OctTree<PointT>*> newLeafs;
00353 
00354     if(octLeaf->isLeaf())
00355     {
00356         double xs,ys,zs;
00357         octLeaf->myCell_->getDimensions(xs,ys,zs);
00358 
00359         double cellSize = (xs+ys+zs)/3.; //average for now
00360 
00361         if(octLeaf->depth_ > this->MAX_DEPTH || cellSize <= this->MIN_CELL_SIZE )
00362         {
00363             //just store point, we can't split any more
00364             return newLeafs;
00365         }
00366 
00367         pcl::PointXYZ myCenter = octLeaf->myCell_->getCenter();
00368 
00369         //branch leaf
00370         for(unsigned int it=0; it<8; it++)
00371         {
00372 
00373             pcl::PointXYZ newCenter;
00374 
00375             //computes the center of the it'th child
00376             newCenter.x = (myCenter.x + pow(-1.,it/4)*xs/4.);
00377             newCenter.y = (myCenter.y + pow(-1.,it/2)*ys/4.);
00378             newCenter.z = (myCenter.z + pow(-1.,(it+1)/2)*zs/4.);
00379 
00380             octLeaf->children_[it] = new OctTree<PointT>(newCenter,xs/2,ys/2,
00381                     zs/2, octLeaf->myCell_, this, this->depth_+1);
00382             newLeafs.push_back(octLeaf->children_[it]);
00383         }
00384         //add current points
00385         for(unsigned int jt=0; jt<octLeaf->myCell_->points_.size(); jt++)
00386         {
00387             size_t ind = octLeaf->getIndexForPoint(octLeaf->myCell_->points_[jt]);
00388             octLeaf->children_[ind]->addPoint(octLeaf->myCell_->points_[jt]);
00389         }
00390         octLeaf->leaf_=false;
00391         octLeaf->myCell_->points_.clear();
00392     }
00393 
00394     return newLeafs;
00395 }
00396 
00401 template <typename PointT>
00402 SpatialIndex<PointT>* AdaptiveOctTree<PointT>::clone()
00403 {
00404     if(this->myCell_ == NULL)
00405     {
00406         return new AdaptiveOctTree();
00407     }
00408     double sx,sy,sz;
00409     this->myCell_->getDimensions(sx,sy,sz);
00410     AdaptiveOctTree<PointT> *tr = new AdaptiveOctTree<PointT>(this->myCell_->getCenter(),sx,sy,sz,this->myCell_);
00411     return tr;
00412 }
00413 
00414 }


ndt_map
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Wed Aug 26 2015 15:24:40