grid_plane.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #define BOOST_PARAMETER_MAX_ARITY 7
00037 #include "jsk_recognition_utils/geo/grid_plane.h"
00038 #include "jsk_recognition_utils/geo_util.h"
00039 #include <boost/tuple/tuple_comparison.hpp>
00040 
00041 namespace jsk_recognition_utils
00042 {
00043   GridPlane::GridPlane(ConvexPolygon::Ptr plane, const double resolution):
00044     convex_(plane), resolution_(resolution)
00045   {
00046 
00047   }
00048 
00049   GridPlane::~GridPlane()
00050   {
00051 
00052   }
00053 
00054   GridPlane::IndexPair GridPlane::projectLocalPointAsIndexPair(
00055     const Eigen::Vector3f& p)
00056   {
00057     double offset_x = p[0] + 0.5 * resolution_;
00058     double offset_y = p[1] + 0.5 * resolution_;
00059     // return boost::make_tuple<int, int>(std::floor(offset_x / resolution_),
00060     //                                    std::floor(offset_y / resolution_));
00061     return boost::make_tuple<int, int>(boost::math::round(p[0] / resolution_),
00062                                        boost::math::round(p[1] / resolution_));
00063   }
00064 
00065   void GridPlane::addIndexPair(IndexPair pair)
00066   {
00067     cells_.insert(pair);
00068   }
00069 
00070   GridPlane::Ptr GridPlane::dilate(int num)
00071   {
00072     GridPlane::Ptr ret (new GridPlane(convex_, resolution_));
00073     for (std::set<IndexPair>::iterator it = cells_.begin();
00074          it != cells_.end();
00075          ++it) {
00076       IndexPair the_index = *it;
00077       for (int xi = - num; xi <= num; xi++) {
00078         for (int yi = - num; yi <= num; yi++) {
00079           if (abs(xi) + abs(yi) <= num) {
00080             IndexPair new_pair = boost::make_tuple<int, int>(
00081               the_index.get<0>() + xi,
00082               the_index.get<1>() + yi);
00083             ret->cells_.insert(new_pair);
00084           }
00085         }
00086       }
00087     }
00088     return ret;
00089   }
00090 
00091   GridPlane::Ptr GridPlane::erode(int num)
00092   {
00093     GridPlane::Ptr ret (new GridPlane(convex_, resolution_));
00094     for (std::set<IndexPair>::iterator it = cells_.begin();
00095          it != cells_.end();
00096          ++it) {
00097       IndexPair the_index = *it;
00098       bool should_removed = false;
00099       for (int xi = - num; xi <= num; xi++) {
00100         for (int yi = - num; yi <= num; yi++) {
00101           if (abs(xi) + abs(yi) <= num) {
00102             IndexPair check_pair = boost::make_tuple<int, int>(
00103               the_index.get<0>() + xi,
00104               the_index.get<1>() + yi);
00105             if (!isOccupied(check_pair)) {
00106               should_removed = true;
00107             }
00108           }
00109         }
00110       }
00111       if (!should_removed) {
00112         ret->cells_.insert(the_index);
00113       }
00114     }
00115     return ret;
00116   }
00117 
00118   bool GridPlane::isOccupied(const IndexPair& pair)
00119   {
00120     bool result = cells_.find(pair) != cells_.end();
00121     // Verbosing for debug
00122     // JSK_ROS_INFO("Checking index pair (%d, %d)", pair.get<0>(), pair.get<1>());
00123     // JSK_ROS_INFO("Result: %d", result);
00124     // JSK_ROS_INFO("cells are:");
00125     // for (IndexPairSet::iterator it = cells_.begin();
00126     //      it != cells_.end();
00127     //      ++it) {
00128     //   JSK_ROS_INFO("  (%d, %d)", it->get<0>(), it->get<1>());
00129     // }
00130     return result;
00131   }
00132 
00133   bool GridPlane::isOccupied(const Eigen::Vector3f& p)
00134   {
00135     IndexPair pair = projectLocalPointAsIndexPair(p);
00136     return isOccupied(pair);
00137   }
00138 
00139   bool GridPlane::isOccupiedGlobal(const Eigen::Vector3f& p)
00140   {
00141     return isOccupied(convex_->coordinates().inverse() * p);
00142   }
00143 
00144   GridPlane::Ptr GridPlane::clone()
00145   {
00146     GridPlane::Ptr ret (new GridPlane(convex_, resolution_));
00147     ret->cells_ = cells_;
00148     return ret;
00149   }
00150   
00151   Eigen::Vector3f GridPlane::unprojectIndexPairAsLocalPoint(
00152     const IndexPair& pair)
00153   {
00154     return Eigen::Vector3f(pair.get<0>() * resolution_,
00155                            pair.get<1>() * resolution_,
00156                            0);
00157   }
00158 
00159   Eigen::Vector3f GridPlane::unprojectIndexPairAsGlobalPoint(
00160     const IndexPair& pair)
00161   {
00162     Eigen::Vector3f local_point = unprojectIndexPairAsLocalPoint(pair);
00163     return convex_->coordinates() * local_point;
00164   }
00165 
00166   void GridPlane::fillCellsFromCube(Cube& cube)
00167   {
00168     ConvexPolygon::Ptr intersect_polygon = cube.intersectConvexPolygon(*convex_);
00169     // 1. transform vertices into local coordinates
00170     // 2. compute min-max
00171     // 3. compute candidates
00172     // 4. filter candidates
00173 
00174     // 1. transform vertices into local coordinates
00175     Vertices local_vertices;
00176     Vertices global_vertices = intersect_polygon->getVertices();
00177     Eigen::Affine3f inv_coords = convex_->coordinates().inverse();
00178     for (size_t i = 0; i < global_vertices.size(); i++) {
00179       local_vertices.push_back(inv_coords * global_vertices[i]);
00180     }
00181     
00182     // 2. compute min-max
00183     double min_x = DBL_MAX;
00184     double min_y = DBL_MAX;
00185     double max_x = - DBL_MAX;
00186     double max_y = - DBL_MAX;
00187     for (size_t i = 0; i < local_vertices.size(); i++) {
00188       min_x = ::fmin(min_x, local_vertices[i][0]);
00189       min_y = ::fmin(min_y, local_vertices[i][1]);
00190       max_x = ::fmax(max_x, local_vertices[i][0]);
00191       max_y = ::fmax(max_y, local_vertices[i][1]);
00192     }
00193     // JSK_ROS_INFO("x: [%f~%f]", min_x, max_x);
00194     // JSK_ROS_INFO("y: [%f~%f]", min_y, max_y);
00195     // 3. compute candidates
00196     std::vector<Polygon::Ptr> triangles
00197       = intersect_polygon->decomposeToTriangles();
00198     for (double x = min_x; x <= max_x; x += resolution_) {
00199       for (double y = min_y; y <= max_y; y += resolution_) {
00200         Eigen::Vector3f local_p(x, y, 0);
00201         Eigen::Vector3f p = convex_->coordinates() * local_p;
00202         // 4. filter candidates
00203         bool insidep = false;
00204         for (size_t i = 0; i < triangles.size(); i++) {
00205           if (triangles[i]->isInside(p)) {
00206             insidep = true;
00207             break;
00208           }
00209         }
00210         if (insidep) {
00211           IndexPair pair = projectLocalPointAsIndexPair(local_p);
00212           addIndexPair(pair);
00213         }
00214       }
00215     }
00216   }
00217 
00218   size_t GridPlane::fillCellsFromPointCloud(
00219     pcl::PointCloud<pcl::PointNormal>::Ptr& cloud,
00220     double distance_threshold,
00221     std::set<int>& non_plane_indices)
00222   {
00223     return fillCellsFromPointCloud(
00224       cloud, distance_threshold, M_PI / 2.0, non_plane_indices);
00225   }
00226   
00227   size_t GridPlane::fillCellsFromPointCloud(
00228     pcl::PointCloud<pcl::PointNormal>::Ptr& cloud,
00229     double distance_threshold,
00230     double normal_threshold,
00231     std::set<int>& non_plane_indices)
00232   {
00233     Eigen::Affine3f local_coordinates = convex_->coordinates();
00234     Eigen::Affine3f inv_local_coordinates = local_coordinates.inverse();
00235     //std::vector<Polygon::Ptr> triangles = convex_->decomposeToTriangles();
00236     
00237     pcl::ExtractPolygonalPrismData<pcl::PointNormal> prism_extract;
00238     pcl::PointCloud<pcl::PointNormal>::Ptr
00239       hull_cloud (new pcl::PointCloud<pcl::PointNormal>);
00240     pcl::PointCloud<pcl::PointNormal>::Ptr
00241       hull_output (new pcl::PointCloud<pcl::PointNormal>);
00242     pcl::PointCloud<pcl::PointNormal>::Ptr
00243       rehull_cloud (new pcl::PointCloud<pcl::PointNormal>);
00244     convex_->boundariesToPointCloud<pcl::PointNormal>(*hull_cloud);
00245     // pcl::ConvexHull<pcl::PointNormal> chull;
00246     // chull.setDimension(2);
00247     // chull.setInputCloud (hull_cloud);
00248     // chull.reconstruct(*hull_output);
00249     
00250     // it's important to make it sure to close the loop of
00251     // convex hull
00252     hull_cloud->points.push_back(hull_cloud->points[0]);
00253     
00254     prism_extract.setInputCloud(cloud);
00255     prism_extract.setHeightLimits(-distance_threshold, distance_threshold);
00256     prism_extract.setInputPlanarHull(hull_cloud);
00257     //prism_extract.setInputPlanarHull(hull_output);
00258     // output_indices is set of indices which are on plane
00259     pcl::PointIndices output_indices;
00260     prism_extract.segment(output_indices);
00261     std::set<int> output_set(output_indices.indices.begin(),
00262                              output_indices.indices.end());
00263     Eigen::Vector3f n = convex_->getNormal();
00264     for (size_t i = 0; i < cloud->points.size(); i++) {
00265       if (output_set.find(i) != output_set.end()) {
00266         // check normal
00267         pcl::PointNormal p = cloud->points[i];
00268         Eigen::Vector3f n_p = p.getNormalVector3fMap();
00269         if (std::abs(n.dot(n_p)) > cos(normal_threshold)) {
00270           non_plane_indices.insert(i);
00271         }
00272       }
00273     }
00274 
00275     
00276     for (size_t i = 0; i < output_indices.indices.size(); i++) {
00277       //for (size_t i = 0; i < cloud->points.size(); i++) {
00278       int point_index = output_indices.indices[i];
00279       pcl::PointNormal p = cloud->points[point_index];
00280       Eigen::Vector3f ep = p.getVector3fMap();
00281       Eigen::Vector3f local_ep = inv_local_coordinates * ep;
00282       IndexPair pair = projectLocalPointAsIndexPair(local_ep);
00283       addIndexPair(pair);
00284     }
00285     return output_indices.indices.size();
00286   }
00287   
00288   size_t GridPlane::fillCellsFromPointCloud(
00289     pcl::PointCloud<pcl::PointNormal>::Ptr& cloud,
00290     double distance_threshold)
00291   {
00292     std::set<int> dummy;
00293     return fillCellsFromPointCloud(cloud, distance_threshold, dummy);
00294   }
00295   
00296   jsk_recognition_msgs::SimpleOccupancyGrid GridPlane::toROSMsg()
00297   {
00298     jsk_recognition_msgs::SimpleOccupancyGrid ros_msg;
00299     std::vector<float> coeff;
00300     convex_->toCoefficients(coeff);
00301     //JSK_ROS_INFO("coef: [%f, %f, %f, %f]", coeff[0], coeff[1], coeff[2], coeff[3]);
00302     ros_msg.coefficients[0] = coeff[0];
00303     ros_msg.coefficients[1] = coeff[1];
00304     ros_msg.coefficients[2] = coeff[2];
00305     ros_msg.coefficients[3] = coeff[3];
00306     ros_msg.resolution = resolution_;
00307     for (std::set<IndexPair>::iterator it = cells_.begin();
00308          it != cells_.end();
00309          ++it) {
00310       IndexPair pair = *it;
00311       Eigen::Vector3f c = unprojectIndexPairAsLocalPoint(pair);
00312       geometry_msgs::Point p;
00313       pointFromVectorToXYZ<Eigen::Vector3f, geometry_msgs::Point>(
00314         c, p);
00315       ros_msg.cells.push_back(p);
00316     }
00317     return ros_msg;
00318   }
00319 
00320   GridPlane GridPlane::fromROSMsg(
00321     const jsk_recognition_msgs::SimpleOccupancyGrid& rosmsg,
00322     const Eigen::Affine3f& offset = Eigen::Affine3f::Identity())
00323   {
00324     boost::mutex::scoped_lock lock(global_chull_mutex);
00325     Plane plane = Plane(rosmsg.coefficients).transform(offset);
00326     // JSK_ROS_INFO("[GridPlane::fromROSMsg] c: [%f, %f, %f, %f]",
00327     //          rosmsg.coefficients[0],
00328     //          rosmsg.coefficients[1],
00329     //          rosmsg.coefficients[2],
00330     //          rosmsg.coefficients[3]);
00331     // JSK_ROS_INFO("[GridPlane::fromROSMsg] transformed c: [%f, %f, %f, %f]",
00332     //          plane.toCoefficients()[0],
00333     //          plane.toCoefficients()[1],
00334     //          plane.toCoefficients()[2],
00335     //          plane.toCoefficients()[3]);
00336     Eigen::Affine3f plane_coords = plane.coordinates();
00337     Eigen::Vector3f plane_origin(plane_coords.translation());
00338     // JSK_ROS_INFO_EIGEN_VECTOR3("[GridPlane::fromROSMsg] plane_origin",
00339     //                        plane_origin);
00340     pcl::PointCloud<pcl::PointNormal>::Ptr
00341       vertices (new pcl::PointCloud<pcl::PointNormal>);
00342     for (size_t i = 0; i < rosmsg.cells.size(); i++) {
00343       Eigen::Vector3f local_p(rosmsg.cells[i].x, rosmsg.cells[i].y, 0);
00344       Eigen::Vector3f global_p = plane.coordinates() * local_p;
00345       pcl::PointNormal p;
00346       p.x = global_p[0];
00347       p.y = global_p[1];
00348       p.z = global_p[2];
00349       // JSK_ROS_INFO("[%f, %f, %f] => [%f, %f, %f]",
00350       //          local_p[0], local_p[1], local_p[2],
00351       //          global_p[0], global_p[1], global_p[2]);
00352       vertices->points.push_back(p);
00353     }
00354     pcl::ConvexHull<pcl::PointNormal> chull;
00355     //chull.setDimension(2);
00356     chull.setInputCloud (vertices);
00357     pcl::PointCloud<pcl::PointNormal>::Ptr
00358       convex_vertices_cloud (new pcl::PointCloud<pcl::PointNormal>);
00359     chull.reconstruct (*convex_vertices_cloud);
00360 
00361     Vertices convex_vertices
00362       = pointCloudToVertices<pcl::PointNormal>(*convex_vertices_cloud);
00363     ConvexPolygon::Ptr convex(new ConvexPolygon(convex_vertices));
00364     // Check orientation
00365     if (!convex->isSameDirection(plane)) {
00366       // JSK_ROS_INFO("[GridPlane::fromROSMsg] flip convex");
00367       //convex = boost::make_shared<ConvexPolygon>(convex->flipConvex());
00368       Vertices reversed_convex_vertices;
00369       std::reverse_copy(convex_vertices.begin(), convex_vertices.end(),
00370                         std::back_inserter(reversed_convex_vertices));
00371       convex.reset(new ConvexPolygon(reversed_convex_vertices));
00372     }
00373     Eigen::Vector3f convex_origin(convex->coordinates().translation());
00374     Eigen::Vector3f convex_normal = convex->getNormal();
00375     // JSK_ROS_INFO_EIGEN_VECTOR3("[GridPlane::fromROSMsg] convex_origin",
00376     //                        convex_origin);
00377     // JSK_ROS_INFO_EIGEN_VECTOR3("[GridPlane::fromROSMsg] convex_normal",
00378     //                        convex_normal);
00379     GridPlane ret(convex, rosmsg.resolution);
00380     //JSK_ROS_INFO("resolution: %f", ret.resolution_);
00381     ret.fillCellsFromPointCloud(vertices, 1000.0);
00382     // JSK_ROS_INFO("cell size: %lu", ret.cells_.size());
00383     // JSK_ROS_INFO("original cell size: %lu", rosmsg.cells.size());
00384     return ret;
00385   }
00386 
00387 }


jsk_recognition_utils
Author(s):
autogenerated on Wed Sep 16 2015 04:36:01