oc_tree.hpp
Go to the documentation of this file.
00001 #include<pointcloud_vrml/pointcloud_utils.h>
00002 
00003 namespace lslgeneric
00004 {
00005 
00006 template <typename PointT>
00007 void OctTree<PointT>::setParameters(double _BIG_CELL_SIZE       ,
00008                                     double _SMALL_CELL_SIZE     ,
00009                                     int _MAX_POINTS             ,
00010                                     int _MAX_DEPTH
00011                                    )
00012 {
00013 
00014     //defaults
00015     this->MAX_POINTS            = _MAX_POINTS           ;
00016     this->MAX_DEPTH             = _MAX_DEPTH            ;
00017     this->BIG_CELL_SIZE = _BIG_CELL_SIZE        ;
00018     this->SMALL_CELL_SIZE       = _SMALL_CELL_SIZE      ;
00019     this->parametersSet_ = true;
00020 
00021     if(!this->leaf_)
00022     {
00023         for(unsigned int it=0; it<8; it++)
00024         {
00025             if(this->children_[it]!=NULL)
00026             {
00027                 this->children_[it]->setParameters(_BIG_CELL_SIZE,_SMALL_CELL_SIZE,
00028                                                    _MAX_POINTS,_MAX_DEPTH);
00029             }
00030         }
00031     }
00032 }
00037 template <typename PointT>
00038 size_t OctTree<PointT>::getIndexForPoint(const PointT &pt) const
00039 {
00052     size_t ret = 0;
00053     if(pt.x < myCell_->getCenter().x)
00054     {
00055         ret += 4;
00056     }
00057     if(pt.y < myCell_->getCenter().y)
00058     {
00059         ret += 2;
00060     }
00061     if(ret%4==0)
00062     {
00063         if(pt.z < myCell_->getCenter().z)
00064         {
00065             ret += 1;
00066         }
00067     }
00068     else
00069     {
00070         if(pt.z > myCell_->getCenter().z)
00071         {
00072             ret += 1;
00073         }
00074     }
00075     return ret;
00076 }
00077 
00080 template <typename PointT>
00081 OctTree<PointT>::OctTree()
00082 {
00083 
00084     this->parent_=NULL;
00085     this->leaf_=true;
00086     this->depth_=0;
00087     for(unsigned int it=0; it<8; it++)
00088     {
00089         this->children_[it]=NULL;
00090     }
00091     myCell_ = NULL;
00092     leafsCached_ = false;
00093     this->setParameters();
00094 }
00095 
00098 template <typename PointT>
00099 OctTree<PointT>::OctTree(PointT center, double xsize, double ysize,
00100                          double zsize, NDTCell<PointT>* type, OctTree<PointT> *_parent, unsigned int _depth)
00101 {
00102 
00103     parent_=_parent;
00104     leaf_=true;
00105     depth_=_depth;
00106     for(unsigned int it=0; it<8; it++)
00107     {
00108         children_[it]=NULL;
00109     }
00110     NDTCell<PointT>* tmp = dynamic_cast<NDTCell<PointT>*>(type->clone());
00111     if(tmp==NULL)
00112     {
00113         //ERR("dynamic cast of cell failed!!\n");
00114         return;
00115     }
00116 
00117     tmp->setCenter(center);
00118     tmp->setDimensions(xsize,ysize,zsize);
00119     tmp->points_.clear();
00120 
00121     myCell_ = tmp;
00122     leafsCached_ = false;
00123     this->setParameters();
00124 }
00125 
00128 template <typename PointT>
00129 OctTree<PointT>::~OctTree()
00130 {
00131 
00132     if(!leaf_)
00133     {
00134         for(unsigned int it=0; it<8; it++)
00135         {
00136             if(children_[it]!=NULL)
00137             {
00138                 delete children_[it]; //calls destructor of child, so we are ok
00139                 children_[it]=NULL;
00140             }
00141         }
00142     }
00143     delete myCell_;
00144 }
00145 
00150 template <typename PointT>
00151 Cell<PointT>* OctTree<PointT>::addPoint(const PointT &point_c)
00152 {
00153 
00154     PointT point = point_c;
00155     if(std::isnan(point.x) ||std::isnan(point.y) ||std::isnan(point.z))
00156     {
00157         return NULL;
00158     }
00159     leafsCached_ = false;
00160     if(this->leaf_)
00161     {
00162         double xs,ys,zs;
00163         myCell_->getDimensions(xs,ys,zs);
00164 
00165         double cellSize = (xs+ys+zs)/3.; //average for now
00166 
00167         if(myCell_->points_.size()<(unsigned int)MAX_POINTS && cellSize <= BIG_CELL_SIZE)
00168         {
00169             if(!myCell_->isInside(point))
00170             {
00171                 //DBG(1,"OctTree: addPoint (%lf,%lf,%lf) not in boundary!\n",point.x,point.y,point.z);
00172                 return NULL;
00173             }
00174             myCell_->points_.push_back(point);
00175         }
00176         else
00177         {
00178             if(depth_>(unsigned int) MAX_DEPTH || cellSize <= 2*SMALL_CELL_SIZE )
00179             {
00180                 //TSV: we have to be sure we won't violate the space constraint if we split
00181                 //just store point, we can't split any more
00182                 if(!myCell_->isInside(point))
00183                 {
00184                     //DBG(1,"OctTree: addPoint (%lf,%lf,%lf) not in boundary!\n",point.x,point.y,point.z);
00185                     return NULL;
00186                 }
00187                 myCell_->points_.push_back(point);
00188                 return myCell_;
00189             }
00190 
00191             PointT myCenter = myCell_->getCenter();
00192 
00193             //branch leaf
00194             for(int it=0; it<8; it++)
00195             {
00196                 PointT newCenter;
00197 
00198                 //computes the center_ of the it'th child
00199                 newCenter.x = (myCenter.x + pow(-1.,it/4)*xs/4.);
00200                 newCenter.y = (myCenter.y + pow(-1.,it/2)*ys/4.);
00201                 newCenter.z = (myCenter.z + pow(-1.,(it+1)/2)*zs/4.);
00202 
00203                 children_[it] = new OctTree<PointT>(newCenter,xs/2,ys/2,
00204                                                     zs/2, myCell_, this, depth_+1);
00205                 children_[it]->setParameters(BIG_CELL_SIZE,SMALL_CELL_SIZE,
00206                                              MAX_POINTS,MAX_DEPTH);
00207             }
00208             //add current points
00209             for(unsigned int jt=0; jt<myCell_->points_.size(); jt++)
00210             {
00211                 size_t ind = getIndexForPoint(myCell_->points_[jt]);
00212                 children_[ind]->addPoint(myCell_->points_[jt]);
00213             }
00214             //finally add the new point
00215             size_t ind = getIndexForPoint(point);
00216             Cell<PointT>* ptcell = children_[ind]->addPoint(point);
00217             this->leaf_=false;
00218             this->myCell_->points_.clear();
00219             return ptcell;
00220         }
00221     }
00222     else
00223     {
00224         //pass down to correct child
00225         size_t ind = getIndexForPoint(point);
00226         return children_[ind]->addPoint(point);
00227     }
00228 }
00229 
00232 template <typename PointT>
00233 Cell<PointT>* OctTree<PointT>::getCellForPoint(const PointT &point)
00234 {
00235 
00236     OctTree<PointT>* pointLeaf = this->getLeafForPoint(point);
00237     return (pointLeaf==NULL) ? NULL : pointLeaf->myCell_;
00238 
00239 }
00240 
00243 template <typename PointT>
00244 OctTree<PointT>* OctTree<PointT>::getLeafForPoint(const PointT &point)
00245 {
00246 
00247     if(this->leaf_ && myCell_!= NULL)
00248     {
00249         if(myCell_->isInside(point))
00250         {
00251             return this;
00252         }
00253     }
00254     else
00255     {
00256         size_t ind = getIndexForPoint(point);
00257         if(children_[ind]!=NULL)
00258         {
00259             return children_[ind]->getLeafForPoint(point);
00260         }
00261     }
00262     return NULL;
00263 
00264 }
00265 
00268 template <typename PointT>
00269 void OctTree<PointT>::computeLeafCells()
00270 {
00271     if(this->isLeaf())
00272     {
00273         myLeafs_.push_back(this->myCell_);
00274         return;
00275     }
00276 
00277     myLeafs_.clear();
00278     std::vector<OctTree<PointT>*> next;
00279     next.push_back(this);
00280 
00281     while(next.size()>0)
00282     {
00283         OctTree<PointT> *cur = next.front();
00284         if(cur!=NULL)
00285         {
00286             if(cur->isLeaf())
00287             {
00288                 myLeafs_.push_back(cur->myCell_);
00289             }
00290             else
00291             {
00292                 for(int i=0; i<8; i++)
00293                 {
00294                     OctTree<PointT>* tmp = cur->getChild(i);
00295                     if(tmp!=NULL)
00296                     {
00297                         next.push_back(tmp);
00298                     }
00299                 }
00300             }
00301         }
00302         next.erase(next.begin());
00303     }
00304 }
00305 
00308 template <typename PointT>
00309 void OctTree<PointT>::setCellType(Cell<PointT> *type)
00310 {
00311 
00312     myCell_ = dynamic_cast<NDTCell<PointT>*>(type->clone());
00313     if(myCell_ == NULL)
00314     {
00315         //cast failed, it's not a derivative of oct cell
00316         myCell_ = new NDTCell<PointT>();
00317     }
00318 
00319 }
00320 
00324 template <typename PointT>
00325 typename SpatialIndex<PointT>::CellVectorItr OctTree<PointT>::begin()
00326 {
00327     if(!leafsCached_)
00328     {
00329         myLeafs_.clear();
00330         computeLeafCells();
00331     }
00332     leafsCached_ = true;
00333     return myLeafs_.begin();
00334 }
00335 
00338 template <typename PointT>
00339 typename SpatialIndex<PointT>::CellVectorItr OctTree<PointT>::end()
00340 {
00341     if(!leafsCached_)
00342     {
00343         myLeafs_.clear();
00344         computeLeafCells();
00345     }
00346     leafsCached_ = true;
00347     return myLeafs_.end();
00348 }
00349 
00352 template <typename PointT>
00353 SpatialIndex<PointT>* OctTree<PointT>::clone() const
00354 {
00355     OctTree<PointT> *tr = new OctTree<PointT>();
00356     tr->setParameters(BIG_CELL_SIZE,SMALL_CELL_SIZE,MAX_POINTS,MAX_DEPTH);
00357     if(myCell_ != NULL)
00358     {
00359         tr->setCellType(myCell_);
00360     }
00361     return tr;
00362 }
00363 
00366 template <typename PointT>
00367 SpatialIndex<PointT>* OctTree<PointT>::copy() const
00368 {
00369     OctTree<PointT> *tr;
00370     if(myCell_ != NULL)
00371     {
00372         PointT center = myCell_->getCenter();
00373         double sx,sy,sz;
00374         myCell_->getDimensions(sx,sy,sz);
00375         tr = new OctTree<PointT>(center,sx,sy,sz,myCell_);
00376     }
00377     else
00378     {
00379         tr = new OctTree<PointT>();
00380     }
00381     tr->setParameters(BIG_CELL_SIZE,SMALL_CELL_SIZE,MAX_POINTS,MAX_DEPTH);
00382     return tr;
00383 }
00384 
00388 template <typename PointT>
00389 void OctTree<PointT>::setCenter(const double &cx, const double &cy, const double &cz)
00390 {
00391     if(myCell_ == NULL)
00392     {
00393         return;
00394     }
00395     PointT center_;
00396     center_.x = cx;
00397     center_.y = cy;
00398     center_.z = cz;
00399 
00400     myCell_->setCenter(center_);
00401 }
00402 
00406 template <typename PointT>
00407 void OctTree<PointT>::setSize(const double &sx, const double &sy, const double &sz)
00408 {
00409     if(myCell_ == NULL)
00410     {
00411         return;
00412     }
00413     myCell_->setDimensions(sx,sy,sz);
00414 }
00415 
00421 template <typename PointT>
00422 void OctTree<PointT>::getNeighbors(const PointT &point, const double &radius, std::vector<Cell<PointT>*> &cells)
00423 {
00424 
00425     cells.clear();
00426     //first find the leaf that contains the point
00427     OctTree<PointT> *target = this->getLeafForPoint(point);
00428     if(target==NULL) return;
00429 
00430     OctTree<PointT> *mparent = target->parent_;
00431     OctTree<PointT> *mthis = target;
00432     std::vector<OctTree<PointT>*> toExpand;
00433     //check if any of the siblings intersect the sphere (key,constraint)
00434 
00435     while(mparent!=NULL)
00436     {
00437         for(unsigned int it=0; it<8; it++)
00438         {
00439             if(mparent->children_[it] == NULL) continue;
00440             if(mparent->children_[it]->intersectSphere(point,radius)
00441                     && mparent->children_[it]!=mthis )
00442             {
00443                 //if yes, add them to the list to expand
00444                 toExpand.push_back(mparent->children_[it]);
00445             }
00446         }
00447         //go up to parent
00448         mthis=mparent;
00449         mparent=mparent->parent_;
00450         //for all nodes in list, go down to leafs that intersect
00451         for(unsigned int nt=0; nt<toExpand.size(); nt++)
00452         {
00453             if(toExpand[nt] == NULL )
00454             {
00455                 //ERR("ERROR in nearest neighbor!!\n");
00456                 continue;
00457             }
00458 
00459             PointT center_ = (toExpand[nt]->myCell_->getCenter());
00460             Eigen::Vector3d d;
00461             d<<center_.x-point.x, center_.y-point.y, center_.z-point.z;
00462             if(toExpand[nt]->isLeaf() &&
00463                     d.norm() < radius)
00464             {
00465                 cells.push_back(toExpand[nt]->myCell_);
00466             }
00467             else
00468             {
00469                 for(unsigned int it=0; it<8; it++)
00470                 {
00471                     if(toExpand[nt]->children_[it]==NULL) continue;
00472                     if(toExpand[nt]->children_[it]->intersectSphere(point,radius))
00473                     {
00474                         toExpand.push_back(toExpand[nt]->children_[it]);
00475                     }
00476                 }
00477             }
00478         }
00479 
00480         toExpand.clear();
00481     }
00482 
00483 }
00484 
00487 template <typename PointT>
00488 bool OctTree<PointT>::intersectSphere(PointT center_, const double &radius) const
00489 {
00490 
00491     PointT mcenter_ = myCell_->getCenter();
00492     Eigen::Vector3d d;
00493     d<<center_.x-mcenter_.x, center_.y-mcenter_.y, center_.z-mcenter_.z;
00494     double dist = d.norm();
00495     Eigen::Vector3d localRadius;
00496     myCell_->getDimensions(localRadius(0),localRadius(1),localRadius(2));
00497     double lRad = localRadius.norm()/2;
00498     double interDist = lRad+radius;
00499     return (interDist>dist);
00500 
00501 }
00502 
00503 template <typename PointT>
00504 Cell<PointT>* OctTree<PointT>::getClosestLeafCell(const PointT &point)
00505 {
00506     if(this->leaf_ && myCell_!= NULL)
00507     {
00508         if(myCell_->isInside(point))
00509         {
00510             return myCell_;
00511         }
00512     }
00513     else
00514     {
00515         size_t ind = getIndexForPoint(point);
00516         if(children_[ind]!=NULL)
00517         {
00518             return children_[ind]->getClosestLeafCell(point);
00519         }
00520         else
00521         {
00522             //the leaf we should be in is empty
00523             //start from here and find closest neighbor
00524             double minDist = INT_MAX;
00525             int index = -1;
00526             for(int i=0; i<8; i++)
00527             {
00528                 if(children_[i]==NULL) continue;
00529                 PointT center = children_[i]->myCell_->getCenter();
00530                 Eigen::Vector3d d;
00531                 d <<center.x-point.x, center.y-point.y, center.z-point.z;
00532                 double dist = d.norm();
00533 
00534                 if(dist<minDist)
00535                 {
00536                     index = i;
00537                     minDist = dist;
00538                 }
00539             }
00540             //std::cout<<"minDist"<<minDist<<std::endl;
00541             if(index>=0 && index<8)
00542             {
00543                 return children_[index]->getClosestLeafCell(point);
00544             }
00545         }
00546     }
00547     return myCell_;
00548 }
00549 
00550 template <typename PointT>
00551 NDTCell<PointT>* OctTree<PointT>::getClosestNDTCell(const PointT &point)
00552 {
00553     if(this->leaf_)
00554     {
00555         //we have reached the bottom of the tree.
00556         //if we have a valid ndt cell, return it.
00557         if(myCell_->isInside(point))
00558         {
00559             NDTCell<PointT>* nd = dynamic_cast<NDTCell<PointT>*>(myCell_);
00560             if(nd!=NULL)
00561             {
00562                 if(nd->hasGaussian_)
00563                 {
00564                     return nd;
00565                 }
00566             }
00567         }
00568     }
00569 
00570     //we go down the tree recursively
00571     size_t ind = getIndexForPoint(point);
00572     if(children_[ind]!=NULL)
00573     {
00574         return children_[ind]->getClosestNDTCell(point);
00575     }
00576 
00577 
00578     //the leaf we should be in is empty
00579     //iterate through all leafs connected to current node, find closest ndt cell
00580     OctTree<PointT> *my_parent = this->parent_;
00581     OctTree<PointT> *me = this;
00582     typename SpatialIndex<PointT>::CellVectorItr it;
00583     NDTCell<PointT> *closest = NULL, *temp = NULL;
00584     double minDist = INT_MAX, dist = INT_MAX;
00585 
00586     while(true)
00587     {
00588         it = me->begin();
00589         while(it!=me->end())
00590         {
00591             temp = dynamic_cast<NDTCell<PointT>*> (*it);
00592             if(temp!=NULL)
00593             {
00594                 if(temp->hasGaussian_)
00595                 {
00596                     dist = lslgeneric::geomDist<PointT>(temp->getCenter(),point);
00597                     if(dist < minDist)
00598                     {
00599                         minDist = dist;
00600                         closest = temp;
00601                     }
00602                 }
00603             }
00604             it++;
00605         }
00606         if(closest!=NULL)
00607         {
00608 //          cout<<"got it!\n";
00609             break;
00610         }
00611         if(my_parent != NULL)
00612         {
00613             me = my_parent;
00614             my_parent = me->parent_;
00615         }
00616         else
00617         {
00618             //nothing more can be done...
00619             break;
00620         }
00621     }
00622 
00623     return closest;
00624 }
00625 }


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