shape_mask.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #ifndef MOVEIT_POINT_CONTAINMENT_FILTER_SELF_MASK_
38 #define MOVEIT_POINT_CONTAINMENT_FILTER_SELF_MASK_
39 
40 #include <sensor_msgs/PointCloud2.h>
42 #include <boost/function.hpp>
43 #include <string>
44 #include <vector>
45 #include <set>
46 #include <map>
47 
48 #include <boost/thread/mutex.hpp>
49 
51 {
52 typedef unsigned int ShapeHandle;
53 
55 class ShapeMask
56 {
57 public:
59  enum
60  {
61  INSIDE = 0,
62  OUTSIDE = 1,
63  CLIP = 2
64  };
65 
66  typedef boost::function<bool(ShapeHandle, Eigen::Affine3d&)> TransformCallback;
67 
69  ShapeMask(const TransformCallback& transform_callback = TransformCallback());
70 
72  ~ShapeMask();
73 
74  ShapeHandle addShape(const shapes::ShapeConstPtr& shape, double scale = 1.0, double padding = 0.0);
75  void removeShape(ShapeHandle handle);
76 
77  void setTransformCallback(const TransformCallback& transform_callback);
78 
83  void maskContainment(const sensor_msgs::PointCloud2& data_in, const Eigen::Vector3d& sensor_pos,
84  const double min_sensor_dist, const double max_sensor_dist, std::vector<int>& mask);
85 
88  int getMaskContainment(double x, double y, double z) const;
89 
92  int getMaskContainment(const Eigen::Vector3d& pt) const;
93 
94 private:
95  struct SeeShape
96  {
98  {
99  body = NULL;
100  }
101 
103  ShapeHandle handle;
104  double volume;
105  };
106 
107  struct SortBodies
108  {
109  bool operator()(const SeeShape& b1, const SeeShape& b2)
110  {
111  if (b1.volume > b2.volume)
112  return true;
113  if (b1.volume < b2.volume)
114  return false;
115  return b1.handle < b2.handle;
116  }
117  };
118 
120  void freeMemory();
121 
122  TransformCallback transform_callback_;
123  ShapeHandle next_handle_;
124  ShapeHandle min_handle_;
125 
126  mutable boost::mutex shapes_lock_;
127  std::set<SeeShape, SortBodies> bodies_;
128  std::map<ShapeHandle, std::set<SeeShape, SortBodies>::iterator> used_handles_;
129  std::vector<bodies::BoundingSphere> bspheres_;
130 };
131 }
132 
133 #endif
~ShapeMask()
Destructor to clean up.
Definition: shape_mask.cpp:47
ShapeHandle addShape(const shapes::ShapeConstPtr &shape, double scale=1.0, double padding=0.0)
Definition: shape_mask.cpp:65
void setTransformCallback(const TransformCallback &transform_callback)
Definition: shape_mask.cpp:59
int getMaskContainment(double x, double y, double z) const
Get the containment mask (INSIDE or OUTSIDE) value for an individual point. It is assumed the point i...
Definition: shape_mask.cpp:187
unsigned int ShapeHandle
Definition: shape_mask.h:52
bool operator()(const SeeShape &b1, const SeeShape &b2)
Definition: shape_mask.h:109
boost::function< bool(ShapeHandle, Eigen::Affine3d &)> TransformCallback
Definition: shape_mask.h:66
std::vector< bodies::BoundingSphere > bspheres_
Definition: shape_mask.h:129
Computing a mask for a pointcloud that states which points are inside the robot.
Definition: shape_mask.h:55
std::map< ShapeHandle, std::set< SeeShape, SortBodies >::iterator > used_handles_
Definition: shape_mask.h:128
void maskContainment(const sensor_msgs::PointCloud2 &data_in, const Eigen::Vector3d &sensor_pos, const double min_sensor_dist, const double max_sensor_dist, std::vector< int > &mask)
Compute the containment mask (INSIDE or OUTSIDE) for a given pointcloud. If a mask element is INSIDE...
Definition: shape_mask.cpp:113
std::set< SeeShape, SortBodies > bodies_
Definition: shape_mask.h:127
void removeShape(ShapeHandle handle)
Definition: shape_mask.cpp:98
TransformCallback transform_callback_
Definition: shape_mask.h:122
ShapeMask(const TransformCallback &transform_callback=TransformCallback())
Construct the filter.
Definition: shape_mask.cpp:42
std::shared_ptr< const Shape > ShapeConstPtr


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Sun Oct 18 2020 13:17:23