00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef NDT_MAP_HMT_HH
00036 #define NDT_MAP_HMT_HH
00037
00038 #include <ndt_map.h>
00039 #include <spatial_index.h>
00040 #include <ndt_cell.h>
00041 #include <depth_camera.h>
00042
00043 #include <cstdlib>
00044 #include <lazy_grid.h>
00045 #include <cv.h>
00046 #include <pcl/point_cloud.h>
00047 #include <pcl/point_types.h>
00048 #include <sys/stat.h>
00049 #include <sys/dir.h>
00050
00051 namespace lslgeneric
00052 {
00053
00063 template <typename PointT>
00064 class NDTMapHMT : public NDTMap<PointT>
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
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<PointT> *lz = new LazyGrid<PointT>(resolution);
00095 this->index_ = lz;
00096
00097
00098
00099 this->isFirstLoad_=true;
00100
00101 NDTCell<PointT> *ptCell = new NDTCell<PointT>();
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
00115 initializeGrids();
00116 }
00117 NDTMapHMT(const NDTMapHMT<PointT>& 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
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<PointT> &pc, double range_limit = -1);
00166 virtual void addPointCloud(const Eigen::Vector3d &origin, const pcl::PointCloud<PointT> &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<PointT> &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
00214 bool tryLoad(const double &cx, const double &cy, const double &cz, LazyGrid<PointT> *&grid);
00215
00217
00218 virtual double getLikelihoodForPoint(PointT pt);
00219
00221 virtual bool getCellAtPoint(const PointT &refPoint, NDTCell<PointT> *&cell);
00222
00223 virtual bool getCentroid(double &cx, double &cy, double &cz)
00224 {
00225 LazyGrid<PointT> *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 PointT &refPoint, NDTCell<PointT> *&cell, bool checkForGaussian=true) const;
00239 virtual std::vector<NDTCell<PointT>*> getCellsForPoint(const PointT pt, int n_neighbours, bool checkForGaussian=true) const;
00243 virtual std::vector<NDTCell<PointT>*> getInitializedCellsForPoint(const PointT pt) const;
00244
00248 virtual std::vector<NDTCell<PointT>*> pseudoTransformNDT(Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T);
00249
00258 virtual std::vector<lslgeneric::NDTCell<PointT>*> getAllCells() const;
00263 virtual std::vector<lslgeneric::NDTCell<PointT>*> 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<PointT> *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<PointT>*> update_set;
00282
00283 double max_range_;
00284 lslgeneric::LazyGrid<PointT>* grid_[3][3];
00285
00286 void initializeGrids();
00287
00288
00289 public:
00290 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00291 pcl::PointCloud<PointT> conflictPoints;
00292
00293
00294 };
00295
00296 }
00297
00298 #include <impl/ndt_map_hmt.hpp>
00299
00300 #endif