lazy_grid.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, AASS Research Center, Orebro University.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of AASS Research Center nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  */
00035 #ifndef LSL_LAZZY_GRID_HH
00036 #define LSL_LAZZY_GRID_HH
00037 
00038 #include <spatial_index.h>
00039 #include <ndt_cell.h>
00040 //#include <pcl/kdtree/kdtree_ann.h>
00041 //#include <pcl/kdtree/kdtree_flann.h>
00042 
00043 namespace lslgeneric
00044 {
00045 
00049 template <typename PointT>
00050 class LazyGrid : public SpatialIndex<PointT>
00051 {
00052 public:
00053     LazyGrid(double cellSize);
00054     LazyGrid(LazyGrid *prot);
00055     LazyGrid(double sizeXmeters, double sizeYmeters, double sizeZmeters,
00056              double cellSizeX, double cellSizeY, double cellSizeZ,
00057              double _centerX, double _centerY, double _centerZ,
00058              Cell<PointT> *cellPrototype );
00059     virtual ~LazyGrid();
00060 
00061     virtual Cell<PointT>* getCellForPoint(const PointT &point);
00062     virtual Cell<PointT>* addPoint(const PointT &point);
00063 
00064     //these two don't make much sense...
00066     virtual typename SpatialIndex<PointT>::CellVectorItr begin();
00068     virtual typename SpatialIndex<PointT>::CellVectorItr end();
00069     virtual int size();
00070 
00072     virtual SpatialIndex<PointT>* clone() const;
00073     virtual SpatialIndex<PointT>* copy() const;
00074 
00076     virtual void getNeighbors(const PointT &point, const double &radius, std::vector<Cell<PointT>*> &cells);
00077 
00079     virtual void setCellType(Cell<PointT> *type);
00080 
00081     virtual void setCenter(const double &cx, const double &cy, const double &cz);
00082     virtual void setSize(const double &sx, const double &sy, const double &sz);
00083 
00084     virtual NDTCell<PointT>* getClosestNDTCell(const PointT &pt, bool checkForGaussian=true);
00085     virtual std::vector<NDTCell<PointT>*> getClosestNDTCells(const PointT &pt, int &n_neigh, bool checkForGaussian=true);
00086     virtual std::vector<NDTCell<PointT>*> getClosestCells(const PointT &pt);
00087 
00088     virtual inline void getCellAt(int indX, int indY, int indZ, Cell<PointT>* &cell){
00089         if(indX < sizeX && indY < sizeY && indZ < sizeZ && indX >=0 && indY >=0 && indZ >=0){
00090             cell = dataArray[indX][indY][indZ];
00091         }else{
00092                 cell = NULL;
00093                 
00094         }
00095     }
00096     virtual inline void getCellAt(const PointT& pt, Cell<PointT>* &cell){
00097         int indX,indY,indZ;
00098         this->getIndexForPoint(pt,indX,indY,indZ);
00099         this->getCellAt(indX,indY,indZ,cell);
00100     }
00101     virtual inline void getNDTCellAt(int indX, int indY, int indZ, NDTCell<PointT>* &cell){
00102                         if(indX < sizeX && indY < sizeY && indZ < sizeZ && indX >=0 && indY >=0 && indZ >=0){
00103                                         cell = dynamic_cast<NDTCell<PointT>*> (dataArray[indX][indY][indZ]);
00104                         }else{
00105                                 cell = NULL;
00106                         }
00107     }
00108     virtual inline void getNDTCellAt(const PointT& pt, NDTCell<PointT>* &cell){
00109                         int indX,indY,indZ;
00110                         this->getIndexForPoint(pt,indX,indY,indZ);
00111                         this->getNDTCellAt(indX,indY,indZ,cell);
00112     }
00113     //virtual bool getLinkedAt(int indX, int indY, int indZ);
00114 
00115     void getCellSize(double &cx, double &cy, double &cz);
00116     void getGridSize(int &cx, int &cy, int &cz);
00117     void getGridSizeInMeters(double &cx, double &cy, double &cz);
00118     void getCenter(double &cx, double &cy, double &cz);
00119     virtual inline void getIndexForPoint(const PointT& pt, int &idx, int &idy, int &idz);
00120     Cell<PointT> * getProtoType()
00121     {
00122         return protoType;
00123     }
00124 
00125     //void initKDTree();
00126     virtual void initialize();
00127     virtual void initializeAll() ;
00128 
00129     Cell<PointT> ****getDataArrayPtr()
00130     {
00131         return dataArray;
00132     }
00133 
00135     virtual int loadFromJFF(FILE * jffin);
00136     inline bool traceLine(const Eigen::Vector3d &origin, const PointT &endpoint, const Eigen::Vector3d &diff, const double& maxz, std::vector<NDTCell<PointT>*> &cells);
00137     inline bool traceLineWithEndpoint(const Eigen::Vector3d &origin, const PointT &endpoint, const Eigen::Vector3d &diff, const double& maxz, std::vector<NDTCell<PointT>*> &cells, Eigen::Vector3d &final_point);
00138     bool isInside(const PointT& pt) {
00139                         int indX,indY,indZ;
00140                         this->getIndexForPoint(pt,indX,indY,indZ);
00141                         return(indX < sizeX && indY < sizeY && indZ < sizeZ && indX >=0 && indY >=0 && indZ >=0);
00142     }
00143 protected:
00144     bool initialized;
00145     Cell<PointT> ****dataArray;
00146     //bool ***linkedCells;
00147     Cell<PointT> *protoType;
00148     std::vector<Cell<PointT>*> activeCells;
00149     //pcl::KdTreeFLANN<PointT> meansTree;
00150     bool centerIsSet, sizeIsSet;
00151     //typename pcl::KdTree<PointT>::PointCloudPtr mp;
00152 
00153     double sizeXmeters, sizeYmeters, sizeZmeters;
00154     double cellSizeX, cellSizeY, cellSizeZ;
00155     double centerX, centerY, centerZ;
00156     int sizeX,sizeY,sizeZ;
00157 
00158     virtual bool checkCellforNDT(int indX, int indY, int indZ, bool checkForGaussian=true);
00159     //void getIndexArrayForPoint(const PointT& pt, int *&idx, int *&idy, int *&idz);
00160 };
00161 
00162 
00163 }; //end namespace
00164 #include<impl/lazy_grid.hpp>
00165 
00166 #endif


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