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 #pragma once
38 
39 #include <sensor_msgs/PointCloud2.h>
41 #include <boost/function.hpp>
42 #include <vector>
43 #include <set>
44 #include <map>
45 
46 #include <boost/thread/mutex.hpp>
47 
49 {
50 typedef unsigned int ShapeHandle;
51 
53 class ShapeMask
54 {
55 public:
57  enum
58  {
59  INSIDE = 0,
60  OUTSIDE = 1,
61  CLIP = 2
62  };
63 
64  typedef boost::function<bool(ShapeHandle, Eigen::Isometry3d&)> TransformCallback;
65 
67  ShapeMask(const TransformCallback& transform_callback = TransformCallback());
68 
70  virtual ~ShapeMask();
71 
72  ShapeHandle addShape(const shapes::ShapeConstPtr& shape, double scale = 1.0, double padding = 0.0);
73  void removeShape(ShapeHandle handle);
74 
75  void setTransformCallback(const TransformCallback& transform_callback);
76 
81  void maskContainment(const sensor_msgs::PointCloud2& data_in, const Eigen::Vector3d& sensor_pos,
82  const double min_sensor_dist, const double max_sensor_dist, std::vector<int>& mask);
83 
86  int getMaskContainment(double x, double y, double z) const;
87 
90  int getMaskContainment(const Eigen::Vector3d& pt) const;
91 
92 protected:
93  struct SeeShape
94  {
95  SeeShape()
96  {
97  body = nullptr;
98  }
99 
102  double volume;
103  };
104 
105  struct SortBodies
106  {
107  bool operator()(const SeeShape& b1, const SeeShape& b2) const
108  {
109  if (b1.volume > b2.volume)
110  return true;
111  if (b1.volume < b2.volume)
112  return false;
113  return b1.handle < b2.handle;
114  }
115  };
116 
118 
120  mutable boost::mutex shapes_lock_;
121  std::set<SeeShape, SortBodies> bodies_;
122  std::vector<bodies::BoundingSphere> bspheres_;
123 
124 private:
126  void freeMemory();
127 
130  std::map<ShapeHandle, std::set<SeeShape, SortBodies>::iterator> used_handles_;
131 };
132 } // namespace point_containment_filter
point_containment_filter::ShapeHandle
unsigned int ShapeHandle
Definition: shape_mask.h:82
point_containment_filter::ShapeMask::shapes_lock_
boost::mutex shapes_lock_
Protects, bodies_ and bspheres_. All public methods acquire this mutex for their whole duration.
Definition: shape_mask.h:152
point_containment_filter::ShapeMask::OUTSIDE
@ OUTSIDE
Definition: shape_mask.h:92
point_containment_filter::ShapeMask::TransformCallback
boost::function< bool(ShapeHandle, Eigen::Isometry3d &)> TransformCallback
Definition: shape_mask.h:96
bodies::Body
point_containment_filter::ShapeMask::~ShapeMask
virtual ~ShapeMask()
Destructor to clean up.
Definition: shape_mask.cpp:49
point_containment_filter::ShapeMask::maskContainment
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:117
point_containment_filter::ShapeMask::CLIP
@ CLIP
Definition: shape_mask.h:93
point_containment_filter::ShapeMask::bodies_
std::set< SeeShape, SortBodies > bodies_
Definition: shape_mask.h:153
point_containment_filter::ShapeMask::SeeShape
Definition: shape_mask.h:125
point_containment_filter::ShapeMask::freeMemory
void freeMemory()
Free memory.
Definition: shape_mask.cpp:54
point_containment_filter::ShapeMask::SeeShape::body
bodies::Body * body
Definition: shape_mask.h:132
point_containment_filter::ShapeMask::INSIDE
@ INSIDE
Definition: shape_mask.h:91
point_containment_filter::ShapeMask::next_handle_
ShapeHandle next_handle_
Definition: shape_mask.h:160
point_containment_filter::ShapeMask::SeeShape::handle
ShapeHandle handle
Definition: shape_mask.h:133
point_containment_filter::ShapeMask::removeShape
void removeShape(ShapeHandle handle)
Definition: shape_mask.cpp:102
point_containment_filter::ShapeMask::bspheres_
std::vector< bodies::BoundingSphere > bspheres_
Definition: shape_mask.h:154
point_containment_filter::ShapeMask::SortBodies::operator()
bool operator()(const SeeShape &b1, const SeeShape &b2) const
Definition: shape_mask.h:139
point_containment_filter::ShapeMask::ShapeMask
ShapeMask(const TransformCallback &transform_callback=TransformCallback())
Construct the filter.
Definition: shape_mask.cpp:44
point_containment_filter::ShapeMask
Computing a mask for a pointcloud that states which points are inside the robot.
Definition: shape_mask.h:85
point_containment_filter::ShapeMask::min_handle_
ShapeHandle min_handle_
Definition: shape_mask.h:161
point_containment_filter::ShapeMask::setTransformCallback
void setTransformCallback(const TransformCallback &transform_callback)
Definition: shape_mask.cpp:61
point_containment_filter
Definition: shape_mask.h:48
bodies.h
point_containment_filter::ShapeMask::used_handles_
std::map< ShapeHandle, std::set< SeeShape, SortBodies >::iterator > used_handles_
Definition: shape_mask.h:162
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
point_containment_filter::ShapeMask::SeeShape::SeeShape
SeeShape()
Definition: shape_mask.h:127
point_containment_filter::ShapeMask::SeeShape::volume
double volume
Definition: shape_mask.h:134
point_containment_filter::ShapeMask::transform_callback_
TransformCallback transform_callback_
Definition: shape_mask.h:149
point_containment_filter::ShapeMask::addShape
ShapeHandle addShape(const shapes::ShapeConstPtr &shape, double scale=1.0, double padding=0.0)
Definition: shape_mask.cpp:67
point_containment_filter::ShapeMask::getMaskContainment
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:190


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Mon May 27 2024 02:27:57