shape_mask.cpp
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 
39 #include <ros/console.h>
41 
43  : transform_callback_(transform_callback), next_handle_(1), min_handle_(1)
44 {
45 }
46 
48 {
49  freeMemory();
50 }
51 
53 {
54  for (std::set<SeeShape>::const_iterator it = bodies_.begin(); it != bodies_.end(); ++it)
55  delete it->body;
56  bodies_.clear();
57 }
58 
60 {
61  boost::mutex::scoped_lock _(shapes_lock_);
62  transform_callback_ = transform_callback;
63 }
64 
66  double scale, double padding)
67 {
68  boost::mutex::scoped_lock _(shapes_lock_);
69  SeeShape ss;
70  ss.body = bodies::createBodyFromShape(shape.get());
71  if (ss.body)
72  {
73  ss.body->setScale(scale);
74  ss.body->setPadding(padding);
75  ss.volume = ss.body->computeVolume();
76  ss.handle = next_handle_;
77  std::pair<std::set<SeeShape, SortBodies>::iterator, bool> insert_op = bodies_.insert(ss);
78  if (!insert_op.second)
79  ROS_ERROR("Internal error in management of bodies in ShapeMask. This is a serious error.");
80  used_handles_[next_handle_] = insert_op.first;
81  }
82  else
83  return 0;
84 
86  const std::size_t sz = min_handle_ + bodies_.size() + 1;
87  for (std::size_t i = min_handle_; i < sz; ++i)
88  if (used_handles_.find(i) == used_handles_.end())
89  {
90  next_handle_ = i;
91  break;
92  }
94 
95  return ret;
96 }
97 
99 {
100  boost::mutex::scoped_lock _(shapes_lock_);
101  std::map<ShapeHandle, std::set<SeeShape, SortBodies>::iterator>::iterator it = used_handles_.find(handle);
102  if (it != used_handles_.end())
103  {
104  delete it->second->body;
105  bodies_.erase(it->second);
106  used_handles_.erase(it);
107  min_handle_ = handle;
108  }
109  else
110  ROS_ERROR("Unable to remove shape handle %u", handle);
111 }
112 
113 void point_containment_filter::ShapeMask::maskContainment(const sensor_msgs::PointCloud2& data_in,
114  const Eigen::Vector3d& sensor_origin,
115  const double min_sensor_dist, const double max_sensor_dist,
116  std::vector<int>& mask)
117 {
118  boost::mutex::scoped_lock _(shapes_lock_);
119  const unsigned int np = data_in.data.size() / data_in.point_step;
120  mask.resize(np);
121 
122  if (bodies_.empty())
123  std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
124  else
125  {
126  Eigen::Affine3d tmp;
127  bspheres_.resize(bodies_.size());
128  std::size_t j = 0;
129  for (std::set<SeeShape>::const_iterator it = bodies_.begin(); it != bodies_.end(); ++it)
130  {
131  if (!transform_callback_(it->handle, tmp))
132  {
133  if (!it->body)
134  ROS_ERROR_STREAM_NAMED("shape_mask", "Missing transform for shape with handle " << it->handle
135  << " without a body");
136  else
137  ROS_ERROR_STREAM_NAMED("shape_mask", "Missing transform for shape " << it->body->getType() << " with handle "
138  << it->handle);
139  }
140  else
141  {
142  it->body->setPose(tmp);
143  it->body->computeBoundingSphere(bspheres_[j++]);
144  }
145  }
146 
147  // compute a sphere that bounds the entire robot
150  const double radiusSquared = bound.radius * bound.radius;
151 
152  // we now decide which points we keep
156 
157  // Cloud iterators are not incremented in the for loop, because of the pragma
158  // Comment out below parallelization as it can result in very high CPU consumption
159  //#pragma omp parallel for schedule(dynamic)
160  for (int i = 0; i < (int)np; ++i)
161  {
162  Eigen::Vector3d pt = Eigen::Vector3d(*(iter_x + i), *(iter_y + i), *(iter_z + i));
163  double d = pt.norm();
164  int out = OUTSIDE;
165  if (d < min_sensor_dist || d > max_sensor_dist)
166  out = CLIP;
167  else if ((bound.center - pt).squaredNorm() < radiusSquared)
168  for (std::set<SeeShape>::const_iterator it = bodies_.begin(); it != bodies_.end() && out == OUTSIDE; ++it)
169  if (it->body->containsPoint(pt))
170  out = INSIDE;
171  mask[i] = out;
172  }
173  }
174 }
175 
177 {
178  boost::mutex::scoped_lock _(shapes_lock_);
179 
180  int out = OUTSIDE;
181  for (std::set<SeeShape>::const_iterator it = bodies_.begin(); it != bodies_.end() && out == OUTSIDE; ++it)
182  if (it->body->containsPoint(pt))
183  out = INSIDE;
184  return out;
185 }
186 
187 int point_containment_filter::ShapeMask::getMaskContainment(double x, double y, double z) const
188 {
189  return getMaskContainment(Eigen::Vector3d(x, y, z));
190 }
d
~ShapeMask()
Destructor to clean up.
Definition: shape_mask.cpp:47
#define ROS_ERROR_STREAM_NAMED(name, args)
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
void setScale(double scale)
Eigen::Vector3d center
boost::function< bool(ShapeHandle, Eigen::Affine3d &)> TransformCallback
Definition: shape_mask.h:66
void mergeBoundingSpheres(const std::vector< BoundingSphere > &spheres, BoundingSphere &mergedSphere)
std::vector< bodies::BoundingSphere > bspheres_
Definition: shape_mask.h:129
Body * createBodyFromShape(const shapes::Shape *shape)
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
virtual double computeVolume() const =0
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
#define ROS_ERROR(...)
void setPadding(double padd)
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