ndt_map_hmt.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 NDT_MAP_HMT_HH
00036 #define NDT_MAP_HMT_HH
00037 
00038 #include <ndt_map/ndt_map.h>
00039 #include <ndt_map/spatial_index.h>
00040 #include <ndt_map/ndt_cell.h>
00041 //#include <ndt_map/depth_camera.h>
00042 #include <ndt_map/lazy_grid.h>
00043 
00044 #include <cstdlib>
00045 #include <cv.h>
00046 #include <sys/stat.h>
00047 #include <sys/dir.h>
00048 
00049 #include <pcl/point_cloud.h>
00050 #include <pcl/point_types.h>
00051 
00052 namespace lslgeneric
00053 {
00054 
00064 class NDTMapHMT : public NDTMap
00065 {
00066 public:
00067 
00074     NDTMapHMT(double resolution_, float cenx, float ceny, float cenz, float sizex, float sizey, float sizez, double max_range, std::string directory, bool _saveOnDelete=false)
00075     {
00076         max_range_ = max_range;
00077         grids_init=false;
00078         my_directory = directory;
00079         resolution = resolution_;
00080 
00081         //check if we can stat my_directory and try to create it if we can't
00082         DIR *mdir = opendir(my_directory.c_str());
00083         if(mdir == NULL) {
00084             std::cerr<<"Error accessing map directory "<<my_directory<<" will attempt to recover:\n";
00085             int res = mkdir(my_directory.c_str(),S_IRWXU);
00086             if (res<0) {
00087                 std::cerr<<"CRITICAL! FAILED to create directory\n";
00088                 return;
00089             }
00090         } else {
00091             closedir(mdir);
00092         }
00093 
00094         LazyGrid *lz = new LazyGrid(resolution);
00095         this->index_ = lz;
00096 
00097         //this is used to prevent memory de-allocation of the *si
00098         //si was allocated outside the NDT class and should be deallocated outside
00099         this->isFirstLoad_=true;
00100 
00101         NDTCell *ptCell = new NDTCell();
00102         this->index_->setCellType(ptCell);
00103         delete ptCell;
00104         this->index_->setCenter(cenx,ceny,cenz);
00105         this->index_->setSize(sizex,sizey,sizez);
00106         this->map_sizex = sizex;
00107         this->map_sizey = sizey;
00108         this->map_sizez = sizez;
00109         this->is3D=true;
00110         lz->initializeAll();
00111         this->guess_size_ = false;
00112         this->saveOnDelete = _saveOnDelete;
00113 
00114         //create the grid of LazyGrids and set their centers
00115         initializeGrids();
00116     }
00117     NDTMapHMT(const NDTMapHMT& other)
00118     {
00119         if(other.index_ != NULL)
00120         {
00121             this->index_ = other.index_->copy();
00122             isFirstLoad_ = false;
00123             this->max_range_ = other.max_range_;
00124             this->grids_init = false;
00125             this->saveOnDelete = other.saveOnDelete;
00126             //TODO copy all LazyGrids
00127         }
00128     }
00132     virtual ~NDTMapHMT()
00133     {
00134         if(saveOnDelete) {
00135             this->writeTo();
00136         }
00137         for(int i=0; i<3; ++i) {
00138             for(int j=0; j<3; ++j) {
00139                 if(grid_[i][j]!=NULL) {
00140                     delete grid_[i][j];
00141                 }
00142             }
00143         }
00144     }
00154     virtual void loadPointCloud(const pcl::PointCloud<pcl::PointXYZ> &pc, double range_limit = -1);
00166     virtual void addPointCloud(const Eigen::Vector3d &origin, const pcl::PointCloud<pcl::PointXYZ> &pc, double classifierTh=0.06, 
00167                                 double maxz = 100.0, double sensor_noise = 0.25, double occupancy_limit = 255);
00168 
00184     virtual void addPointCloudMeanUpdate(const Eigen::Vector3d &origin, 
00185             const pcl::PointCloud<pcl::PointXYZ> &pc, 
00186             const Eigen::Vector3d &localmapsize,
00187             unsigned int maxnumpoints = 1e9, float occupancy_limit=255 ,double maxz = 100.0, double sensor_noise = 0.25);
00188 
00189 
00198     virtual void addDistributionToCell(const Eigen::Matrix3d &ucov,const Eigen::Vector3d &umean, unsigned int numpointsindistribution, 
00199                                                                                                                         float r=0, float g=0,float b=0, unsigned int maxnumpoints=1e9, float max_occupancy=1024);
00200 
00206     virtual void computeNDTCells(int cellupdatemode = CELL_UPDATE_MODE_SAMPLE_VARIANCE, unsigned int maxnumpoints = 1e9, float occupancy_limit=255, Eigen::Vector3d origin = Eigen::Vector3d(0,0,0), double sensor_noise=0.1);
00207 
00211     int writeTo();
00212     int loadFrom();
00213     //cehck if there is a grid at this center and try to load it
00214     bool tryLoad(const double &cx, const double &cy, const double &cz, LazyGrid *&grid);
00215 
00217     //computes the likelihood of a single observation
00218     virtual double getLikelihoodForPoint(pcl::PointXYZ pt);
00219 
00221     virtual bool getCellAtPoint(const pcl::PointXYZ &refPoint, NDTCell *&cell);
00222 
00223     virtual bool getCentroid(double &cx, double &cy, double &cz)
00224     {
00225         LazyGrid *lz = grid_[1][1];
00226         if(lz == NULL) return false;
00227         lz->getCenter(cx, cy, cz);
00228         return true;
00229     }
00234     virtual bool getCellForPoint(const pcl::PointXYZ &refPoint, NDTCell *&cell, bool checkForGaussian=true) const;
00239     virtual std::vector<NDTCell*> getCellsForPoint(const pcl::PointXYZ pt, int n_neighbours, bool checkForGaussian=true) const;
00243     virtual std::vector<NDTCell*> getInitializedCellsForPoint(const pcl::PointXYZ pt) const;
00244 
00248     virtual std::vector<NDTCell*> pseudoTransformNDT(Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T);
00249 
00258     virtual std::vector<lslgeneric::NDTCell*> getAllCells() const;
00263     virtual std::vector<lslgeneric::NDTCell*> getAllInitializedCells();
00264 
00265     int numberOfActiveCells();
00266     void setInsertPosition(const Eigen::Vector3d &newPos);
00267     bool tryLoadPosition(const Eigen::Vector3d &newPos);
00268 protected:
00269     bool is3D;
00270     bool saveOnDelete;
00271     SpatialIndex *index_;
00272     bool isFirstLoad_;
00273     float map_sizex;
00274     float map_sizey;
00275     float map_sizez;
00276     float centerx,centery,centerz;
00277     bool grids_init;
00278     Eigen::Vector3d last_insert;
00279     std::string my_directory;
00280     double resolution;
00281     std::set<NDTCell*> update_set;
00282 
00283     double max_range_;
00284     lslgeneric::LazyGrid* grid_[3][3];
00285     //helper functions
00286     void initializeGrids();
00287     
00288 
00289 public:
00290     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00291     pcl::PointCloud<pcl::PointXYZ> conflictPoints; 
00292                 
00293 
00294 };
00295 
00296 } // end namespace
00297 
00298 
00299 #endif


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