collision_octomap_filter.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, 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: Adam Leeper */
36 
40 #include <octomap/math/Vector3.h>
41 #include <octomap/math/Utils.h>
42 #include <octomap/octomap.h>
44 #include <memory>
45 
46 // static const double ISO_VALUE = 0.5; // TODO magic number! (though, probably a good one).
47 // static const double R_MULTIPLE = 1.5; // TODO magic number! (though, probably a good one).
48 
49 // forward declarations
50 bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, const double& spacing, const double& iso_value,
51  const double& r_multiple, const octomath::Vector3& contact_point,
52  octomath::Vector3& normal, double& depth, bool estimate_depth);
53 
54 bool findSurface(const octomap::point3d_list& cloud, const double& spacing, const double& iso_value,
55  const double& r_multiple, const octomath::Vector3& seed, octomath::Vector3& surface_point,
56  octomath::Vector3& normal);
57 
58 bool sampleCloud(const octomap::point3d_list& cloud, const double& spacing, const double& r_multiple,
59  const octomath::Vector3& position, double& intensity, octomath::Vector3& gradient);
60 
61 int collision_detection::refineContactNormals(const World::ObjectConstPtr& object, CollisionResult& res,
62  double cell_bbx_search_distance, double allowed_angle_divergence,
63  bool estimate_depth, double iso_value, double metaball_radius_multiple)
64 {
65  if (!object)
66  {
67  ROS_ERROR_NAMED("collision_detection", "No valid Object passed in, cannot refine Normals!");
68  return 0;
69  }
70  if (res.contact_count < 1)
71  {
72  ROS_WARN_NAMED("collision_detection", "There do not appear to be any contacts, so there is nothing to refine!");
73  return 0;
74  }
75 
76  int modified = 0;
77 
78  // iterate through contacts
79  for (auto& contact : res.contacts)
80  {
81  std::string contact1 = contact.first.first;
82  std::string contact2 = contact.first.second;
83  std::string octomap_name = "";
84  std::vector<collision_detection::Contact>& contact_vector = contact.second;
85 
86  if (contact1.find("octomap") != std::string::npos)
87  octomap_name = contact1;
88  else if (contact2.find("octomap") != std::string::npos)
89  octomap_name = contact2;
90  else
91  {
92  continue;
93  }
94 
95  double cell_size = 0;
96  if (!object->shapes_.empty())
97  {
98  const shapes::ShapeConstPtr& shape = object->shapes_[0];
99  std::shared_ptr<const shapes::OcTree> shape_octree = std::dynamic_pointer_cast<const shapes::OcTree>(shape);
100  if (shape_octree)
101  {
102  std::shared_ptr<const octomap::OcTree> octree = shape_octree->octree;
103  cell_size = octree->getResolution();
104  for (auto& contact_info : contact_vector)
105  {
106  const Eigen::Vector3d& point = contact_info.pos;
107  const Eigen::Vector3d& normal = contact_info.normal;
108 
109  octomath::Vector3 contact_point(point[0], point[1], point[2]);
110  octomath::Vector3 contact_normal(normal[0], normal[1], normal[2]);
111  octomath::Vector3 diagonal = octomath::Vector3(1, 1, 1);
112  octomath::Vector3 bbx_min = contact_point - diagonal * cell_size * cell_bbx_search_distance;
113  octomath::Vector3 bbx_max = contact_point + diagonal * cell_size * cell_bbx_search_distance;
114  octomap::point3d_list node_centers;
116  octree->begin_leafs_bbx(bbx_min, bbx_max);
118  octree->end_leafs_bbx();
119  int count = 0;
120  for (; it != leafs_end; ++it)
121  {
122  octomap::point3d pt = it.getCoordinate();
123  // double prob = it->getOccupancy();
124  if (octree->isNodeOccupied(*it)) // magic number!
125  {
126  count++;
127  node_centers.push_back(pt);
128  // ROS_INFO_NAMED("collision_detection", "Adding point %d with prob %.3f at [%.3f, %.3f, %.3f]",
129  // count, prob, pt.x(), pt.y(), pt.z());
130  }
131  }
132  // ROS_INFO_NAMED("collision_detection", "Contact point at [%.3f, %.3f, %.3f], cell size %.3f, occupied cells
133  // %d",
134  // contact_point.x(), contact_point.y(), contact_point.z(), cell_size, count);
135 
136  // octree->getOccupiedLeafsBBX(node_centers, bbx_min, bbx_max);
137  // ROS_ERROR_NAMED("collision_detection", "bad stuff in collision_octomap_filter.cpp; need to port octomap
138  // call for groovy");
139 
141  double depth;
142  if (getMetaballSurfaceProperties(node_centers, cell_size, iso_value, metaball_radius_multiple, contact_point,
143  n, depth, estimate_depth))
144  {
145  // only modify normal if the refinement predicts a "very different" result.
146  double divergence = contact_normal.angleTo(n);
147  if (divergence > allowed_angle_divergence)
148  {
149  modified++;
150  // ROS_INFO_NAMED("collision_detection", "Normals differ by %.3f, changing: [%.3f, %.3f, %.3f] -> [%.3f,
151  // %.3f, %.3f]",
152  // divergence, contact_normal.x(), contact_normal.y(), contact_normal.z(),
153  // n.x(), n.y(), n.z());
154  contact_info.normal = Eigen::Vector3d(n.x(), n.y(), n.z());
155  }
156 
157  if (estimate_depth)
158  contact_info.depth = depth;
159  }
160  }
161  }
162  }
163  }
164  return modified;
165 }
166 
167 bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, const double& spacing, const double& iso_value,
168  const double& r_multiple, const octomath::Vector3& contact_point,
169  octomath::Vector3& normal, double& depth, bool estimate_depth)
170 {
171  double intensity;
172  if (estimate_depth)
173  {
174  octomath::Vector3 surface_point;
175  if (findSurface(cloud, spacing, iso_value, r_multiple, contact_point, surface_point, normal))
176  {
177  depth = normal.dot(surface_point - contact_point); // do we prefer this, or magnitude of surface - contact?
178  return true;
179  }
180  else
181  {
182  return false;
183  }
184  }
185  else // just get normals, no depth
186  {
187  octomath::Vector3 gradient;
188  if (sampleCloud(cloud, spacing, r_multiple, contact_point, intensity, gradient))
189  {
190  normal = gradient.normalized();
191  return true;
192  }
193  else
194  {
195  return false;
196  }
197  }
198 }
199 
200 // --------------------------------------------------------------------------
201 // This algorithm is from Salisbury & Tarr's 1997 paper. It will find the
202 // closest point on the surface starting from a seed point that is close by
203 // following the direction of the field gradient.
204 bool findSurface(const octomap::point3d_list& cloud, const double& spacing, const double& iso_value,
205  const double& r_multiple, const octomath::Vector3& seed, octomath::Vector3& surface_point,
206  octomath::Vector3& normal)
207 {
208  const double epsilon = 1e-10;
209  const int iterations = 10;
210  double intensity = 0;
211 
212  octomath::Vector3 p = seed, dp, gs;
213  for (int i = 0; i < iterations; ++i)
214  {
215  if (!sampleCloud(cloud, spacing, r_multiple, p, intensity, gs))
216  return false;
217  double s = iso_value - intensity;
218  dp = (gs * -s) * (1.0 / std::max(gs.dot(gs), epsilon));
219  p = p + dp;
220  if (dp.dot(dp) < epsilon)
221  {
222  surface_point = p;
223  normal = gs.normalized();
224  return true;
225  }
226  }
227  return false;
228  // return p;
229 }
230 
231 bool sampleCloud(const octomap::point3d_list& cloud, const double& spacing, const double& r_multiple,
232  const octomath::Vector3& position, double& intensity, octomath::Vector3& gradient)
233 {
234  intensity = 0.f;
235  gradient = octomath::Vector3(0, 0, 0);
236 
237  double R = r_multiple * spacing; // TODO magic number!
238  // double T = 0.5; // TODO magic number!
239 
240  int NN = cloud.size();
241  if (NN == 0)
242  {
243  return false;
244  }
245 
246  // variables for Wyvill
247  double a = 0, b = 0, c = 0, R2 = 0, R4 = 0, R6 = 0, a1 = 0, b1 = 0, c1 = 0, a2 = 0, b2 = 0, c2 = 0;
248  bool WYVILL = true;
249 
250  octomap::point3d_list::const_iterator it;
251  for (it = cloud.begin(); it != cloud.end(); ++it)
252  {
253  octomath::Vector3 v = (*it);
254 
255  if (WYVILL)
256  {
257  R2 = R * R;
258  R4 = R2 * R2;
259  R6 = R4 * R2;
260  a = -4.0 / 9.0;
261  b = 17.0 / 9.0;
262  c = -22.0 / 9.0;
263  a1 = a / R6;
264  b1 = b / R4;
265  c1 = c / R2;
266  a2 = 6 * a1;
267  b2 = 4 * b1;
268  c2 = 2 * c1;
269  }
270  else
271  {
272  ROS_ERROR_NAMED("collision_detection", "This should not be called!");
273  }
274 
275  double f_val = 0;
276  octomath::Vector3 f_grad(0, 0, 0);
277 
278  octomath::Vector3 pos = position - v;
279  double r = pos.norm();
280  pos = pos * (1.0 / r);
281  if (r > R) // must skip points outside valid bounds.
282  {
283  continue;
284  }
285  double r2 = r * r;
286  double r3 = r * r2;
287  double r4 = r2 * r2;
288  double r5 = r3 * r2;
289  double r6 = r3 * r3;
290 
291  if (WYVILL)
292  {
293  f_val = (a1 * r6 + b1 * r4 + c1 * r2 + 1);
294  f_grad = pos * (a2 * r5 + b2 * r3 + c2 * r);
295  }
296  else
297  {
298  ROS_ERROR_NAMED("collision_detection", "This should not be called!");
299  double r_scaled = r / R;
300  // TODO still need to address the scaling...
301  f_val = pow((1 - r_scaled), 4) * (4 * r_scaled + 1);
302  f_grad = pos * (-4.0 / R * pow(1.0 - r_scaled, 3) * (4.0 * r_scaled + 1.0) + 4.0 / R * pow(1 - r_scaled, 4));
303  }
304 
305  // TODO: The whole library should be overhauled to follow the "gradient points out"
306  // convention of implicit functions.
307  intensity += f_val;
308  gradient += f_grad;
309  }
310  // implicit surface gradient convention points out, so we flip it.
311  gradient *= -1.0;
312  return true; // it worked
313 }
double dot(const Vector3 &other) const
#define ROS_WARN_NAMED(name,...)
double norm() const
XmlRpcServer s
ContactMap contacts
A map returning the pairs of ids of the bodies in contact, plus information about the contacts themse...
leaf_bbx_iterator begin_leafs_bbx(const OcTreeKey &min, const OcTreeKey &max, unsigned char maxDepth=0) const
double angleTo(const Vector3 &other) const
std::shared_ptr< const octomap::OcTree > octree
double epsilon
static const double depth
std::list< octomath::Vector3 > point3d_list
int refineContactNormals(const World::ObjectConstPtr &object, CollisionResult &res, double cell_bbx_search_distance=1.0, double allowed_angle_divergence=0.0, bool estimate_depth=false, double iso_value=0.5, double metaball_radius_multiple=1.5)
Re-proceses contact normals for an octomap by estimating a metaball iso-surface using the centers of ...
bool findSurface(const octomap::point3d_list &cloud, const double &spacing, const double &iso_value, const double &r_multiple, const octomath::Vector3 &seed, octomath::Vector3 &surface_point, octomath::Vector3 &normal)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
Vector3 normalized() const
bool getMetaballSurfaceProperties(const octomap::point3d_list &cloud, const double &spacing, const double &iso_value, const double &r_multiple, const octomath::Vector3 &contact_point, octomath::Vector3 &normal, double &depth, bool estimate_depth)
#define ROS_ERROR_NAMED(name,...)
const leaf_bbx_iterator end_leafs_bbx() const
r
bool sampleCloud(const octomap::point3d_list &cloud, const double &spacing, const double &r_multiple, const octomath::Vector3 &position, double &intensity, octomath::Vector3 &gradient)
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Oct 18 2018 02:47:08