collision_octomap_filter.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Adam Leeper */
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 // static const double ISO_VALUE  = 0.5; // TODO magic number! (though, probably a good one).
00045 // static const double R_MULTIPLE = 1.5; // TODO magic number! (though, probably a good one).
00046 
00047 // forward declarations
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   // iterate through contacts
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             // double prob = it->getOccupancy();
00123             if (octree->isNodeOccupied(*it))  // magic number!
00124             {
00125               count++;
00126               node_centers.push_back(pt);
00127               // logInform("Adding point %d with prob %.3f at [%.3f, %.3f, %.3f]", count, prob, pt.x(), pt.y(), pt.z());
00128             }
00129           }
00130           // logInform("Contact point at [%.3f, %.3f, %.3f], cell size %.3f, occupied cells %d",
00131           //          contact_point.x(), contact_point.y(), contact_point.z(), cell_size, count);
00132 
00133           // octree->getOccupiedLeafsBBX(node_centers, bbx_min, bbx_max);
00134           // logError("bad stuff in collision_octomap_filter.cpp; need to port octomap call for groovy");
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             // only modify normal if the refinement predicts a "very different" result.
00142             double divergence = contact_normal.angleTo(n);
00143             if (divergence > allowed_angle_divergence)
00144             {
00145               modified++;
00146               //              logInform("Normals differ by %.3f, changing: [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]",
00147               //                        divergence,
00148               //                        contact_normal.x(), contact_normal.y(), contact_normal.z(),
00149               //                        n.x(), n.y(), n.z());
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);  // do we prefer this, or magnitude of surface - contact?
00174       return true;
00175     }
00176     else
00177     {
00178       return false;
00179     }
00180   }
00181   else  // just get normals, no depth
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 // This algorithm is from Salisbury & Tarr's 1997 paper.  It will find the
00198 // closest point on the surface starting from a seed point that is close by
00199 // following the direction of the field gradient.
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   //    return p;
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;  // TODO magic number!
00234   // double T = 0.5; // TODO magic number!
00235 
00236   int NN = cloud.size();
00237   if (NN == 0)
00238   {
00239     return false;
00240   }
00241 
00242   // variables for Wyvill
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)  // must skip points outside valid bounds.
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       // TODO still need to address the scaling...
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     // TODO:  The whole library should be overhauled to follow the "gradient points out"
00302     //        convention of implicit functions.
00303     intensity += f_val;
00304     gradient += f_grad;
00305   }
00306   // implicit surface gradient convention points out, so we flip it.
00307   gradient *= -1.0;
00308   return true;  // it worked
00309 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Apr 23 2018 10:24:45