ndt_map_hmt.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, AASS Research Center, Orebro University.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of AASS Research Center nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 */
#ifndef NDT_MAP_HMT_HH
#define NDT_MAP_HMT_HH

#include <ndt_map/ndt_map.h>
#include <ndt_map/spatial_index.h>
#include <ndt_map/ndt_cell.h>
#include <ndt_map/depth_camera.h>
#include <ndt_map/lazy_grid.h>

#include <cstdlib>
#include <cv.h>
#include <sys/stat.h>
#include <sys/dir.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

namespace lslgeneric
{

template <typename PointT>
class NDTMapHMT : public NDTMap<PointT>
{
public:

    NDTMapHMT(double resolution_, float cenx, float ceny, float cenz, float sizex, float sizey, float sizez, double max_range, std::string directory, bool _saveOnDelete=false)
    {
        max_range_ = max_range;
        grids_init=false;
        my_directory = directory;
        resolution = resolution_;

        //check if we can stat my_directory and try to create it if we can't
        DIR *mdir = opendir(my_directory.c_str());
        if(mdir == NULL) {
            std::cerr<<"Error accessing map directory "<<my_directory<<" will attempt to recover:\n";
            int res = mkdir(my_directory.c_str(),S_IRWXU);
            if (res<0) {
                std::cerr<<"CRITICAL! FAILED to create directory\n";
                return;
            }
        } else {
            closedir(mdir);
        }

        LazyGrid<PointT> *lz = new LazyGrid<PointT>(resolution);
        this->index_ = lz;

        //this is used to prevent memory de-allocation of the *si
        //si was allocated outside the NDT class and should be deallocated outside
        this->isFirstLoad_=true;

        NDTCell<PointT> *ptCell = new NDTCell<PointT>();
        this->index_->setCellType(ptCell);
        delete ptCell;
        this->index_->setCenter(cenx,ceny,cenz);
        this->index_->setSize(sizex,sizey,sizez);
        this->map_sizex = sizex;
        this->map_sizey = sizey;
        this->map_sizez = sizez;
        this->is3D=true;
        lz->initializeAll();
        this->guess_size_ = false;
        this->saveOnDelete = _saveOnDelete;

        //create the grid of LazyGrids and set their centers
        initializeGrids();
    }
    NDTMapHMT(const NDTMapHMT<PointT>& other)
    {
        if(other.index_ != NULL)
        {
            this->index_ = other.index_->copy();
            isFirstLoad_ = false;
            this->max_range_ = other.max_range_;
            this->grids_init = false;
            this->saveOnDelete = other.saveOnDelete;
            //TODO copy all LazyGrids
        }
    }
    virtual ~NDTMapHMT()
    {
        if(saveOnDelete) {
            this->writeTo();
        }
        for(int i=0; i<3; ++i) {
            for(int j=0; j<3; ++j) {
                if(grid_[i][j]!=NULL) {
                    delete grid_[i][j];
                }
            }
        }
    }
    virtual void loadPointCloud(const pcl::PointCloud<PointT> &pc, double range_limit = -1);
    virtual void addPointCloud(const Eigen::Vector3d &origin, const pcl::PointCloud<PointT> &pc, double classifierTh=0.06, 
                                double maxz = 100.0, double sensor_noise = 0.25, double occupancy_limit = 255);

    virtual void addPointCloudMeanUpdate(const Eigen::Vector3d &origin, 
            const pcl::PointCloud<PointT> &pc, 
            const Eigen::Vector3d &localmapsize,
            unsigned int maxnumpoints = 1e9, float occupancy_limit=255 ,double maxz = 100.0, double sensor_noise = 0.25);


    virtual void addDistributionToCell(const Eigen::Matrix3d &ucov,const Eigen::Vector3d &umean, unsigned int numpointsindistribution, 
                                                                                                                        float r=0, float g=0,float b=0, unsigned int maxnumpoints=1e9, float max_occupancy=1024);

    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);

    int writeTo();
    int loadFrom();
    //cehck if there is a grid at this center and try to load it
    bool tryLoad(const double &cx, const double &cy, const double &cz, LazyGrid<PointT> *&grid);

    //computes the likelihood of a single observation
    virtual double getLikelihoodForPoint(PointT pt);

    virtual bool getCellAtPoint(const PointT &refPoint, NDTCell<PointT> *&cell);

    virtual bool getCentroid(double &cx, double &cy, double &cz)
    {
        LazyGrid<PointT> *lz = grid_[1][1];
        if(lz == NULL) return false;
        lz->getCenter(cx, cy, cz);
        return true;
    }
    virtual bool getCellForPoint(const PointT &refPoint, NDTCell<PointT> *&cell, bool checkForGaussian=true) const;
    virtual std::vector<NDTCell<PointT>*> getCellsForPoint(const PointT pt, int n_neighbours, bool checkForGaussian=true) const;
    virtual std::vector<NDTCell<PointT>*> getInitializedCellsForPoint(const PointT pt) const;

    virtual std::vector<NDTCell<PointT>*> pseudoTransformNDT(Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T);

    virtual std::vector<lslgeneric::NDTCell<PointT>*> getAllCells() const;
    virtual std::vector<lslgeneric::NDTCell<PointT>*> getAllInitializedCells();

    int numberOfActiveCells();
    void setInsertPosition(const Eigen::Vector3d &newPos);
    bool tryLoadPosition(const Eigen::Vector3d &newPos);
protected:
    bool is3D;
    bool saveOnDelete;
    SpatialIndex<PointT> *index_;
    bool isFirstLoad_;
    float map_sizex;
    float map_sizey;
    float map_sizez;
    float centerx,centery,centerz;
    bool grids_init;
    Eigen::Vector3d last_insert;
    std::string my_directory;
    double resolution;
    std::set<NDTCell<PointT>*> update_set;

    double max_range_;
    lslgeneric::LazyGrid<PointT>* grid_[3][3];
    //helper functions
    void initializeGrids();
    

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    pcl::PointCloud<PointT> conflictPoints; 
                

};

} // end namespace

#include <ndt_map/impl/ndt_map_hmt.hpp>

#endif