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)
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 )
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
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
00147 for(int i=0; i<sizeX; i++)
00148 {
00149 dataArray[i] = new Cell<PointT>**[sizeY];
00150
00151 for(int j=0; j<sizeY; j++)
00152 {
00153 dataArray[i][j] = new Cell<PointT>*[sizeZ];
00154
00155
00156 memset(dataArray[i][j],0,sizeZ*sizeof(Cell<PointT>*));
00157
00158 }
00159 }
00160 initialized = true;
00161 }
00162
00163 template <typename PointT>
00164 LazyGrid<PointT>::~LazyGrid()
00165 {
00166 if(initialized)
00167 {
00168
00169 int cnt = 0;
00170
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
00185 }
00186 delete[] dataArray[i];
00187
00188 }
00189 delete[] dataArray;
00190
00191 if(protoType!=NULL)
00192 {
00193 delete protoType;
00194 }
00195
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
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
00226
00227
00228
00229
00230
00231
00232
00233 int indX,indY,indZ;
00234
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
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
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
00267
00268
00269
00270
00271
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
00280
00281
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
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
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;
00405
00406
00407
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
00442
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
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;
00470
00471
00472
00473
00474
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
00498
00499
00500
00501
00502
00503
00504
00505
00506
00507
00508
00509
00510
00511
00512
00513
00514
00515
00516
00517
00518
00519
00520
00521
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
00532 std::vector<int> id;
00533 std::vector<float> dist;
00534 id.reserve(1);
00535 dist.reserve(1);
00536 const PointT pt(point);
00537
00538
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
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;
00572
00573
00574
00575
00576
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];
00698
00699
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
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
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
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
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
00817
00818
00819
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
00891
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
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 }