Go to the documentation of this file.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
00036
00037 #ifndef MOVEIT_POINT_CONTAINMENT_FILTER_SELF_MASK_
00038 #define MOVEIT_POINT_CONTAINMENT_FILTER_SELF_MASK_
00039
00040 #include <sensor_msgs/PointCloud.h>
00041 #include <geometric_shapes/bodies.h>
00042 #include <boost/function.hpp>
00043 #include <string>
00044 #include <vector>
00045 #include <set>
00046 #include <map>
00047
00048 #include <boost/thread/mutex.hpp>
00049 #include <pcl/point_cloud.h>
00050 #include <pcl/point_types.h>
00051
00052 namespace point_containment_filter
00053 {
00054
00055 typedef unsigned int ShapeHandle;
00056
00058 class ShapeMask
00059 {
00060
00061 public:
00062
00064 enum
00065 {
00066 INSIDE = 0,
00067 OUTSIDE = 1,
00068 CLIP = 2
00069 };
00070
00071 typedef boost::function<bool (ShapeHandle, Eigen::Affine3d&)> TransformCallback;
00072
00074 ShapeMask(const TransformCallback& transform_callback = TransformCallback());
00075
00077 ~ShapeMask();
00078
00079 ShapeHandle addShape(const shapes::ShapeConstPtr &shape, double scale = 1.0, double padding = 0.0);
00080 void removeShape(ShapeHandle handle);
00081
00082 void setTransformCallback (const TransformCallback& transform_callback);
00083
00087 void maskContainment(const pcl::PointCloud<pcl::PointXYZ>& data_in, const Eigen::Vector3d &sensor_pos,
00088 const double min_sensor_dist, const double max_sensor_dist, std::vector<int> &mask);
00089
00092 int getMaskContainment(double x, double y, double z) const;
00093
00094
00097 int getMaskContainment(const Eigen::Vector3d &pt) const;
00098
00099 private:
00100
00101 struct SeeShape
00102 {
00103 SeeShape()
00104 {
00105 body = NULL;
00106 }
00107
00108 bodies::Body *body;
00109 ShapeHandle handle;
00110 double volume;
00111 };
00112
00113 struct SortBodies
00114 {
00115 bool operator()(const SeeShape &b1, const SeeShape &b2)
00116 {
00117 if (b1.volume > b2.volume)
00118 return true;
00119 if (b1.volume < b2.volume)
00120 return false;
00121 return b1.handle < b2.handle;
00122 }
00123 };
00124
00126 void freeMemory();
00127
00128 TransformCallback transform_callback_;
00129 ShapeHandle next_handle_;
00130 ShapeHandle min_handle_;
00131
00132 mutable boost::mutex shapes_lock_;
00133 std::set<SeeShape, SortBodies> bodies_;
00134 std::map<ShapeHandle, std::set<SeeShape, SortBodies>::iterator> used_handles_;
00135 std::vector<bodies::BoundingSphere> bspheres_;
00136 };
00137
00138 }
00139
00140 #endif