cell_vector.hpp
Go to the documentation of this file.
00001 #include <climits>
00002 namespace lslgeneric
00003 {
00004 template <typename PointT>
00005 CellVector<PointT>::CellVector():mp(new pcl::PointCloud<PointT>())
00006 {
00007     protoType= new NDTCell<PointT>();
00008     treeUpdated = false;
00009 }
00010 
00011 template <typename PointT>
00012 CellVector<PointT>::CellVector(Cell<PointT>* cellPrototype):mp(new pcl::PointCloud<PointT>())
00013 {
00014     protoType = cellPrototype->clone();
00015     treeUpdated = false;
00016 }
00017 
00018 template <typename PointT>
00019 CellVector<PointT>::~CellVector()
00020 {
00021     //go through all cells and delete the non-NULL ones
00022     for(unsigned int i=0; i<activeCells.size(); ++i)
00023     {
00024         if(activeCells[i]!=NULL)
00025         {
00026             delete activeCells[i];
00027         }
00028     }
00029 }
00030 
00031 template <typename PointT>
00032 Cell<PointT>* CellVector<PointT>::getCellForPoint(const PointT &point)
00033 {
00034     Cell<PointT>* ret = NULL;
00035     if (treeUpdated)
00036     {
00037         std::vector<int> id;
00038         std::vector<float> dist;
00039         int NCELLS = 1;
00040         id.reserve(NCELLS);
00041         dist.reserve(NCELLS);
00042         const PointT pt(point);
00043         if(!meansTree.nearestKSearch(pt,NCELLS,id,dist)) return ret;
00044 
00045         Cell<PointT>* tmp = activeCells[id[0]];
00046         ret = static_cast<NDTCell<PointT>*>(tmp);
00047     }
00048     else
00049     {
00050         float min_dist = std::numeric_limits<float>::max( );
00051         typename SpatialIndex<PointT>::CellVectorItr it = this->begin();
00052         while(it!=this->end())
00053         {
00054             float tmp=pcl::squaredEuclideanDistance((*it)->getCenter(), point);
00055             if (tmp < min_dist)
00056             {
00057                 min_dist = tmp;
00058                 ret = (*it);
00059             }
00060             it++;
00061         }
00062     }
00063     return ret;
00064 }
00065 
00066 template <typename PointT>
00067 Cell<PointT>* CellVector<PointT>::addPoint(const PointT &point)
00068 {
00069     return NULL;
00070     // Do nothing...
00071 }
00072 
00073 template <typename PointT>
00074 void CellVector<PointT>::addCellPoints(pcl::PointCloud<PointT> pc, const std::vector<size_t> &indices)
00075 {
00076     activeCells.push_back(protoType->clone());
00077     for (size_t i = 0; i < indices.size(); i++)
00078         (activeCells.back())->addPoint(pc[indices[i]]); // Add the point to the cell.
00079     treeUpdated = false;
00080 }
00081 
00082 
00083 template <typename PointT>
00084 void CellVector<PointT>::addCell(Cell<PointT>* cell)
00085 {
00086     activeCells.push_back(cell);
00087 }
00088 
00089 template <typename PointT>
00090 void CellVector<PointT>::addNDTCell(NDTCell<PointT>* cell)
00091 {
00092     this->addCell(static_cast<Cell<PointT>*>(cell));
00093 }
00094 
00095 template <typename PointT>
00096 typename SpatialIndex<PointT>::CellVectorItr CellVector<PointT>::begin()
00097 {
00098     //cout<<"active cells "<<activeCells.size()<<endl;
00099     return activeCells.begin();
00100 }
00101 
00102 template <typename PointT>
00103 typename SpatialIndex<PointT>::CellVectorItr CellVector<PointT>::end()
00104 {
00105     return activeCells.end();
00106 }
00107 
00108 template <typename PointT>
00109 int CellVector<PointT>::size()
00110 {
00111     return activeCells.size();
00112 }
00113 
00114 template <typename PointT>
00115 SpatialIndex<PointT>* CellVector<PointT>::clone() const
00116 {
00117     return new CellVector<PointT>();
00118 }
00119 
00120 template <typename PointT>
00121 SpatialIndex<PointT>* CellVector<PointT>::copy() const
00122 {
00123     //std::cout<<"COPY CELL VECTOR\n";
00124     //assert(false); // This needs to be updated!
00125     CellVector<PointT> *ret = new CellVector<PointT>();
00126     for(unsigned int i =0; i< activeCells.size(); i++)
00127     {
00128         NDTCell<PointT>* r = dynamic_cast<NDTCell<PointT>*> (activeCells[i]->copy());
00129         if(r == NULL) continue;
00130         for(size_t i=0; i<r->points_.size(); i++)
00131         {
00132             ret->activeCells.push_back(r->copy());
00133         }
00134     }
00135     return ret;
00136 }
00137 
00138 template <typename PointT>
00139 void CellVector<PointT>::getNeighbors(const PointT &point, const double &radius, std::vector<Cell<PointT>*> &cells)
00140 {
00141 
00142     if (treeUpdated)
00143     {
00144         std::vector<int> id;
00145         std::vector<float> dist;
00146         int NCELLS = 4;
00147         id.reserve(NCELLS);
00148         dist.reserve(NCELLS);
00149         const PointT pt(point);
00150 
00151         if(!meansTree.nearestKSearch(pt,NCELLS,id,dist)) return;
00152 
00153         for(int i=0; i<NCELLS; i++)
00154         {
00155             Cell<PointT>* tmp = activeCells[id[i]];
00156             if (tmp != NULL)
00157                 cells.push_back(tmp);
00158         }
00159     }
00160     else
00161     {
00162         float radius_sqr = radius*radius;
00163         typename SpatialIndex<PointT>::CellVectorItr it = this->begin();
00164         while(it!=this->end())
00165         {
00166             float tmp=pcl::squaredEuclideanDistance((*it)->getCenter(), point);
00167             if (tmp < radius_sqr)
00168             {
00169                 cells.push_back(*it);
00170             }
00171         }
00172     }
00173 }
00174 
00175 template <typename PointT>
00176 void CellVector<PointT>::initKDTree()
00177 {
00178 
00179     NDTCell<PointT>* ndcell = NULL;
00180     PointT curr;
00181     Eigen::Vector3d m;
00182     pcl::PointCloud<PointT> mc;
00183 
00184     for(size_t i=0; i<activeCells.size(); i++)
00185     {
00186         ndcell = dynamic_cast<NDTCell<PointT>*> (activeCells[i]);
00187         if(ndcell == NULL) continue;
00188         if(!ndcell->hasGaussian_) continue;
00189         m = ndcell->getMean();
00190         curr.x = m(0);
00191         curr.y = m(1);
00192         curr.z = m(2);
00193         mc.push_back(curr);
00194     }
00195 
00196     if(mc.points.size() > 0)
00197     {
00198         *mp = mc;
00199         meansTree.setInputCloud(mp);
00200     }
00201 
00202     //++++++++++++++++++treeUpdated = true;
00203 }
00204 
00205 template <typename PointT>
00206 void CellVector<PointT>::setCellType(Cell<PointT> *type)
00207 {
00208     if(type!=NULL)
00209     {
00210         protoType = type->clone();
00211     }
00212 }
00213 
00214 template <typename PointT>
00215 NDTCell<PointT>* CellVector<PointT>::getClosestNDTCell(const PointT &point)
00216 {
00217 
00218     Cell<PointT>* tmp = getCellForPoint(point);
00219     NDTCell<PointT>* ret = dynamic_cast<NDTCell<PointT>*>(tmp);
00220     return ret;
00221 }
00222 
00223 template <typename PointT>
00224 std::vector<NDTCell<PointT>*> CellVector<PointT>::getClosestNDTCells(const PointT &point, double &radius)
00225 {
00226 
00227     std::vector<NDTCell<PointT>*> ret;
00228     std::vector<Cell<PointT>*> cells;
00229     getNeighbors(point, radius, cells);
00230     for (size_t i = 0; i < cells.size(); i++)
00231     {
00232         NDTCell<PointT>* tmp = dynamic_cast<NDTCell<PointT>*>(cells[i]);
00233         if (tmp != NULL)
00234         {
00235             ret.push_back(tmp);
00236         }
00237     }
00238     return ret;
00239 }
00240 
00241 template <typename PointT>
00242 NDTCell<PointT>*
00243 CellVector<PointT>::getCellIdx(unsigned int idx) const
00244 {
00245     if (idx >= activeCells.size())
00246         return NULL;
00247     NDTCell<PointT>* tmp = dynamic_cast<NDTCell<PointT>*>(activeCells[idx]);
00248     if (tmp != NULL)
00249     {
00250         return tmp;
00251     }
00252     return NULL;
00253 }
00254 
00255 template <typename PointT>
00256 void CellVector<PointT>::cleanCellsAboveSize(double size)
00257 {
00258     //clean cells with variance more then x meters in any direction
00259     Eigen::Vector3d evals;
00260     lslgeneric::SpatialIndex<pcl::PointXYZ>::CellVectorItr it = this->begin();
00261     lslgeneric::SpatialIndex<pcl::PointXYZ>::CellVectorItr it_tmp;
00262     while(it!=this->end())
00263     {
00264         lslgeneric::NDTCell<pcl::PointXYZ> *ndcell = dynamic_cast<lslgeneric::NDTCell<pcl::PointXYZ>* >(*it);
00265         if(ndcell != NULL)
00266         {
00267             if(ndcell->hasGaussian_)
00268             {
00269                 evals = ndcell->getEvals();
00270                 if(sqrt(evals(2)) < size)
00271                 {
00272                     it++;
00273                     continue;
00274                 }
00275                 //std::cout<<"rem cell at "<<ndcell->getMean().transpose()<<" evals are "<<evals.transpose()<<std::endl;
00276                 ndcell->hasGaussian_ = false;
00277             }
00278             delete ndcell;
00279             ndcell = NULL;
00280         }
00281         it_tmp = it;
00282         it_tmp--;
00283         this->activeCells.erase(it);
00284         it = it_tmp;
00285         it++;
00286     }
00287 
00288 }
00289 template <typename PointT>
00290 int CellVector<PointT>::loadFromJFF(FILE * jffin)
00291 {
00292     NDTCell<PointT> prototype_;
00293     if(fread(&prototype_, sizeof(Cell<PointT>), 1, jffin) <= 0)
00294     {
00295         JFFERR("reading prototype_ failed");
00296     }
00297     protoType = prototype_.clone();
00298     // load all cells
00299     while (1)
00300     {
00301         if(prototype_.loadFromJFF(jffin) < 0)
00302         {
00303             if(feof(jffin))
00304             {
00305                 break;
00306             }
00307             else
00308             {
00309                 JFFERR("loading cell failed");
00310             }
00311         }
00312 
00313         if(!feof(jffin))
00314         {
00315             // std::cout << prototype_.getOccupancy() << std::endl; /* for debugging */
00316         }
00317         else
00318         {
00319             break;
00320         }
00321         //initialize cell
00322         this->addCell(prototype_.copy());
00323     }
00324 
00325     this->initKDTree();
00326 
00327     return 0;
00328 }
00329 
00330 }


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