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  CONSOLE_BRIDGE_logError("No valid Object passed in, cannot refine Normals!");
68  return 0;
69  }
70  if (res.contact_count < 1)
71  {
72  CONSOLE_BRIDGE_logWarn("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  // logInform("Adding point %d with prob %.3f at [%.3f, %.3f, %.3f]", count, prob, pt.x(), pt.y(), pt.z());
129  }
130  }
131  // logInform("Contact point at [%.3f, %.3f, %.3f], cell size %.3f, occupied cells %d",
132  // contact_point.x(), contact_point.y(), contact_point.z(), cell_size, count);
133 
134  // octree->getOccupiedLeafsBBX(node_centers, bbx_min, bbx_max);
135  // CONSOLE_BRIDGE_logError("bad stuff in collision_octomap_filter.cpp; need to port octomap call for groovy");
136 
138  double depth;
139  if (getMetaballSurfaceProperties(node_centers, cell_size, iso_value, metaball_radius_multiple, contact_point,
140  n, depth, estimate_depth))
141  {
142  // only modify normal if the refinement predicts a "very different" result.
143  double divergence = contact_normal.angleTo(n);
144  if (divergence > allowed_angle_divergence)
145  {
146  modified++;
147  // logInform("Normals differ by %.3f, changing: [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]",
148  // divergence,
149  // contact_normal.x(), contact_normal.y(), contact_normal.z(),
150  // n.x(), n.y(), n.z());
151  contact_info.normal = Eigen::Vector3d(n.x(), n.y(), n.z());
152  }
153 
154  if (estimate_depth)
155  contact_info.depth = depth;
156  }
157  }
158  }
159  }
160  }
161  return modified;
162 }
163 
164 bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, const double& spacing, const double& iso_value,
165  const double& r_multiple, const octomath::Vector3& contact_point,
166  octomath::Vector3& normal, double& depth, bool estimate_depth)
167 {
168  double intensity;
169  if (estimate_depth)
170  {
171  octomath::Vector3 surface_point;
172  if (findSurface(cloud, spacing, iso_value, r_multiple, contact_point, surface_point, normal))
173  {
174  depth = normal.dot(surface_point - contact_point); // do we prefer this, or magnitude of surface - contact?
175  return true;
176  }
177  else
178  {
179  return false;
180  }
181  }
182  else // just get normals, no depth
183  {
184  octomath::Vector3 gradient;
185  if (sampleCloud(cloud, spacing, r_multiple, contact_point, intensity, gradient))
186  {
187  normal = gradient.normalized();
188  return true;
189  }
190  else
191  {
192  return false;
193  }
194  }
195 }
196 
197 // --------------------------------------------------------------------------
198 // This algorithm is from Salisbury & Tarr's 1997 paper. It will find the
199 // closest point on the surface starting from a seed point that is close by
200 // following the direction of the field gradient.
201 bool findSurface(const octomap::point3d_list& cloud, const double& spacing, const double& iso_value,
202  const double& r_multiple, const octomath::Vector3& seed, octomath::Vector3& surface_point,
203  octomath::Vector3& normal)
204 {
205  const double epsilon = 1e-10;
206  const int iterations = 10;
207  double intensity = 0;
208 
209  octomath::Vector3 p = seed, dp, gs;
210  for (int i = 0; i < iterations; ++i)
211  {
212  if (!sampleCloud(cloud, spacing, r_multiple, p, intensity, gs))
213  return false;
214  double s = iso_value - intensity;
215  dp = (gs * -s) * (1.0 / std::max(gs.dot(gs), epsilon));
216  p = p + dp;
217  if (dp.dot(dp) < epsilon)
218  {
219  surface_point = p;
220  normal = gs.normalized();
221  return true;
222  }
223  }
224  return false;
225  // return p;
226 }
227 
228 bool sampleCloud(const octomap::point3d_list& cloud, const double& spacing, const double& r_multiple,
229  const octomath::Vector3& position, double& intensity, octomath::Vector3& gradient)
230 {
231  intensity = 0.f;
232  gradient = octomath::Vector3(0, 0, 0);
233 
234  double R = r_multiple * spacing; // TODO magic number!
235  // double T = 0.5; // TODO magic number!
236 
237  int NN = cloud.size();
238  if (NN == 0)
239  {
240  return false;
241  }
242 
243  // variables for Wyvill
244  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;
245  bool WYVILL = true;
246 
247  octomap::point3d_list::const_iterator it;
248  for (it = cloud.begin(); it != cloud.end(); ++it)
249  {
250  octomath::Vector3 v = (*it);
251 
252  if (WYVILL)
253  {
254  R2 = R * R;
255  R4 = R2 * R2;
256  R6 = R4 * R2;
257  a = -4.0 / 9.0;
258  b = 17.0 / 9.0;
259  c = -22.0 / 9.0;
260  a1 = a / R6;
261  b1 = b / R4;
262  c1 = c / R2;
263  a2 = 6 * a1;
264  b2 = 4 * b1;
265  c2 = 2 * c1;
266  }
267  else
268  {
269  CONSOLE_BRIDGE_logError("This should not be called!");
270  }
271 
272  double f_val = 0;
273  octomath::Vector3 f_grad(0, 0, 0);
274 
275  octomath::Vector3 pos = position - v;
276  double r = pos.norm();
277  pos = pos * (1.0 / r);
278  if (r > R) // must skip points outside valid bounds.
279  {
280  continue;
281  }
282  double r2 = r * r;
283  double r3 = r * r2;
284  double r4 = r2 * r2;
285  double r5 = r3 * r2;
286  double r6 = r3 * r3;
287 
288  if (WYVILL)
289  {
290  f_val = (a1 * r6 + b1 * r4 + c1 * r2 + 1);
291  f_grad = pos * (a2 * r5 + b2 * r3 + c2 * r);
292  }
293  else
294  {
295  CONSOLE_BRIDGE_logError("This should not be called!");
296  double r_scaled = r / R;
297  // TODO still need to address the scaling...
298  f_val = pow((1 - r_scaled), 4) * (4 * r_scaled + 1);
299  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));
300  }
301 
302  // TODO: The whole library should be overhauled to follow the "gradient points out"
303  // convention of implicit functions.
304  intensity += f_val;
305  gradient += f_grad;
306  }
307  // implicit surface gradient convention points out, so we flip it.
308  gradient *= -1.0;
309  return true; // it worked
310 }
double dot(const Vector3 &other) const
#define CONSOLE_BRIDGE_logError(fmt,...)
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
#define CONSOLE_BRIDGE_logWarn(fmt,...)
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)
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 Sun Jan 21 2018 03:54:28