polygon.h
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 #ifndef JSK_RECOGNITION_UTILS_GEO_POLYGON_H_
00037 #define JSK_RECOGNITION_UTILS_GEO_POLYGON_H_
00038 
00039 #include <Eigen/Geometry>
00040 #include <boost/shared_ptr.hpp>
00041 #include <pcl/point_cloud.h>
00042 #include <pcl/point_types.h>
00043 #include <boost/random.hpp>
00044 #include <geometry_msgs/Polygon.h>
00045 #include <jsk_recognition_msgs/PolygonArray.h>
00046 
00047 #include "jsk_recognition_utils/geo/plane.h"
00048 #include "jsk_recognition_utils/geo/segment.h"
00049 #include "jsk_recognition_utils/sensor_model/camera_depth_sensor.h"
00050 #include "jsk_recognition_utils/pcl_conversion_util.h"
00051 #include "jsk_recognition_utils/random_util.h"
00052 
00053 namespace jsk_recognition_utils
00054 {
00055   class Polygon: public Plane
00056   {
00057   public:
00058     typedef boost::shared_ptr<Polygon> Ptr;
00059     typedef boost::tuple<Ptr, Ptr> PtrPair;
00060     Polygon(const Vertices& vertices);
00061     Polygon(const Vertices& vertices,
00062             const std::vector<float>& coefficients);
00063     virtual ~Polygon();
00064     virtual std::vector<Polygon::Ptr> decomposeToTriangles();
00065     virtual void clearTriangleDecompositionCache() {
00066       cached_triangles_.clear();
00067     }
00068     
00069     virtual Eigen::Vector3f getNormalFromVertices();
00070     virtual bool isTriangle();
00071     Eigen::Vector3f randomSampleLocalPoint(boost::mt19937& random_generator);
00072     virtual void getLocalMinMax(double& min_x, double& min_y,
00073                                 double& max_x, double& max_y);
00074     template <class PointT>
00075     typename pcl::PointCloud<PointT>::Ptr samplePoints(double grid_size)
00076     {
00077       typename pcl::PointCloud<PointT>::Ptr
00078         ret (new pcl::PointCloud<PointT>);
00079       double min_x, min_y, max_x, max_y;
00080       getLocalMinMax(min_x, min_y, max_x, max_y);
00081       // ROS_INFO("min_x: %f", min_x);
00082       // ROS_INFO("min_y: %f", min_y);
00083       // ROS_INFO("max_x: %f", max_x);
00084       // ROS_INFO("max_y: %f", max_y);
00085       // Decompose into triangle first for optimization
00086       std::vector<Polygon::Ptr> triangles = decomposeToTriangles();
00087 
00088       for (double x = min_x; x < max_x; x += grid_size) {
00089         for (double y = min_y; y < max_y; y += grid_size) {
00090           Eigen::Vector3f candidate(x, y, 0);
00091           Eigen::Vector3f candidate_global = coordinates() * candidate;
00092           // check candidate is inside of the polygon or not
00093           bool insidep = false;
00094           for (size_t i = 0; i < triangles.size(); i++) {
00095             if (triangles[i]->isInside(candidate_global)) {
00096               insidep = true;
00097               break;
00098             }
00099           }
00100           if (insidep) {
00101             PointT p;
00102             p.x = candidate_global[0];
00103             p.y = candidate_global[1];
00104             p.z = candidate_global[2];
00105             p.normal_x = normal_[0];
00106             p.normal_y = normal_[1];
00107             p.normal_z = normal_[2];
00108             ret->points.push_back(p);
00109           }
00110         }
00111       }
00112       return ret;
00113     }
00114 
00119     std::vector<Segment::Ptr> edges() const;
00120     
00125     double distance(const Eigen::Vector3f& point);
00126 
00132     double distance(const Eigen::Vector3f& point,
00133                     Eigen::Vector3f& nearest_point);
00134 
00154     virtual Eigen::Vector3f nearestPoint(const Eigen::Vector3f& p,
00155                                          double& distance);
00156     virtual size_t getNumVertices();
00157     virtual size_t getFarestPointIndex(const Eigen::Vector3f& O);
00158     virtual Eigen::Vector3f directionAtPoint(size_t i);
00159     virtual Eigen::Vector3f getVertex(size_t i);
00160     virtual PointIndexPair getNeighborIndex(size_t index);
00161     virtual Vertices getVertices() { return vertices_; };
00162     virtual double area();
00163     virtual bool isPossibleToRemoveTriangleAtIndex(
00164       size_t index,
00165       const Eigen::Vector3f& direction);
00166     virtual PtrPair separatePolygon(size_t index);
00172     virtual bool isInside(const Eigen::Vector3f& p);
00173     size_t previousIndex(size_t i);
00174     size_t nextIndex(size_t i);
00175     
00176     static Polygon fromROSMsg(const geometry_msgs::Polygon& polygon);
00177     static Polygon::Ptr fromROSMsgPtr(const geometry_msgs::Polygon& polygon);
00178     static Polygon createPolygonWithSkip(const Vertices& vertices);
00179 
00186     static std::vector<Polygon::Ptr> fromROSMsg(const jsk_recognition_msgs::PolygonArray& msg,
00187                                                 const Eigen::Affine3f& offset = Eigen::Affine3f::Identity());
00188     
00194     virtual void transformBy(const Eigen::Affine3d& transform);
00195 
00201     virtual void transformBy(const Eigen::Affine3f& transform);
00202     
00209     virtual bool maskImage(const jsk_recognition_utils::CameraDepthSensor& model,
00210                            cv::Mat& image) const;
00211     
00216     virtual void drawLineToImage(const jsk_recognition_utils::CameraDepthSensor& model,
00217                                  cv::Mat& image,
00218                                  const cv::Scalar& color,
00219                                  const int line_width = 1) const;
00220     virtual bool isConvex();
00221     virtual Eigen::Vector3f centroid();
00222     template<class PointT> void boundariesToPointCloud(
00223       pcl::PointCloud<PointT>& output) {
00224       output.points.resize(vertices_.size());
00225       for (size_t i = 0; i < vertices_.size(); i++) {
00226         Eigen::Vector3f v = vertices_[i];
00227         PointT p;
00228         p.x = v[0]; p.y = v[1]; p.z = v[2];
00229         output.points[i] = p;
00230       }
00231       output.height = 1;
00232       output.width = output.points.size();
00233     }
00234     
00235   protected:
00236     Vertices vertices_;
00237     std::vector<Polygon::Ptr> cached_triangles_;
00238   private:
00239     
00240   };
00241 }
00242 
00243 #endif


jsk_recognition_utils
Author(s):
autogenerated on Tue Jul 2 2019 19:40:37