62 double cell_bbx_search_distance,
double allowed_angle_divergence,
63 bool estimate_depth,
double iso_value,
double metaball_radius_multiple)
67 ROS_ERROR_NAMED(
"collision_detection",
"No valid Object passed in, cannot refine Normals!");
72 ROS_WARN_NAMED(
"collision_detection",
"There do not appear to be any contacts, so there is nothing to refine!");
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;
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;
96 if (!object->shapes_.empty())
99 std::shared_ptr<const shapes::OcTree> shape_octree = std::dynamic_pointer_cast<
const shapes::OcTree>(shape);
102 std::shared_ptr<const octomap::OcTree> octree = shape_octree->
octree;
103 cell_size = octree->getResolution();
104 for (
auto& contact_info : contact_vector)
106 const Eigen::Vector3d& point = contact_info.pos;
107 const Eigen::Vector3d& normal = contact_info.normal;
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;
120 for (; it != leafs_end; ++it)
124 if (octree->isNodeOccupied(*it))
127 node_centers.push_back(pt);
143 n, depth, estimate_depth))
146 double divergence = contact_normal.
angleTo(n);
147 if (divergence > allowed_angle_divergence)
154 contact_info.normal = Eigen::Vector3d(n.
x(), n.
y(), n.
z());
158 contact_info.depth =
depth;
175 if (
findSurface(cloud, spacing, iso_value, r_multiple, contact_point, surface_point, normal))
177 depth = normal.
dot(surface_point - contact_point);
188 if (
sampleCloud(cloud, spacing, r_multiple, contact_point, intensity, gradient))
209 const int iterations = 10;
210 double intensity = 0;
213 for (
int i = 0; i < iterations; ++i)
215 if (!
sampleCloud(cloud, spacing, r_multiple, p, intensity, gs))
217 double s = iso_value - intensity;
218 dp = (gs * -s) * (1.0 / std::max(gs.dot(gs), epsilon));
220 if (dp.dot(dp) < epsilon)
237 double R = r_multiple * spacing;
240 int NN = cloud.size();
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;
250 octomap::point3d_list::const_iterator it;
251 for (it = cloud.begin(); it != cloud.end(); ++it)
279 double r = pos.
norm();
280 pos = pos * (1.0 / r);
293 f_val = (a1 * r6 + b1 * r4 + c1 * r2 + 1);
294 f_grad = pos * (a2 * r5 + b2 * r3 + c2 * r);
299 double r_scaled = r / R;
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));
double dot(const Vector3 &other) const
#define ROS_WARN_NAMED(name,...)
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
Representation of a collision checking result.
double angleTo(const Vector3 &other) const
std::size_t contact_count
Number of contacts returned.
std::shared_ptr< const octomap::OcTree > octree
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
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