lazy_grid.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 template <typename PointT>
00010 LazyGrid<PointT>::LazyGrid(double cellSize) //:mp(new pcl::PointCloud<PointT>())
00011 {
00012     initialized = false;
00013     centerIsSet = false;
00014     sizeIsSet = false;
00015     cellSizeX = cellSizeY = cellSizeZ = cellSize;
00016 
00017 }
00018 
00019 template <typename PointT>
00020 LazyGrid<PointT>::LazyGrid(LazyGrid<PointT> *prot)
00021 {
00022 
00023     sizeXmeters = prot->sizeXmeters;
00024     sizeYmeters = prot->sizeYmeters;
00025     sizeZmeters = prot->sizeZmeters;
00026 
00027     cellSizeX = prot->cellSizeX;
00028     cellSizeY = prot->cellSizeY;
00029     cellSizeZ = prot->cellSizeZ;
00030 
00031     sizeX = abs(ceil(sizeXmeters/cellSizeX));
00032     sizeY = abs(ceil(sizeYmeters/cellSizeY));
00033     sizeZ = abs(ceil(sizeZmeters/cellSizeZ));
00034 
00035     centerX = prot->centerX;
00036     centerY = prot->centerY;
00037     centerZ = prot->centerZ;
00038 
00039     protoType = prot->protoType->clone();
00040     initialize();
00041 
00042 }
00043 
00044 template <typename PointT>
00045 LazyGrid<PointT>::LazyGrid(double _sizeXmeters, double _sizeYmeters, double _sizeZmeters,
00046                            double _cellSizeX, double _cellSizeY, double _cellSizeZ,
00047                            double _centerX, double _centerY, double _centerZ,
00048                            Cell<PointT> *cellPrototype ) //:mp(new pcl::PointCloud<PointT>())
00049 {
00050 
00051     sizeXmeters = _sizeXmeters;
00052     sizeYmeters = _sizeYmeters;
00053     sizeZmeters = _sizeZmeters;
00054 
00055     cellSizeX = _cellSizeX;
00056     cellSizeY = _cellSizeY;
00057     cellSizeZ = _cellSizeZ;
00058 
00059     sizeX = abs(ceil(sizeXmeters/cellSizeX));
00060     sizeY = abs(ceil(sizeYmeters/cellSizeY));
00061     sizeZ = abs(ceil(sizeZmeters/cellSizeZ));
00062 
00063     centerX = _centerX;
00064     centerY = _centerY;
00065     centerZ = _centerZ;
00066 
00067     protoType = cellPrototype->clone();
00068     initialize();
00069 
00070 
00071 }
00072 
00073 template <typename PointT>
00074 void LazyGrid<PointT>::setCenter(const double &cx, const double &cy, const double &cz)
00075 {
00076     centerX = cx;
00077     centerY = cy;
00078     centerZ = cz;
00079 
00080     centerIsSet = true;
00081     if(sizeIsSet)
00082     {
00083         initialize();
00084     }
00085 }
00086 
00087 template <typename PointT>
00088 void LazyGrid<PointT>::setSize(const double &sx, const double &sy, const double &sz)
00089 {
00090 
00091     sizeXmeters = sx;
00092     sizeYmeters = sy;
00093     sizeZmeters = sz;
00094 
00095     sizeX = abs(ceil(sizeXmeters/cellSizeX));
00096     sizeY = abs(ceil(sizeYmeters/cellSizeY));
00097     sizeZ = abs(ceil(sizeZmeters/cellSizeZ));
00098 
00099     sizeIsSet = true;
00100     if(centerIsSet)
00101     {
00102         initialize();
00103     }
00104 }
00105 
00106 template <typename PointT>
00107 void LazyGrid<PointT>::initializeAll()
00108 {
00109     if(!initialized)
00110     {
00111         this->initialize();
00112     }
00113     PointT centerCell;
00114     for(int i=0; i<sizeX; i++)
00115     {
00116         for(int j=0; j<sizeY; j++)
00117         {
00118             for(int k=0; k<sizeZ; k++)
00119             {
00120                 dataArray[i][j][k] = new NDTCell<PointT>();
00121 
00122                 //dataArray[i][j][k] = protoType->clone(); ///FIXME WHO WHAT WHERE AND WHY??
00123                 dataArray[i][j][k]->setDimensions(cellSizeX,cellSizeY,cellSizeZ);
00124 
00125                 int idcX, idcY, idcZ;
00126                 PointT center;
00127                 center.x = centerX;
00128                 center.y = centerY;
00129                 center.z = centerZ;
00130                 this->getIndexForPoint(center, idcX,idcY,idcZ);
00131                 centerCell.x = centerX + (i-idcX)*cellSizeX;
00132                 centerCell.y = centerY + (j-idcY)*cellSizeY;
00133                 centerCell.z = centerZ + (k-idcZ)*cellSizeZ;
00134                 dataArray[i][j][k]->setCenter(centerCell);
00135                 activeCells.push_back(dataArray[i][j][k]);
00136             }
00137         }
00138     }
00139 }
00140 
00141 template <typename PointT>
00142 void LazyGrid<PointT>::initialize()
00143 {
00144 
00145     dataArray = new Cell<PointT>***[sizeX];
00146     //linkedCells = new bool**[sizeX];
00147     for(int i=0; i<sizeX; i++)
00148     {
00149         dataArray[i] = new Cell<PointT>**[sizeY];
00150         //linkedCells[i] = new bool*[sizeY];
00151         for(int j=0; j<sizeY; j++)
00152         {
00153             dataArray[i][j] = new Cell<PointT>*[sizeZ];
00154             //linkedCells[i][j] = new bool[sizeZ];
00155             //set all cells to NULL
00156             memset(dataArray[i][j],0,sizeZ*sizeof(Cell<PointT>*));
00157             //memset(linkedCells[i][j],0,sizeZ*sizeof(bool));
00158         }
00159     }
00160     initialized = true;
00161 }
00162 
00163 template <typename PointT>
00164 LazyGrid<PointT>::~LazyGrid()
00165 {
00166     if(initialized)
00167     {
00168                                 //fprintf(stderr,"LAZY GRID DESTRUCTION -- ");
00169                                 int cnt = 0;
00170         //go through all cells and delete the non-NULL ones
00171         for(unsigned int i=0; i<activeCells.size(); ++i)
00172         {
00173             if(activeCells[i])
00174             {
00175                 delete activeCells[i];
00176                                                                 cnt++;
00177             }
00178         }
00179         for(int i=0; i<sizeX; i++)
00180         {
00181             for(int j=0; j<sizeY; j++)
00182             {
00183                 delete[] dataArray[i][j];
00184                 //delete[] linkedCells[i][j];
00185             }
00186             delete[] dataArray[i];
00187             //delete[] linkedCells[i];
00188         }
00189         delete[] dataArray;
00190         //delete[] linkedCells;
00191         if(protoType!=NULL)
00192         {
00193             delete protoType;
00194         }
00195         //fprintf(stderr,"Deleted %d cells and array of (%d x %d)!!!\n",cnt, sizeX, sizeY);
00196     }
00197 }
00198 
00199 template <typename PointT>
00200 Cell<PointT>* LazyGrid<PointT>::getCellForPoint(const PointT &point)
00201 {
00202 
00203     int indX,indY,indZ;
00204     this->getIndexForPoint(point,indX,indY,indZ);
00205 
00206     if(indX >= sizeX || indY >= sizeY || indZ >= sizeZ || indX<0 || indY<0 || indZ<0) return NULL;
00207     if(!initialized) return NULL;
00208     if(dataArray == NULL) return NULL;
00209     if(dataArray[indX] == NULL) return NULL;
00210     if(dataArray[indX][indY] == NULL) return NULL;
00211 
00212     //    cout<<"LZ: "<<indX<<" "<<indY<<" "<<indZ<<endl;
00213     return dataArray[indX][indY][indZ];
00214 }
00215 
00216 template <typename PointT>
00217 Cell<PointT>* LazyGrid<PointT>::addPoint(const PointT &point_c)
00218 {
00219 
00220     PointT point = point_c;
00221     if(std::isnan(point.x) ||std::isnan(point.y) ||std::isnan(point.z))
00222     {
00223         return NULL;
00224     }
00225     /*    int *idX,*idY,*idZ;
00226       idX = new int[4];
00227       idY = new int[4];
00228       idZ = new int[4];
00229 
00230       this->getIndexArrayForPoint(point,idX,idY,idZ);
00231      */
00232     // for(int i=0; i<4; i++) {
00233     int indX,indY,indZ;
00234     //indX = idX[i]; indZ = idZ[i]; indY = idY[i];
00235     this->getIndexForPoint(point,indX,indY,indZ);
00236     PointT centerCell;
00237 
00238     if(indX >= sizeX || indY >= sizeY || indZ >= sizeZ || indX<0 || indY<0 || indZ<0)
00239     {
00240         //continue;
00241         return NULL;
00242     }
00243 
00244     if(!initialized) return NULL;
00245     if(dataArray == NULL) return NULL;
00246     if(dataArray[indX] == NULL) return NULL;
00247     if(dataArray[indX][indY] == NULL) return NULL;
00248 
00249     if(dataArray[indX][indY][indZ]==NULL)
00250     {
00251         //initialize cell
00252         dataArray[indX][indY][indZ] = protoType->clone();
00253         dataArray[indX][indY][indZ]->setDimensions(cellSizeX,cellSizeY,cellSizeZ);
00254 
00255         int idcX, idcY, idcZ;
00256         PointT center;
00257         center.x = centerX;
00258         center.y = centerY;
00259         center.z = centerZ;
00260         this->getIndexForPoint(center, idcX,idcY,idcZ);
00261         centerCell.x = centerX + (indX-idcX)*cellSizeX;
00262         centerCell.y = centerY + (indY-idcY)*cellSizeY;
00263         centerCell.z = centerZ + (indZ-idcZ)*cellSizeZ;
00264         dataArray[indX][indY][indZ]->setCenter(centerCell);
00265         /*
00266            cout<<"center: "<<centerX<<" "<<centerY<<" "<<centerZ<<endl;
00267            cout<<"size  : "<<sizeX<<" "<<sizeY<<" "<<sizeZ<<endl;
00268            cout<<"p  : "<<point.x<<" "<<point.y<<" "<<point.z<<endl;
00269            cout<<"c  : "<<centerCell.x<<" "<<centerCell.y<<" "<<centerCell.z<<endl;
00270            cout<<"id : "<<indX<<" "<<indY<<" "<<indZ<<endl;
00271            cout<<"cs : "<<cellSizeX<<" "<<cellSizeY<<" "<<cellSizeZ<<endl;
00272          */
00273         activeCells.push_back(dataArray[indX][indY][indZ]);
00274     }
00275     dataArray[indX][indY][indZ]->addPoint(point);
00276     return dataArray[indX][indY][indZ];
00277     //}
00278     /*
00279        delete []idX;
00280        delete []idY;
00281        delete []idZ; */
00282 }
00283 
00284 template <typename PointT>
00285 typename SpatialIndex<PointT>::CellVectorItr LazyGrid<PointT>::begin()
00286 {
00287     return activeCells.begin();
00288 }
00289 
00290 template <typename PointT>
00291 typename SpatialIndex<PointT>::CellVectorItr LazyGrid<PointT>::end()
00292 {
00293     return activeCells.end();
00294 }
00295 
00296 template <typename PointT>
00297 int LazyGrid<PointT>::size()
00298 {
00299     return activeCells.size();
00300 }
00301 
00302 template <typename PointT>
00303 SpatialIndex<PointT>* LazyGrid<PointT>::clone() const
00304 {
00305     return new LazyGrid<PointT>(cellSizeX);
00306 }
00307 
00308 template <typename PointT>
00309 SpatialIndex<PointT>* LazyGrid<PointT>::copy() const
00310 {
00311     LazyGrid<PointT> *ret = new LazyGrid<PointT>(cellSizeX);
00312     typename std::vector<Cell<PointT>*>::const_iterator it;
00313     it = activeCells.begin();
00314     while(it!=activeCells.end())
00315     {
00316         NDTCell<PointT>* r = dynamic_cast<NDTCell<PointT>*> (*it);
00317         if(r == NULL) continue;
00318         for(unsigned int i=0; i<r->points_.size(); i++)
00319         {
00320             ret->addPoint(r->points_[i]);
00321         }
00322         it++;
00323     }
00324     return ret;
00325 }
00326 
00327 template <typename PointT>
00328 void LazyGrid<PointT>::getNeighbors(const PointT &point, const double &radius, std::vector<Cell<PointT>*> &cells)
00329 {
00330 
00331     int indX,indY,indZ;
00332     this->getIndexForPoint(point, indX,indY,indZ);
00333     if(indX >= sizeX || indY >= sizeY || indZ >= sizeZ)
00334     {
00335         cells.clear();
00336         return;
00337     }
00338 
00339     for(int x = indX - radius/cellSizeX; x<indX+radius/cellSizeX; x++)
00340     {
00341         if(x < 0 || x >= sizeX) continue;
00342         for(int y = indY - radius/cellSizeY; y<indY+radius/cellSizeY; y++)
00343         {
00344             if(y < 0 || y >= sizeY) continue;
00345             for(int z = indZ - radius/cellSizeZ; z<indZ+radius/cellSizeZ; z++)
00346             {
00347                 if(z < 0 || z >= sizeZ) continue;
00348                 if(dataArray[x][y][z]==NULL) continue;
00349                 cells.push_back(dataArray[x][y][z]);
00350             }
00351         }
00352     }
00353 
00354 }
00355 
00356 template <typename PointT>
00357 inline void LazyGrid<PointT>::getIndexForPoint(const PointT& point, int &indX, int &indY, int &indZ)
00358 {
00359     indX = floor((point.x - centerX)/cellSizeX+0.5) + sizeX/2.0;
00360     indY = floor((point.y - centerY)/cellSizeY+0.5) + sizeY/2.0;
00361     indZ = floor((point.z - centerZ)/cellSizeZ+0.5) + sizeZ/2.0;
00362 }
00363 
00364 /*
00365    void LazyGrid<PointT>::getIndexArrayForPoint(const PointT& point, int* &indX, int* &indY, int *&indZ) {
00366    indX[0] = floor((point.x - centerX)/cellSizeX+0.5) + sizeX/2;
00367    indY[0] = floor((point.y - centerY)/cellSizeY+0.5) + sizeY/2;
00368    indZ[0] = floor((point.z - centerZ)/cellSizeZ+0.5) + sizeZ/2;
00369 
00370 //get center of the cell
00371 int idcX, idcY, idcZ;
00372 PointT center;
00373 center.x = centerX;
00374 center.y = centerY;
00375 center.z = centerZ;
00376 this->getIndexForPoint(center, idcX,idcY,idcZ);
00377 double centerCellx = centerX + (indX[0]-idcX)*cellSizeX;
00378 double centerCelly = centerY + (indY[0]-idcY)*cellSizeY;
00379 double centerCellz = centerZ + (indZ[0]-idcZ)*cellSizeZ;
00380 int pX,pY,pZ;
00381 pX = point.x - centerCellx > 0? 1 : -1;
00382 pY = point.y - centerCelly > 0? 1 : -1;
00383 pZ = point.z - centerCellz > 0? 1 : -1;
00384 indX[1] = indX[0]+pX;
00385 indY[1] = indY[0];
00386 indZ[1] = indZ[0];
00387 indX[2] = indX[0];
00388 indY[2] = indY[0]+pY;
00389 indZ[2] = indZ[0];
00390 indX[3] = indX[0];
00391 indY[3] = indY[0];
00392 indZ[3] = indZ[0]+pZ;
00393 }
00394  */
00395 template <typename PointT>
00396 std::vector<NDTCell<PointT>*> LazyGrid<PointT>::getClosestCells(const PointT &pt)
00397 {
00398     int indXn,indYn,indZn;
00399     int indX,indY,indZ;
00400     this->getIndexForPoint(pt,indX,indY,indZ);
00401     NDTCell<PointT> *ret = NULL;
00402     std::vector<NDTCell<PointT>*> cells;
00403 
00404     int i =2; //how many cells on each side
00405     //the strange thing for the indeces is for convenience of writing
00406     //basicly, we go through 2* the number of cells and use parity to
00407     //decide if we subtract or add. should work nicely
00408     for(int x=1; x<2*i+2; x++)
00409     {
00410         indXn = (x%2 == 0) ? indX+x/2 : indX-x/2;
00411         for(int y=1; y<2*i+2; y++)
00412         {
00413             indYn = (y%2 == 0) ? indY+y/2 : indY-y/2;
00414             for(int z=1; z<2*i+2; z++)
00415             {
00416                 indZn = (z%2 == 0) ? indZ+z/2 : indZ-z/2;
00417                 if(checkCellforNDT(indXn,indYn,indZn,true))
00418                 {
00419                     ret = dynamic_cast<NDTCell<PointT>*> (dataArray[indXn][indYn][indZn]);
00420                     cells.push_back(ret);
00421                 }
00422             }
00423         }
00424     }
00425     return cells;
00426 }
00427 
00428 template <typename PointT>
00429 std::vector<NDTCell<PointT>*> LazyGrid<PointT>::getClosestNDTCells(const PointT &point, int &n_neigh, bool checkForGaussian)
00430 {
00431 
00432 #if 0
00433     std::vector<int> id;
00434     std::vector<float> dist;
00435     int NCELLS = 4;
00436     id.reserve(NCELLS);
00437     dist.reserve(NCELLS);
00438     const PointT pt(point);
00439     std::vector<NDTCell<PointT>*> cells;
00440 
00441     //if(meansTree.input_.get()==NULL) { return cells;} ///FIXME::: This is needed
00442     //if(meansTree.input_->size() < 1) { return cells;} ///FIXME::: This is needed
00443     if(!meansTree.nearestKSearch(pt,NCELLS,id,dist))
00444     {
00445         return cells;
00446     }
00447     NDTCell<PointT> *ret = NULL;
00448 
00449     for(int i=0; i<NCELLS; i++)
00450     {
00451         //if(dist[i]>2*radius) continue;
00452         PointT close = mp->points[id[i]];
00453         int indX,indY,indZ;
00454         this->getIndexForPoint(close, indX,indY,indZ);
00455         if(checkCellforNDT(indX,indY,indZ))
00456         {
00457             ret = dynamic_cast<NDTCell<PointT>*> (dataArray[indX][indY][indZ]);
00458             cells.push_back(ret);
00459         }
00460     }
00461     return cells;
00462 #endif
00463     int indXn,indYn,indZn;
00464     int indX,indY,indZ;
00465     this->getIndexForPoint(point,indX,indY,indZ);
00466     NDTCell<PointT> *ret = NULL;
00467     std::vector<NDTCell<PointT>*> cells;
00468 
00469     int i = n_neigh; //how many cells on each side
00470 
00471     //for(int i=1; i<= maxNumberOfCells; i++) {
00472     //the strange thing for the indeces is for convenience of writing
00473     //basicly, we go through 2* the number of cells and use parity to
00474     //decide if we subtract or add. should work nicely
00475     for(int x=1; x<2*i+2; x++)
00476     {
00477         indXn = (x%2 == 0) ? indX+x/2 : indX-x/2;
00478         for(int y=1; y<2*i+2; y++)
00479         {
00480             indYn = (y%2 == 0) ? indY+y/2 : indY-y/2;
00481             for(int z=1; z<2*i+2; z++)
00482             {
00483                 indZn = (z%2 == 0) ? indZ+z/2 : indZ-z/2;
00484                 if(checkCellforNDT(indXn,indYn,indZn,checkForGaussian))
00485                 {
00486                     ret = dynamic_cast<NDTCell<PointT>*> (dataArray[indXn][indYn][indZn]);
00487                     cells.push_back(ret);
00488                 }
00489             }
00490         }
00491     }
00492 
00493     return cells;
00494 }
00495 
00496 /*
00497 template <typename PointT>
00498 void LazyGrid<PointT>::initKDTree()
00499 {
00500 
00501     NDTCell<PointT>* ndcell = NULL;
00502     PointT curr;
00503     Eigen::Vector3d m;
00504     pcl::PointCloud<PointT> mc;
00505 
00506     for(unsigned int i=0; i<activeCells.size(); i++)
00507     {
00508         ndcell = dynamic_cast<NDTCell<PointT>*> (activeCells[i]);
00509         if(ndcell == NULL) continue;
00510         if(!ndcell->hasGaussian_) continue;
00511         m = ndcell->getMean();
00512         curr.x = m(0);
00513         curr.y = m(1);
00514         curr.z = m(2);
00515         mc.push_back(curr);
00516     }
00517 
00518     if(mc.points.size() > 0)
00519     {
00520         *mp = mc;
00521         meansTree.setInputCloud(mp);
00522     }
00523 
00524 }*/
00525 
00526 template <typename PointT>
00527 NDTCell<PointT>* LazyGrid<PointT>::getClosestNDTCell(const PointT &point, bool checkForGaussian)
00528 {
00529 
00530 #if 0
00531     //KD tree based old stuff
00532     std::vector<int> id;
00533     std::vector<float> dist;
00534     id.reserve(1);
00535     dist.reserve(1);
00536     const PointT pt(point);
00537     // if(meansTree.input_.get()==NULL) {return NULL;} ///FIXME::: This is needed
00538     // if(meansTree.input_->size() < 1) {return NULL;} ///FIXME::: This is needed
00539     if(!meansTree.nearestKSearch(pt,1,id,dist))
00540     {
00541         return NULL;
00542     }
00543     PointT close = mp->points[id[0]];
00544 
00545     NDTCell<PointT> *ret = NULL;
00546     int indX,indY,indZ;
00547     this->getIndexForPoint(close, indX,indY,indZ);
00548     if(checkCellforNDT(indX,indY,indZ))
00549     {
00550         ret = dynamic_cast<NDTCell<PointT>*> (dataArray[indX][indY][indZ]);
00551     }
00552     return ret;
00553 #endif
00554 
00555     int indXn,indYn,indZn;
00556     int indX,indY,indZ;
00557     this->getIndexForPoint(point,indX,indY,indZ);
00558     NDTCell<PointT> *ret = NULL;
00559     std::vector<NDTCell<PointT>*> cells;
00560 
00561     if(!checkForGaussian)
00562     {
00563         //just give me whatever is in this cell
00564         if(checkCellforNDT(indX,indY,indZ,checkForGaussian))
00565         {
00566             ret = dynamic_cast<NDTCell<PointT>*> (dataArray[indX][indY][indZ]);
00567         }
00568         return ret;
00569     }
00570 
00571     int i =1; //how many cells on each side
00572 
00573     //for(int i=1; i<= maxNumberOfCells; i++) {
00574     //the strange thing for the indeces is for convenience of writing
00575     //basicly, we go through 2* the number of cells and use parity to
00576     //decide if we subtract or add. should work nicely
00577     for(int x=1; x<2*i+2; x++)
00578     {
00579         indXn = (x%2 == 0) ? indX+x/2 : indX-x/2;
00580         for(int y=1; y<2*i+2; y++)
00581         {
00582             indYn = (y%2 == 0) ? indY+y/2 : indY-y/2;
00583             for(int z=1; z<2*i+2; z++)
00584             {
00585                 indZn = (z%2 == 0) ? indZ+z/2 : indZ-z/2;
00586                 if(checkCellforNDT(indXn,indYn,indZn))
00587                 {
00588                     ret = dynamic_cast<NDTCell<PointT>*> (dataArray[indXn][indYn][indZn]);
00589                     cells.push_back(ret);
00590                 }
00591             }
00592         }
00593     }
00594 
00595     double minDist = INT_MAX;
00596     Eigen::Vector3d tmean;
00597     PointT pt = point;
00598     for(unsigned int i=0; i<cells.size(); i++)
00599     {
00600         tmean = cells[i]->getMean();
00601         tmean(0) -= pt.x;
00602         tmean(1) -= pt.y;
00603         tmean(2) -= pt.z;
00604         double d = tmean.norm();
00605         if(d<minDist)
00606         {
00607             minDist = d;
00608             ret = cells[i];
00609         }
00610     }
00611     cells.clear();
00612     return ret;
00613 }
00614 
00615 template <typename PointT>
00616 bool LazyGrid<PointT>::checkCellforNDT(int indX, int indY, int indZ, bool checkForGaussian)
00617 {
00618 
00619     if(indX < sizeX && indY < sizeY && indZ < sizeZ &&
00620             indX >=0 && indY >=0 && indZ >=0)
00621     {
00622         if(dataArray[indX][indY][indZ]!=NULL)
00623         {
00624             NDTCell<PointT>* ret = dynamic_cast<NDTCell<PointT>*> (dataArray[indX][indY][indZ]);
00625             if(ret!=NULL)
00626             {
00627                 if(ret->hasGaussian_ || (!checkForGaussian))
00628                 {
00629                     return true;
00630                 }
00631             }
00632         }
00633     }
00634     return false;
00635 }
00636 
00637 template <typename PointT>
00638 void LazyGrid<PointT>::setCellType(Cell<PointT> *type)
00639 {
00640     if(type!=NULL)
00641     {
00642         protoType = type->clone();
00643     }
00644 }
00645 
00646 
00647 #if 0
00648 template <typename PointT>
00649 bool LazyGrid<PointT>::getLinkedAt(int indX, int indY, int indZ)
00650 {
00651     if(indX < sizeX && indY < sizeY && indZ < sizeZ &&
00652             indX >=0 && indY >=0 && indZ >=0)
00653     {
00654         return linkedCells[indX][indY][indZ];
00655     }
00656     return false;
00657 
00658 }
00659 #endif
00660 
00661 template <typename PointT>
00662 void LazyGrid<PointT>::getCellSize(double &cx, double &cy, double &cz)
00663 {
00664     cx = cellSizeX;
00665     cy = cellSizeY;
00666     cz = cellSizeZ;
00667 }
00668 
00669 template <typename PointT>
00670 void LazyGrid<PointT>::getCenter(double &cx, double &cy, double &cz)
00671 {
00672     cx = centerX;
00673     cy = centerY;
00674     cz = centerZ;
00675 
00676 }
00677 
00678 template <typename PointT>
00679 void LazyGrid<PointT>::getGridSize(int &cx, int &cy, int &cz)
00680 {
00681     cx = sizeX;
00682     cy = sizeY;
00683     cz = sizeZ;
00684 }
00685 
00686 template <typename PointT>
00687 void LazyGrid<PointT>::getGridSizeInMeters(double &cx, double &cy, double &cz)
00688 {
00689     cx = sizeXmeters;
00690     cy = sizeYmeters;
00691     cz = sizeZmeters;
00692 }
00693 
00694 template <typename PointT>
00695 int LazyGrid<PointT>::loadFromJFF(FILE * jffin)
00696 {
00697     double lazyGridData[9]; // = { sizeXmeters, sizeYmeters, sizeZmeters,
00698     //     cellSizeX,   cellSizeY,   cellSizeZ,
00699     //     centerX,     centerY,     centerZ };
00700     NDTCell<PointT> prototype_;
00701     if(fread(&lazyGridData, sizeof(double), 9, jffin) <= 0)
00702     {
00703         JFFERR("reading lazyGridData failed");
00704     }
00705     if(fread(&prototype_, sizeof(Cell<PointT>), 1, jffin) <= 0)
00706     {
00707         JFFERR("reading prototype_ failed");
00708     }
00709 
00710     // just in case someone was messing around with the new NDTMap
00711     centerIsSet = false;
00712     sizeIsSet = false;
00713 
00714     protoType = prototype_.clone();
00715 
00716     this->setSize(lazyGridData[0], lazyGridData[1], lazyGridData[2]);
00717 
00718     cellSizeX = lazyGridData[3];
00719     cellSizeY = lazyGridData[4];
00720     cellSizeZ = lazyGridData[5];
00721 
00722     this->setCenter(lazyGridData[6], lazyGridData[7], lazyGridData[8]);
00723 
00724     this->initializeAll();
00725     int indX, indY, indZ;
00726     float r,g,b;
00727     double xs,ys,zs;
00728     PointT centerCell;
00729     float occ;
00730     unsigned int N;
00731 
00732     // load all cells
00733     while (1)
00734     {
00735         if(prototype_.loadFromJFF(jffin) < 0)
00736         {
00737             if(feof(jffin))
00738             {
00739                 break;
00740             }
00741             else
00742             {
00743                 JFFERR("loading cell failed");
00744             }
00745         }
00746 
00747         if(!feof(jffin))
00748         {
00749             // std::cout << prototype_.getOccupancy() << std::endl; /* for debugging */
00750         }
00751         else
00752         {
00753             break;
00754         }
00755         centerCell = prototype_.getCenter();
00756         this->getIndexForPoint(centerCell, indX, indY, indZ);
00757         if(indX < 0 || indX >= sizeX) continue; 
00758         if(indY < 0 || indY >= sizeY) continue; 
00759         if(indZ < 0 || indZ >= sizeZ) continue; 
00760         if(!initialized) return -1;
00761         if(dataArray == NULL) return -1;
00762         if(dataArray[indX] == NULL) return -1;
00763         if(dataArray[indX][indY] == NULL) return -1;
00764 
00765         if(dataArray[indX][indY][indZ] != NULL)
00766         {
00767             NDTCell<PointT>* ret = dynamic_cast<NDTCell<PointT>*>(dataArray[indX][indY][indZ]);
00768             prototype_.getRGB(r,g,b);
00769             prototype_.getDimensions(xs,ys,zs);
00770 
00771             ret->setDimensions(xs,ys,zs);
00772             ret->setCenter(centerCell);
00773             ret->setMean(prototype_.getMean());
00774             ret->setCov(prototype_.getCov());
00775             ret->setRGB(r,g,b);
00776             ret->setOccupancy(prototype_.getOccupancy());
00777             ret->setEmptyval(prototype_.getEmptyval());
00778             ret->setEventData(prototype_.getEventData());
00779             ret->setN(prototype_.getN());
00780             ret->isEmpty = prototype_.isEmpty;
00781             ret->hasGaussian_ = prototype_.hasGaussian_;
00782             ret->consistency_score = prototype_.consistency_score;
00783 
00784         } else {
00785             //initialize cell
00786             std::cerr<<"NEW CELL\n";
00787             dataArray[indX][indY][indZ] = prototype_.copy();
00788             activeCells.push_back(dataArray[indX][indY][indZ]);
00789         }
00790     }
00791 
00792     return 0;
00793 }
00794 
00795 template <typename PointT>
00796 inline
00797 bool LazyGrid<PointT>::traceLine(const Eigen::Vector3d &origin, const PointT &endpoint,const Eigen::Vector3d &diff_ ,
00798                                                                                                                                         const double& maxz, std::vector<NDTCell<PointT>*> &cells)
00799 {
00800         if(endpoint.z>maxz)
00801         {
00802                 return false;
00803         }
00804         
00805         double min1 = std::min(cellSizeX,cellSizeY);
00806         double min2 = std::min(cellSizeZ,cellSizeY);
00807         double resolution = std::min(min1,min2); 
00808         
00809         if(resolution<0.01)
00810         {
00811                 fprintf(stderr,"Resolution very very small (%lf) :( \n",resolution);
00812                 return false;
00813         }
00814         double l = diff_.norm();
00815         int N = l / (resolution);
00816         //if(N <= 0)
00817         //{
00818         //      //fprintf(stderr,"N=%d (r=%lf l=%lf) :( ",N,resolution,l);
00819         //      return false;
00820         //}
00821         
00822         NDTCell<PointT>* ptCell = NULL;    
00823         PointT pt;
00824         PointT po;
00825         po.x = origin(0); po.y = origin(1); po.z = origin(2);
00826         Eigen::Vector3d diff = diff_/(float)N;
00827         
00828         int idxo=0, idyo=0,idzo=0;
00829         for(int i=0; i<N-2; i++)
00830         {
00831                 pt.x = origin(0) + ((float)(i+1)) *diff(0);
00832                 pt.y = origin(1) + ((float)(i+1)) *diff(1);
00833                 pt.z = origin(2) + ((float)(i+1)) *diff(2);
00834                 int idx,idy,idz;
00835                 idx = floor((pt.x - centerX)/cellSizeX+0.5) + sizeX/2.0;
00836                 idy = floor((pt.y - centerY)/cellSizeY+0.5) + sizeY/2.0;
00837                 idz = floor((pt.z - centerZ)/cellSizeZ+0.5) + sizeZ/2.0;
00840                 if(idx == idxo && idy==idyo && idz ==idzo)
00841                 {
00842                         continue;
00843                 }
00844                 else
00845                 {
00846                         idxo = idx;
00847                         idyo = idy;
00848                         idzo = idz;
00849                 }
00850                 
00851                 if(idx < sizeX && idy < sizeY && idz < sizeZ && idx >=0 && idy >=0 && idz >=0){
00852                         ptCell = dynamic_cast<NDTCell<PointT> *> (dataArray[idx][idy][idz]);
00853                         if(ptCell !=NULL) {
00854                                 cells.push_back(ptCell);
00855                         } else {
00856                                 this->addPoint(pt); 
00857                         }
00858                 }
00859         }
00860         return true;
00861         
00862 }
00863 
00864 template <typename PointT>
00865 inline
00866 bool LazyGrid<PointT>::traceLineWithEndpoint(const Eigen::Vector3d &origin, const PointT &endpoint,const Eigen::Vector3d &diff_ ,const double& maxz, std::vector<NDTCell<PointT>*> &cells, Eigen::Vector3d &final_point)
00867 {
00868     if(endpoint.z>maxz)
00869     {
00870         return false;
00871     }
00872 
00873     double min1 = std::min(cellSizeX,cellSizeY);
00874     double min2 = std::min(cellSizeZ,cellSizeY);
00875     double resolution = std::min(min1,min2); 
00876 
00877     if(resolution<0.01)
00878     {
00879         fprintf(stderr,"Resolution very very small (%lf) :( \n",resolution);
00880         return false;
00881     }
00882     double l = diff_.norm();
00883     int N = l / (resolution);
00884     NDTCell<PointT>* ptCell = NULL;    
00885     PointT pt;
00886     PointT po;
00887     po.x = origin(0); po.y = origin(1); po.z = origin(2);
00888     if(N == 0)
00889     {
00890         //fprintf(stderr,"N=%d (r=%lf l=%lf) :( ",N,resolution,l);
00891         //return false;
00892         this->getNDTCellAt(po,ptCell);
00893         if(ptCell!=NULL) {
00894             cells.push_back(ptCell);
00895         }
00896         return true;
00897     }
00898 
00899     Eigen::Vector3d diff = diff_/(float)N;
00900 
00901     int idxo=0, idyo=0,idzo=0;
00902     bool complete = true;
00903     for(int i=0; i<N-2; i++)
00904     {
00905         pt.x = origin(0) + ((float)(i+1)) *diff(0);
00906         pt.y = origin(1) + ((float)(i+1)) *diff(1);
00907         pt.z = origin(2) + ((float)(i+1)) *diff(2);
00908         int idx,idy,idz;
00909         idx = floor((pt.x - centerX)/cellSizeX+0.5) + sizeX/2.0;
00910         idy = floor((pt.y - centerY)/cellSizeY+0.5) + sizeY/2.0;
00911         idz = floor((pt.z - centerZ)/cellSizeZ+0.5) + sizeZ/2.0;
00914         if(idx == idxo && idy==idyo && idz ==idzo)
00915         {
00916             continue;
00917         }
00918         else
00919         {
00920             idxo = idx;
00921             idyo = idy;
00922             idzo = idz;
00923         }
00924 
00925         if(idx < sizeX && idy < sizeY && idz < sizeZ && idx >=0 && idy >=0 && idz >=0){
00926             ptCell = dynamic_cast<NDTCell<PointT> *> (dataArray[idx][idy][idz]);
00927             if(ptCell !=NULL) {
00928                 cells.push_back(ptCell);
00929             } else {
00930                 this->addPoint(pt); 
00931             }
00932         } else {
00933             //out of the map, we won't be coming back any time soon
00934             complete = false;
00935             final_point = origin+(float(i))*diff;
00936             break;
00937         }
00938     }
00939     if(complete) final_point = origin+diff_;
00940     return true;
00941 
00942 }
00943 
00944 } //end namespace


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