SubmodMap.h

Go to the documentation of this file.

/*
 * SubmodMap.h
 *
 *  Created on: 14.11.2011
 *      Author: hess
 */

#ifndef SubmodMap_H_
#define SubmodMap_H_
#include <pcl/common/common_headers.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <surfacelet/SurfacePatch.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
namespace surfacelet {

class SubmodMap {

public:

    SubmodMap();

    void insertScan(pcl::PointCloud<pcl::PointXYZ>& cloud, pcl::PointCloud<pcl::PointXYZ>& not_used_cloud);
    void insertScan(pcl::PointCloud<pcl::PointXYZ>& cloud, pcl::PointCloud<pcl::PointXYZ>& not_used_cloud,
        pcl::PointXYZ view_point);
    void constructNormalMarker(const std::string frame_id, visualization_msgs::MarkerArray& array) const;
    void constructMarker(const std::string frame_id, const std::vector<bool>& reachable,
        visualization_msgs::MarkerArray& array);
    void constructMarker(const std::string frame_id, const std::vector<bool>& reachable,
        const std::vector<bool>& collision, visualization_msgs::MarkerArray& array);
    void constructMarker(const std::string frame_id, visualization_msgs::MarkerArray& array);
//    std::vector<SurfacePatch> getMap();
//    void  setMap(std::vector<SurfacePatch> patches);
    std::vector<SurfacePatch> map_;

private:
    void computePatchesFromCloud(pcl::PointCloud<pcl::PointXYZ>& cloud, const pcl::PointXYZ view_point,
        std::vector<SurfacePatch>& patches, pcl::PointCloud<pcl::PointXYZ>& not_used_cloud);
    bool pointRepresented(pcl::PointNormal p);
    bool searchPatchForPoint(pcl::PointXYZ p);
    bool generatePatch(const pcl::PointCloud<pcl::PointXYZ>& cloud, const pcl::PointXYZ view_point,
        const pcl::PointXYZ center, const std::vector<int>& mask, SurfacePatch& patch,
        std::vector<int>& removed_points);

    pcl::KdTreeFLANN<SurfacePatch> tree_;

    bool first_insert;
    float radius_;
    float ransac_distance_;
    int ransac_iterations_;

};
}

#endif /* SubmodMap_H_ */