ObjectLocalizer.h
Go to the documentation of this file.
00001 
00002 
00003 
00004 #ifndef __OBJECTLOCALIZER_H__
00005 #define __OBJECTLOCALIZER_H__
00006 
00007 #include <map>
00008 #include <string>
00009 #include <tf/tf.h>
00010 
00011 #include <ias_drawer_executive/Keywords.h>
00012 
00013 class ObjectLocalizer;
00014 
00015 typedef std::map<std::string, ObjectLocalizer*> localizer_map;
00016 
00017 class ObjectLocalizer
00018 {
00019 public:
00020 
00021     ObjectLocalizer(std::string object_type);
00022 
00023     static bool localize(std::string object_class, tf::Stamped<tf::Pose> *poses, int numHits = 1, Keywords keys = Keywords());
00024 
00025     virtual bool localize_imp(std::string object_class, tf::Stamped<tf::Pose> *poses, int numHits = 1, Keywords keys = Keywords());
00026 
00027 private:
00028 
00029     std::string object_type_;
00030 
00031     static localizer_map localizers;
00032 };
00033 
00034 
00035 #include <pcl/point_types.h>
00036 #include <pcl_ros/point_cloud.h>
00037 
00038 class PotLocalizer : public ObjectLocalizer
00039 {
00040 public :
00041 
00042     PotLocalizer();
00043 
00044     virtual bool localize_imp(std::string object_class, tf::Stamped<tf::Pose> *poses, int numHits = 1, Keywords keys = Keywords());
00045 
00046     void getPointsInBox(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr inBox, btVector3 min, btVector3 max);
00047 
00048     bool getCirclesFromCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr pot_cyl_cloud,
00049                          double radius_goal,
00050                          double radius_tolerance,
00051                          std::vector<tf::Vector3> &center,
00052                          std::vector<double> &radius,
00053                          std::vector<int> &numinliers,
00054                          size_t  iterations = 5);
00055 
00056     bool getHandleRotation(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, tf::Stamped<tf::Pose> &potPose,double pot_handle_radius_min, double pot_handle_radius_max, btVector3 min, btVector3 max);
00057 
00058     // min.z should be the table height, while min->max should span a box where all points necessary for detection lie in
00059     bool getLidPose(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, tf::Stamped<tf::Pose> &lid,double radius, tf::Vector3 min, tf::Vector3 max);
00060 
00061     // min.z should be the table height, while min->max should span a box where all points necessary for detection lie in
00062     bool getPotPoseViaLid(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, tf::Stamped<tf::Pose> &potPose, tf::Vector3 min, tf::Vector3 max);
00063 
00064 };
00065 
00066 
00067 
00068 
00069 #endif


ias_drawer_executive
Author(s): Thomas Ruehr
autogenerated on Mon Oct 6 2014 08:59:24