61 double cell_bbx_search_distance,
double allowed_angle_divergence,
62 bool estimate_depth,
double iso_value,
double metaball_radius_multiple)
66 ROS_ERROR_NAMED(
"collision_detection",
"No valid Object passed in, cannot refine Normals!");
71 ROS_WARN_NAMED(
"collision_detection",
"There do not appear to be any contacts, so there is nothing to refine!");
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;
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;
95 if (!object->shapes_.empty())
98 std::shared_ptr<const shapes::OcTree> shape_octree = std::dynamic_pointer_cast<const shapes::OcTree>(shape);
101 std::shared_ptr<const octomap::OcTree> octree = shape_octree->octree;
102 cell_size = octree->getResolution();
103 for (
auto& contact_info : contact_vector)
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;
119 for (; it != leafs_end; ++it)
123 if (octree->isNodeOccupied(*it))
126 node_centers.push_back(pt);
142 n, depth, estimate_depth))
145 double divergence = contact_normal.
angleTo(n);
146 if (divergence > allowed_angle_divergence)
157 contact_info.depth = depth;
174 if (
findSurface(cloud, spacing, iso_value, r_multiple, contact_point, surface_point, normal))
176 depth = normal.
dot(surface_point - contact_point);
187 if (
sampleCloud(cloud, spacing, r_multiple, contact_point, intensity, gradient))
208 const int iterations = 10;
209 double intensity = 0;
212 for (
int i = 0; i < iterations; ++i)
214 if (!
sampleCloud(cloud, spacing, r_multiple, p, intensity, gs))
216 double s = iso_value - intensity;
217 dp = (gs * -
s) * (1.0 / std::max(gs.dot(gs),
epsilon));
236 double r = r_multiple * spacing;
239 int nn = cloud.size();
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;
275 double r = pos.
norm();
276 pos = pos * (1.0 /
r);
289 f_val = (a1 * r6 + b1 * r4 + c1 * r2 + 1);
290 f_grad = pos * (a2 * r5 + b2 * r3 + c2 *
r);
295 double r_scaled =
r /
r;
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));