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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Sep 18 2022 02:26:15