00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/collision_detection/collision_common.h>
00038 #include <moveit/collision_detection/collision_octomap_filter.h>
00039 #include <moveit/collision_detection/collision_world.h>
00040 #include <octomap/math/Vector3.h>
00041 #include <octomap/math/Utils.h>
00042 #include <octomap/octomap.h>
00043
00044
00045
00046
00047
00048 bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, const double& spacing, const double& iso_value,
00049 const double& r_multiple, const octomath::Vector3& contact_point,
00050 octomath::Vector3& normal, double& depth, bool estimate_depth);
00051
00052 bool findSurface(const octomap::point3d_list& cloud, const double& spacing, const double& iso_value,
00053 const double& r_multiple, const octomath::Vector3& seed, octomath::Vector3& surface_point,
00054 octomath::Vector3& normal);
00055
00056 bool sampleCloud(const octomap::point3d_list& cloud, const double& spacing, const double& r_multiple,
00057 const octomath::Vector3& position, double& intensity, octomath::Vector3& gradient);
00058
00059 int collision_detection::refineContactNormals(const World::ObjectConstPtr& object, CollisionResult& res,
00060 double cell_bbx_search_distance, double allowed_angle_divergence,
00061 bool estimate_depth, double iso_value, double metaball_radius_multiple)
00062 {
00063 if (!object)
00064 {
00065 logError("No valid Object passed in, cannot refine Normals!");
00066 return 0;
00067 }
00068 if (res.contact_count < 1)
00069 {
00070 logWarn("There do not appear to be any contacts, so there is nothing to refine!");
00071 return 0;
00072 }
00073
00074 int modified = 0;
00075
00076
00077 for (collision_detection::CollisionResult::ContactMap::iterator it = res.contacts.begin(); it != res.contacts.end();
00078 ++it)
00079 {
00080 std::string contact1 = it->first.first;
00081 std::string contact2 = it->first.second;
00082 std::string octomap_name = "";
00083 std::vector<collision_detection::Contact>& contact_vector = it->second;
00084
00085 if (contact1.find("octomap") != std::string::npos)
00086 octomap_name = contact1;
00087 else if (contact2.find("octomap") != std::string::npos)
00088 octomap_name = contact2;
00089 else
00090 {
00091 continue;
00092 }
00093
00094 double cell_size = 0;
00095 if (!object->shapes_.empty())
00096 {
00097 const shapes::ShapeConstPtr& shape = object->shapes_[0];
00098 boost::shared_ptr<const shapes::OcTree> shape_octree = boost::dynamic_pointer_cast<const shapes::OcTree>(shape);
00099 if (shape_octree)
00100 {
00101 boost::shared_ptr<const octomap::OcTree> octree = shape_octree->octree;
00102 cell_size = octree->getResolution();
00103 for (size_t contact_index = 0; contact_index < contact_vector.size(); contact_index++)
00104 {
00105 const Eigen::Vector3d& point = contact_vector[contact_index].pos;
00106 const Eigen::Vector3d& normal = contact_vector[contact_index].normal;
00107
00108 octomath::Vector3 contact_point(point[0], point[1], point[2]);
00109 octomath::Vector3 contact_normal(normal[0], normal[1], normal[2]);
00110 octomath::Vector3 diagonal = octomath::Vector3(1, 1, 1);
00111 octomath::Vector3 bbx_min = contact_point - diagonal * cell_size * cell_bbx_search_distance;
00112 octomath::Vector3 bbx_max = contact_point + diagonal * cell_size * cell_bbx_search_distance;
00113 octomap::point3d_list node_centers;
00114 octomap::OcTreeBaseImpl<octomap::OcTreeNode, octomap::AbstractOccupancyOcTree>::leaf_bbx_iterator it =
00115 octree->begin_leafs_bbx(bbx_min, bbx_max);
00116 octomap::OcTreeBaseImpl<octomap::OcTreeNode, octomap::AbstractOccupancyOcTree>::leaf_bbx_iterator leafs_end =
00117 octree->end_leafs_bbx();
00118 int count = 0;
00119 for (; it != leafs_end; ++it)
00120 {
00121 octomap::point3d pt = it.getCoordinate();
00122
00123 if (octree->isNodeOccupied(*it))
00124 {
00125 count++;
00126 node_centers.push_back(pt);
00127
00128 }
00129 }
00130
00131
00132
00133
00134
00135
00136 octomath::Vector3 n;
00137 double depth;
00138 if (getMetaballSurfaceProperties(node_centers, cell_size, iso_value, metaball_radius_multiple, contact_point,
00139 n, depth, estimate_depth))
00140 {
00141
00142 double divergence = contact_normal.angleTo(n);
00143 if (divergence > allowed_angle_divergence)
00144 {
00145 modified++;
00146
00147
00148
00149
00150 contact_vector[contact_index].normal = Eigen::Vector3d(n.x(), n.y(), n.z());
00151 }
00152
00153 if (estimate_depth)
00154 contact_vector[contact_index].depth = depth;
00155 }
00156 }
00157 }
00158 }
00159 }
00160 return modified;
00161 }
00162
00163 bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, const double& spacing, const double& iso_value,
00164 const double& r_multiple, const octomath::Vector3& contact_point,
00165 octomath::Vector3& normal, double& depth, bool estimate_depth)
00166 {
00167 double intensity;
00168 if (estimate_depth)
00169 {
00170 octomath::Vector3 surface_point;
00171 if (findSurface(cloud, spacing, iso_value, r_multiple, contact_point, surface_point, normal))
00172 {
00173 depth = normal.dot(surface_point - contact_point);
00174 return true;
00175 }
00176 else
00177 {
00178 return false;
00179 }
00180 }
00181 else
00182 {
00183 octomath::Vector3 gradient;
00184 if (sampleCloud(cloud, spacing, r_multiple, contact_point, intensity, gradient))
00185 {
00186 normal = gradient.normalized();
00187 return true;
00188 }
00189 else
00190 {
00191 return false;
00192 }
00193 }
00194 }
00195
00196
00197
00198
00199
00200 bool findSurface(const octomap::point3d_list& cloud, const double& spacing, const double& iso_value,
00201 const double& r_multiple, const octomath::Vector3& seed, octomath::Vector3& surface_point,
00202 octomath::Vector3& normal)
00203 {
00204 const double epsilon = 1e-10;
00205 const int iterations = 10;
00206 double intensity = 0;
00207
00208 octomath::Vector3 p = seed, dp, gs;
00209 for (int i = 0; i < iterations; ++i)
00210 {
00211 if (!sampleCloud(cloud, spacing, r_multiple, p, intensity, gs))
00212 return false;
00213 double s = iso_value - intensity;
00214 dp = (gs * -s) * (1.0 / std::max(gs.dot(gs), epsilon));
00215 p = p + dp;
00216 if (dp.dot(dp) < epsilon)
00217 {
00218 surface_point = p;
00219 normal = gs.normalized();
00220 return true;
00221 }
00222 }
00223 return false;
00224
00225 }
00226
00227 bool sampleCloud(const octomap::point3d_list& cloud, const double& spacing, const double& r_multiple,
00228 const octomath::Vector3& position, double& intensity, octomath::Vector3& gradient)
00229 {
00230 intensity = 0.f;
00231 gradient = octomath::Vector3(0, 0, 0);
00232
00233 double R = r_multiple * spacing;
00234
00235
00236 int NN = cloud.size();
00237 if (NN == 0)
00238 {
00239 return false;
00240 }
00241
00242
00243 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;
00244 bool WYVILL = true;
00245
00246 octomap::point3d_list::const_iterator it;
00247 for (it = cloud.begin(); it != cloud.end(); ++it)
00248 {
00249 octomath::Vector3 v = (*it);
00250
00251 if (WYVILL)
00252 {
00253 R2 = R * R;
00254 R4 = R2 * R2;
00255 R6 = R4 * R2;
00256 a = -4.0 / 9.0;
00257 b = 17.0 / 9.0;
00258 c = -22.0 / 9.0;
00259 a1 = a / R6;
00260 b1 = b / R4;
00261 c1 = c / R2;
00262 a2 = 6 * a1;
00263 b2 = 4 * b1;
00264 c2 = 2 * c1;
00265 }
00266 else
00267 {
00268 logError("This should not be called!");
00269 }
00270
00271 double f_val = 0;
00272 octomath::Vector3 f_grad(0, 0, 0);
00273
00274 octomath::Vector3 pos = position - v;
00275 double r = pos.norm();
00276 pos = pos * (1.0 / r);
00277 if (r > R)
00278 {
00279 continue;
00280 }
00281 double r2 = r * r;
00282 double r3 = r * r2;
00283 double r4 = r2 * r2;
00284 double r5 = r3 * r2;
00285 double r6 = r3 * r3;
00286
00287 if (WYVILL)
00288 {
00289 f_val = (a1 * r6 + b1 * r4 + c1 * r2 + 1);
00290 f_grad = pos * (a2 * r5 + b2 * r3 + c2 * r);
00291 }
00292 else
00293 {
00294 logError("This should not be called!");
00295 double r_scaled = r / R;
00296
00297 f_val = pow((1 - r_scaled), 4) * (4 * r_scaled + 1);
00298 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));
00299 }
00300
00301
00302
00303 intensity += f_val;
00304 gradient += f_grad;
00305 }
00306
00307 gradient *= -1.0;
00308 return true;
00309 }