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