geometry_map.h

Go to the documentation of this file.

#ifndef __GEOMETRY_MAP_H__
#define __GEOMETRY_MAP_H__

// external includes

#include <Eigen/Core>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>

//cob includes
//#include "cob_3d_mapping_geometry_map/vis/geometry_map_visualisation.h"
#include "cob_3d_mapping_common/polygon.h"
#include "cob_3d_mapping_common/cylinder.h"

class GeometryMap
{
public:
  typedef std::vector<cob_3d_mapping::Polygon::Ptr>::iterator polygon_iterator;

  typedef std::vector<cob_3d_mapping::Polygon::Ptr>::iterator cylinder_iterator;

  GeometryMap () :
      new_id_ (0), frame_counter_ (0), file_path_ ("./"), save_to_file_ (false), cos_angle_ (0.97), d_ (0.01), last_tf_err_ (
          Eigen::Affine3f::Identity ())
  {
  }

  ~GeometryMap ()
  {
  }

  void
  addMapEntry (cob_3d_mapping::Polygon::Ptr& p_ptr);

  void
  addMapEntry (cob_3d_mapping::Cylinder::Ptr& c_ptr);

  //void addMapEntry(cob_3d_mapping::ShapeCluster::Ptr& sc_ptr);
  //bool
  //computeTfError(const std::vector<cob_3d_mapping::Polygon::Ptr>& list_polygon, const Eigen::Affine3f& tf_old, Eigen::Affine3f& adjust_tf);

  inline void
  incrFrame ()
  {
    ++frame_counter_;
  }
  ;

  void
  cleanUp ();

  void
  saveMapEntry (std::string path, int ctr, cob_3d_mapping::Polygon& p);

  void
  saveMap (std::string path);

  void
  clearMap ();

  float
  rounding (float x);

  void
  colorizeMap ();

  inline std::vector<cob_3d_mapping::Polygon::Ptr>*
  getMapPolygon ()
  {
    return &(map_polygon_);
  }

  inline std::vector<cob_3d_mapping::Polygon::Ptr>*
  getMapCylinder ()
  {
    return &(map_cylinder_);
  }

  //inline std::vector<cob_3d_mapping::ShapeCluster::Ptr>* getMap_shape_cluster() { return &(map_shape_cluster_); }

  void
  setFilePath (std::string file_path)
  {
    file_path_ = file_path;
  }

  void
  setSaveToFile (bool save_to_file)
  {
    save_to_file_ = save_to_file;
  }

  void
  setMergeThresholds (double cos_angle, double d)
  {
    cos_angle_ = cos_angle;
    d_ = d;
  }

  inline const Eigen::Affine3f&
  getLastError ()
  {
    return last_tf_err_;
  }

protected:
  std::vector<cob_3d_mapping::Polygon::Ptr> map_polygon_; 
  std::vector<cob_3d_mapping::Polygon::Ptr> map_cylinder_; 
  //std::vector<cob_3d_mapping::ShapeCluster::Ptr> map_shape_cluster_; /**< Array containing all shape clusters. */
  unsigned int new_id_; 
  //int counter_output;
  int frame_counter_; 
  std::string file_path_; 
  bool save_to_file_; 
  double cos_angle_; 
  double d_; 
  Eigen::Affine3f last_tf_err_; 
};

#endif //__GEOMETRY_MAP_H__