distance_field.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: Mrinal Kalakrishnan, Ken Anderson, E. Gil Jones */
00036 
00037 #include <moveit/distance_field/distance_field.h>
00038 #include <moveit/distance_field/find_internal_points.h>
00039 #include <geometric_shapes/body_operations.h>
00040 #include <eigen_conversions/eigen_msg.h>
00041 #include <console_bridge/console.h>
00042 #include <octomap/octomap.h>
00043 #include <octomap/OcTree.h>
00044 
00045 distance_field::DistanceField::DistanceField(double size_x, double size_y, double size_z, double resolution,
00046                                              double origin_x, double origin_y, double origin_z)
00047   : size_x_(size_x)
00048   , size_y_(size_y)
00049   , size_z_(size_z)
00050   , origin_x_(origin_x)
00051   , origin_y_(origin_y)
00052   , origin_z_(origin_z)
00053   , resolution_(resolution)
00054   , inv_twice_resolution_(1.0 / (2.0 * resolution_))
00055 {
00056 }
00057 
00058 distance_field::DistanceField::~DistanceField()
00059 {
00060 }
00061 
00062 double distance_field::DistanceField::getDistanceGradient(double x, double y, double z, double& gradient_x,
00063                                                           double& gradient_y, double& gradient_z, bool& in_bounds) const
00064 {
00065   int gx, gy, gz;
00066 
00067   worldToGrid(x, y, z, gx, gy, gz);
00068 
00069   // if out of bounds, return max_distance, and 0 gradient
00070   // we need extra padding of 1 to get gradients
00071   if (gx < 1 || gy < 1 || gz < 1 || gx >= getXNumCells() - 1 || gy >= getYNumCells() - 1 || gz >= getZNumCells() - 1)
00072   {
00073     gradient_x = 0.0;
00074     gradient_y = 0.0;
00075     gradient_z = 0.0;
00076     in_bounds = false;
00077     return getUninitializedDistance();
00078   }
00079 
00080   gradient_x = (getDistance(gx + 1, gy, gz) - getDistance(gx - 1, gy, gz)) * inv_twice_resolution_;
00081   gradient_y = (getDistance(gx, gy + 1, gz) - getDistance(gx, gy - 1, gz)) * inv_twice_resolution_;
00082   gradient_z = (getDistance(gx, gy, gz + 1) - getDistance(gx, gy, gz - 1)) * inv_twice_resolution_;
00083 
00084   in_bounds = true;
00085   return getDistance(gx, gy, gz);
00086 }
00087 
00088 void distance_field::DistanceField::getIsoSurfaceMarkers(double min_distance, double max_distance,
00089                                                          const std::string& frame_id, const ros::Time stamp,
00090                                                          visualization_msgs::Marker& inf_marker) const
00091 {
00092   inf_marker.points.clear();
00093   inf_marker.header.frame_id = frame_id;
00094   inf_marker.header.stamp = stamp;
00095   inf_marker.ns = "distance_field";
00096   inf_marker.id = 1;
00097   inf_marker.type = visualization_msgs::Marker::CUBE_LIST;
00098   inf_marker.action = visualization_msgs::Marker::MODIFY;
00099   inf_marker.scale.x = resolution_;
00100   inf_marker.scale.y = resolution_;
00101   inf_marker.scale.z = resolution_;
00102   inf_marker.color.r = 1.0;
00103   inf_marker.color.g = 0.0;
00104   inf_marker.color.b = 0.0;
00105   inf_marker.color.a = 0.1;
00106   // inf_marker.lifetime = ros::Duration(30.0);
00107 
00108   inf_marker.points.reserve(100000);
00109   for (int x = 0; x < getXNumCells(); ++x)
00110   {
00111     for (int y = 0; y < getYNumCells(); ++y)
00112     {
00113       for (int z = 0; z < getZNumCells(); ++z)
00114       {
00115         double dist = getDistance(x, y, z);
00116 
00117         if (dist >= min_distance && dist <= max_distance)
00118         {
00119           int last = inf_marker.points.size();
00120           inf_marker.points.resize(last + 1);
00121           double nx, ny, nz;
00122           gridToWorld(x, y, z, nx, ny, nz);
00123           Eigen::Translation3d vec(nx, ny, nz);
00124           inf_marker.points[last].x = vec.x();
00125           inf_marker.points[last].y = vec.y();
00126           inf_marker.points[last].z = vec.z();
00127         }
00128       }
00129     }
00130   }
00131 }
00132 
00133 void distance_field::DistanceField::getGradientMarkers(double min_distance, double max_distance,
00134                                                        const std::string& frame_id, const ros::Time& stamp,
00135                                                        visualization_msgs::MarkerArray& marker_array) const
00136 {
00137   Eigen::Vector3d unitX(1, 0, 0);
00138   Eigen::Vector3d unitY(0, 1, 0);
00139   Eigen::Vector3d unitZ(0, 0, 1);
00140 
00141   int id = 0;
00142 
00143   for (int x = 0; x < getXNumCells(); ++x)
00144   {
00145     for (int y = 0; y < getYNumCells(); ++y)
00146     {
00147       for (int z = 0; z < getZNumCells(); ++z)
00148       {
00149         double worldX, worldY, worldZ;
00150         gridToWorld(x, y, z, worldX, worldY, worldZ);
00151 
00152         double gradientX, gradientY, gradientZ;
00153         bool in_bounds;
00154         double distance = getDistanceGradient(worldX, worldY, worldZ, gradientX, gradientY, gradientZ, in_bounds);
00155         Eigen::Vector3d gradient(gradientX, gradientY, gradientZ);
00156 
00157         if (in_bounds && distance >= min_distance && distance <= max_distance && gradient.norm() > 0)
00158         {
00159           visualization_msgs::Marker marker;
00160 
00161           marker.header.frame_id = frame_id;
00162           marker.header.stamp = stamp;
00163 
00164           marker.ns = "distance_field_gradient";
00165           marker.id = id++;
00166           marker.type = visualization_msgs::Marker::ARROW;
00167           marker.action = visualization_msgs::Marker::ADD;
00168 
00169           marker.pose.position.x = worldX;
00170           marker.pose.position.y = worldY;
00171           marker.pose.position.z = worldZ;
00172 
00173           // Eigen::Vector3d axis = gradient.cross(unitX).norm() > 0 ? gradient.cross(unitX) : unitY;
00174           // double angle = -gradient.angle(unitX);
00175           // Eigen::AngleAxisd rotation(angle, axis);
00176 
00177           // marker.pose.orientation.x = rotation.rotation().x();
00178           // marker.pose.orientation.y = rotation.rotation().y();
00179           // marker.pose.orientation.z = rotation.rotation().z();
00180           // marker.pose.orientation.w = rotation.rotation().w();
00181 
00182           marker.scale.x = getResolution();
00183           marker.scale.y = getResolution();
00184           marker.scale.z = getResolution();
00185 
00186           marker.color.r = 0.0;
00187           marker.color.g = 0.0;
00188           marker.color.b = 1.0;
00189           marker.color.a = 1.0;
00190 
00191           marker_array.markers.push_back(marker);
00192         }
00193       }
00194     }
00195   }
00196 }
00197 
00198 bool distance_field::DistanceField::getShapePoints(const shapes::Shape* shape, const Eigen::Affine3d& pose,
00199                                                    EigenSTL::vector_Vector3d* points)
00200 {
00201   if (shape->type == shapes::OCTREE)
00202   {
00203     const shapes::OcTree* oc = dynamic_cast<const shapes::OcTree*>(shape);
00204     if (!oc)
00205     {
00206       logError("Problem dynamic casting shape that claims to be OcTree");
00207       return false;
00208     }
00209     getOcTreePoints(oc->octree.get(), points);
00210   }
00211   else
00212   {
00213     bodies::Body* body = bodies::createBodyFromShape(shape);
00214     body->setPose(pose);
00215     findInternalPointsConvex(*body, resolution_, *points);
00216     delete body;
00217   }
00218   return true;
00219 }
00220 
00221 void distance_field::DistanceField::addShapeToField(const shapes::Shape* shape, const Eigen::Affine3d& pose)
00222 {
00223   EigenSTL::vector_Vector3d point_vec;
00224   getShapePoints(shape, pose, &point_vec);
00225   addPointsToField(point_vec);
00226 }
00227 
00228 // DEPRECATED
00229 void distance_field::DistanceField::addShapeToField(const shapes::Shape* shape, const geometry_msgs::Pose& pose)
00230 {
00231   Eigen::Affine3d pose_e;
00232   tf::poseMsgToEigen(pose, pose_e);
00233   addShapeToField(shape, pose_e);
00234 }
00235 
00236 void distance_field::DistanceField::getOcTreePoints(const octomap::OcTree* octree, EigenSTL::vector_Vector3d* points)
00237 {
00238   // lower extent
00239   double min_x, min_y, min_z;
00240   gridToWorld(0, 0, 0, min_x, min_y, min_z);
00241 
00242   octomap::point3d bbx_min(min_x, min_y, min_z);
00243 
00244   int num_x = getXNumCells();
00245   int num_y = getYNumCells();
00246   int num_z = getZNumCells();
00247 
00248   // upper extent
00249   double max_x, max_y, max_z;
00250   gridToWorld(num_x, num_y, num_z, max_x, max_y, max_z);
00251 
00252   octomap::point3d bbx_max(max_x, max_y, max_z);
00253 
00254   for (octomap::OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(bbx_min, bbx_max), end = octree->end_leafs_bbx();
00255        it != end; ++it)
00256   {
00257     if (octree->isNodeOccupied(*it))
00258     {
00259       if (it.getSize() <= resolution_)
00260       {
00261         Eigen::Vector3d point(it.getX(), it.getY(), it.getZ());
00262         points->push_back(point);
00263       }
00264       else
00265       {
00266         double ceil_val = ceil(it.getSize() / resolution_) * resolution_ / 2.0;
00267         for (double x = it.getX() - ceil_val; x <= it.getX() + ceil_val; x += resolution_)
00268         {
00269           for (double y = it.getY() - ceil_val; y <= it.getY() + ceil_val; y += resolution_)
00270           {
00271             for (double z = it.getZ() - ceil_val; z <= it.getZ() + ceil_val; z += resolution_)
00272             {
00273               points->push_back(Eigen::Vector3d(x, y, z));
00274             }
00275           }
00276         }
00277       }
00278     }
00279   }
00280 }
00281 
00282 void distance_field::DistanceField::addOcTreeToField(const octomap::OcTree* octree)
00283 {
00284   EigenSTL::vector_Vector3d points;
00285   getOcTreePoints(octree, &points);
00286   addPointsToField(points);
00287 }
00288 
00289 void distance_field::DistanceField::moveShapeInField(const shapes::Shape* shape, const Eigen::Affine3d& old_pose,
00290                                                      const Eigen::Affine3d& new_pose)
00291 {
00292   if (shape->type == shapes::OCTREE)
00293   {
00294     logWarn("Move shape not supported for Octree");
00295     return;
00296   }
00297   bodies::Body* body = bodies::createBodyFromShape(shape);
00298   body->setPose(old_pose);
00299   EigenSTL::vector_Vector3d old_point_vec;
00300   findInternalPointsConvex(*body, resolution_, old_point_vec);
00301   body->setPose(new_pose);
00302   EigenSTL::vector_Vector3d new_point_vec;
00303   findInternalPointsConvex(*body, resolution_, new_point_vec);
00304   delete body;
00305   updatePointsInField(old_point_vec, new_point_vec);
00306 }
00307 
00308 // DEPRECATED
00309 void distance_field::DistanceField::moveShapeInField(const shapes::Shape* shape, const geometry_msgs::Pose& old_pose,
00310                                                      const geometry_msgs::Pose& new_pose)
00311 {
00312   Eigen::Affine3d old_pose_e, new_pose_e;
00313   tf::poseMsgToEigen(old_pose, old_pose_e);
00314   tf::poseMsgToEigen(new_pose, new_pose_e);
00315   moveShapeInField(shape, old_pose_e, new_pose_e);
00316 }
00317 
00318 void distance_field::DistanceField::removeShapeFromField(const shapes::Shape* shape, const Eigen::Affine3d& pose)
00319 {
00320   bodies::Body* body = bodies::createBodyFromShape(shape);
00321   body->setPose(pose);
00322   EigenSTL::vector_Vector3d point_vec;
00323   findInternalPointsConvex(*body, resolution_, point_vec);
00324   delete body;
00325   removePointsFromField(point_vec);
00326 }
00327 
00328 // DEPRECATED
00329 void distance_field::DistanceField::removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::Pose& pose)
00330 {
00331   Eigen::Affine3d pose_e;
00332   tf::poseMsgToEigen(pose, pose_e);
00333   removeShapeFromField(shape, pose_e);
00334 }
00335 
00336 void distance_field::DistanceField::getPlaneMarkers(distance_field::PlaneVisualizationType type, double length,
00337                                                     double width, double height, const Eigen::Vector3d& origin,
00338                                                     const std::string& frame_id, const ros::Time stamp,
00339                                                     visualization_msgs::Marker& plane_marker) const
00340 {
00341   plane_marker.header.frame_id = frame_id;
00342   plane_marker.header.stamp = stamp;
00343   plane_marker.ns = "distance_field_plane";
00344   plane_marker.id = 1;
00345   plane_marker.type = visualization_msgs::Marker::CUBE_LIST;
00346   plane_marker.action = visualization_msgs::Marker::ADD;
00347   plane_marker.scale.x = resolution_;
00348   plane_marker.scale.y = resolution_;
00349   plane_marker.scale.z = resolution_;
00350   // plane_marker.lifetime = ros::Duration(30.0);
00351 
00352   plane_marker.points.reserve(100000);
00353   plane_marker.colors.reserve(100000);
00354 
00355   double minX = 0;
00356   double maxX = 0;
00357   double minY = 0;
00358   double maxY = 0;
00359   double minZ = 0;
00360   double maxZ = 0;
00361 
00362   switch (type)
00363   {
00364     case XYPlane:
00365       minZ = height;
00366       maxZ = height;
00367 
00368       minX = -length / 2.0;
00369       maxX = length / 2.0;
00370       minY = -width / 2.0;
00371       maxY = width / 2.0;
00372       break;
00373     case XZPlane:
00374       minY = height;
00375       maxY = height;
00376 
00377       minX = -length / 2.0;
00378       maxX = length / 2.0;
00379       minZ = -width / 2.0;
00380       maxZ = width / 2.0;
00381       break;
00382     case YZPlane:
00383       minX = height;
00384       maxX = height;
00385 
00386       minY = -length / 2.0;
00387       maxY = length / 2.0;
00388       minZ = -width / 2.0;
00389       maxZ = width / 2.0;
00390       break;
00391   }
00392 
00393   minX += origin.x();
00394   minY += origin.y();
00395   minZ += origin.z();
00396 
00397   maxX += origin.x();
00398   maxY += origin.y();
00399   maxZ += origin.z();
00400 
00401   int minXCell = 0;
00402   int maxXCell = 0;
00403   int minYCell = 0;
00404   int maxYCell = 0;
00405   int minZCell = 0;
00406   int maxZCell = 0;
00407 
00408   worldToGrid(minX, minY, minZ, minXCell, minYCell, minZCell);
00409   worldToGrid(maxX, maxY, maxZ, maxXCell, maxYCell, maxZCell);
00410   plane_marker.color.a = 1.0;
00411   for (int x = minXCell; x <= maxXCell; ++x)
00412   {
00413     for (int y = minYCell; y <= maxYCell; ++y)
00414     {
00415       for (int z = minZCell; z <= maxZCell; ++z)
00416       {
00417         if (!isCellValid(x, y, z))
00418         {
00419           continue;
00420         }
00421         double dist = getDistance(x, y, z);
00422         int last = plane_marker.points.size();
00423         plane_marker.points.resize(last + 1);
00424         plane_marker.colors.resize(last + 1);
00425         double nx, ny, nz;
00426         gridToWorld(x, y, z, nx, ny, nz);
00427         Eigen::Vector3d vec(nx, ny, nz);
00428         plane_marker.points[last].x = vec.x();
00429         plane_marker.points[last].y = vec.y();
00430         plane_marker.points[last].z = vec.z();
00431         if (dist < 0.0)
00432         {
00433           plane_marker.colors[last].r = fmax(fmin(0.1 / fabs(dist), 1.0), 0.0);
00434           plane_marker.colors[last].g = fmax(fmin(0.05 / fabs(dist), 1.0), 0.0);
00435           plane_marker.colors[last].b = fmax(fmin(0.01 / fabs(dist), 1.0), 0.0);
00436         }
00437         else
00438         {
00439           plane_marker.colors[last].b = fmax(fmin(0.1 / (dist + 0.001), 1.0), 0.0);
00440           plane_marker.colors[last].g = fmax(fmin(0.05 / (dist + 0.001), 1.0), 0.0);
00441           plane_marker.colors[last].r = fmax(fmin(0.01 / (dist + 0.001), 1.0), 0.0);
00442         }
00443       }
00444     }
00445   }
00446 }
00447 
00448 void distance_field::DistanceField::setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::Point& point,
00449                                              std_msgs::ColorRGBA& color, double max_distance) const
00450 {
00451   double wx, wy, wz;
00452   gridToWorld(xCell, yCell, zCell, wx, wy, wz);
00453 
00454   point.x = wx;
00455   point.y = wy;
00456   point.z = wz;
00457 
00458   color.r = 1.0;
00459   color.g = dist / max_distance;  // dist/max_distance * 0.5;
00460   color.b = dist / max_distance;  // dist/max_distance * 0.1;
00461 }
00462 
00463 void distance_field::DistanceField::getProjectionPlanes(const std::string& frame_id, const ros::Time& stamp,
00464                                                         double max_dist, visualization_msgs::Marker& marker) const
00465 {
00466   int maxXCell = getXNumCells();
00467   int maxYCell = getYNumCells();
00468   int maxZCell = getZNumCells();
00469 
00470   double* x_projection = new double[maxYCell * maxZCell];
00471   double* y_projection = new double[maxZCell * maxXCell];
00472   double* z_projection = new double[maxXCell * maxYCell];
00473   double initial_val = sqrt(INT_MAX);
00474 
00475   // Initialize
00476   for (int y = 0; y < maxYCell; y++)
00477     for (int x = 0; x < maxXCell; x++)
00478       z_projection[x + y * maxXCell] = initial_val;
00479 
00480   for (int z = 0; z < maxZCell; z++)
00481     for (int y = 0; y < maxYCell; y++)
00482       x_projection[y + z * maxYCell] = initial_val;
00483 
00484   for (int z = 0; z < maxZCell; z++)
00485     for (int x = 0; x < maxXCell; x++)
00486       y_projection[x + z * maxXCell] = initial_val;
00487 
00488   // Calculate projections
00489   for (int z = 0; z < maxZCell; z++)
00490   {
00491     for (int y = 0; y < maxYCell; y++)
00492     {
00493       for (int x = 0; x < maxXCell; x++)
00494       {
00495         double dist = getDistance(x, y, z);
00496         z_projection[x + y * maxXCell] = std::min(dist, z_projection[x + y * maxXCell]);
00497         x_projection[y + z * maxYCell] = std::min(dist, x_projection[y + z * maxYCell]);
00498         y_projection[x + z * maxXCell] = std::min(dist, y_projection[x + z * maxXCell]);
00499       }
00500     }
00501   }
00502 
00503   // Make markers
00504   marker.points.clear();
00505   marker.header.frame_id = frame_id;
00506   marker.header.stamp = stamp;
00507   marker.ns = "distance_field_projection_plane";
00508   marker.id = 1;
00509   marker.type = visualization_msgs::Marker::CUBE_LIST;
00510   marker.action = visualization_msgs::Marker::MODIFY;
00511   marker.scale.x = getResolution();
00512   marker.scale.y = getResolution();
00513   marker.scale.z = getResolution();
00514   marker.color.a = 1.0;
00515   // marker.lifetime = ros::Duration(30.0);
00516 
00517   int x, y, z;
00518   int index = 0;
00519   marker.points.resize(maxXCell * maxYCell + maxYCell * maxZCell + maxZCell * maxXCell);
00520   marker.colors.resize(maxXCell * maxYCell + maxYCell * maxZCell + maxZCell * maxXCell);
00521 
00522   z = 0;
00523   for (y = 0; y < maxYCell; y++)
00524   {
00525     for (x = 0; x < maxXCell; x++)
00526     {
00527       double dist = z_projection[x + y * maxXCell];
00528       setPoint(x, y, z, dist, marker.points[index], marker.colors[index], max_dist);
00529       index++;
00530     }
00531   }
00532 
00533   x = 0;
00534   for (z = 0; z < maxZCell; z++)
00535   {
00536     for (y = 0; y < maxYCell; y++)
00537     {
00538       double dist = x_projection[y + z * maxYCell];
00539       setPoint(x, y, z, dist, marker.points[index], marker.colors[index], max_dist);
00540       index++;
00541     }
00542   }
00543 
00544   y = 0;
00545   for (z = 0; z < maxZCell; z++)
00546   {
00547     for (x = 0; x < maxXCell; x++)
00548     {
00549       double dist = y_projection[x + z * maxXCell];
00550       setPoint(x, y, z, dist, marker.points[index], marker.colors[index], max_dist);
00551       index++;
00552     }
00553   }
00554 
00555   if (x_projection)
00556     delete[] x_projection;
00557   if (y_projection)
00558     delete[] y_projection;
00559   if (z_projection)
00560     delete[] z_projection;
00561 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 19 2019 19:23:49