polygon.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 
00038 #include "jsk_recognition_utils/geo/polygon.h"
00039 #include "jsk_recognition_utils/geo_util.h"
00040 #include <jsk_topic_tools/log_utils.h>
00041 #include "jsk_recognition_utils/pcl/ear_clipping_patched.h"
00042 #include "jsk_recognition_utils/sensor_model_utils.h"
00043 
00044 namespace jsk_recognition_utils
00045 {
00046   Polygon Polygon::createPolygonWithSkip(const Vertices& vertices)
00047   {
00048     const double thr = 0.01;
00049     Polygon not_skipped_polygon(vertices);
00050     Vertices skipped_vertices;
00051     for (size_t i = 0; i < vertices.size(); i++) {
00052       size_t next_i = not_skipped_polygon.nextIndex(i);
00053       Eigen::Vector3f v0 = vertices[i];
00054       Eigen::Vector3f v1 = vertices[next_i];
00055       if ((v1 - v0).norm() > thr) {
00056         skipped_vertices.push_back(vertices[i]);
00057       }
00058     }
00059     return Polygon(skipped_vertices);
00060   }
00061 
00062   Eigen::Vector3f Polygon::centroid()
00063   {
00064     Eigen::Vector3f c(0, 0, 0);
00065     if (vertices_.size() == 0) {
00066       return c;
00067     }
00068     else {
00069       for (size_t i = 0; i < vertices_.size(); i++) {
00070         c = c + vertices_[i];
00071       }
00072       return c / vertices_.size();
00073     }
00074   }
00075 
00076   std::vector<Plane::Ptr> convertToPlanes(
00077     std::vector<pcl::ModelCoefficients::Ptr> coefficients)
00078   {
00079     std::vector<Plane::Ptr> ret;
00080     for (size_t i = 0; i < coefficients.size(); i++) {
00081       ret.push_back(Plane::Ptr (new Plane(coefficients[i]->values)));
00082     }
00083     return ret;
00084   }
00085   
00086   
00087   Polygon::Polygon(const Vertices& vertices):
00088     Plane((vertices[1] - vertices[0]).cross(vertices[2] - vertices[0]).normalized(), vertices[0]),
00089     vertices_(vertices)
00090   {
00091     
00092   }
00093 
00094   Polygon::Polygon(const Vertices& vertices,
00095                    const std::vector<float>& coefficients):
00096     Plane(coefficients), vertices_(vertices)
00097   {
00098     
00099   }
00100   
00101   Polygon::~Polygon()
00102   {
00103 
00104   }
00105 
00106   size_t Polygon::getFarestPointIndex(const Eigen::Vector3f& O)
00107   {
00108     double max_distance = - DBL_MAX;
00109     size_t max_index = 0;
00110     for (size_t i = 0; i < vertices_.size(); i++) {
00111       Eigen::Vector3f v = vertices_[i];
00112       double d = (O - v).norm();
00113       if (max_distance < d) {
00114         max_distance = d;
00115         max_index = i;
00116       }
00117     }
00118     return max_index;
00119   }
00120 
00121   PointIndexPair Polygon::getNeighborIndex(size_t index)
00122   {
00123     return boost::make_tuple<size_t, size_t>(
00124       previousIndex(index), nextIndex(index));
00125   }
00126 
00127   double Polygon::area()
00128   {
00129     if (isTriangle()) {
00130       return (vertices_[1] - vertices_[0]).cross(vertices_[2] - vertices_[0]).norm() / 2.0;
00131     }
00132     else {
00133       std::vector<Polygon::Ptr> triangles = decomposeToTriangles();
00134       double sum = 0;
00135       for (size_t i = 0; i < triangles.size(); i++) {
00136         sum += triangles[i]->area();
00137       }
00138       return sum;
00139     }
00140   }
00141   
00142   Eigen::Vector3f Polygon::directionAtPoint(size_t i)
00143   {
00144     Eigen::Vector3f O = vertices_[i];
00145     Eigen::Vector3f A = vertices_[previousIndex(i)];
00146     Eigen::Vector3f B = vertices_[nextIndex(i)];
00147     Eigen::Vector3f OA = A - O;
00148     Eigen::Vector3f OB = B - O;
00149     Eigen::Vector3f n = (OA.normalized()).cross(OB.normalized());
00150     if (n.norm() == 0) {
00151       // ROS_ERROR("normal is 0");
00152       // ROS_ERROR("O: [%f, %f, %f]", O[0], O[1], O[2]);
00153       // ROS_ERROR("A: [%f, %f, %f]", A[0], A[1], A[2]);
00154       // ROS_ERROR("B: [%f, %f, %f]", B[0], B[1], B[2]);
00155       // ROS_ERROR("OA: [%f, %f, %f]", OA[0], OA[1], OA[2]);
00156       // ROS_ERROR("OB: [%f, %f, %f]", OB[0], OB[1], OB[2]);
00157       //exit(1);
00158     }
00159     return n.normalized();
00160   }
00161   
00162   bool Polygon::isTriangle() {
00163     return vertices_.size() == 3;
00164   }
00165 
00166   void Polygon::getLocalMinMax(double& min_x, double& min_y,
00167                                double& max_x, double& max_y)
00168   {
00169     min_x = DBL_MAX;
00170     min_y = DBL_MAX;
00171     max_x = - DBL_MAX;
00172     max_y = - DBL_MAX;
00173     
00174     Eigen::Affine3f inv_coords = coordinates().inverse();
00175     for (size_t i = 0; i < vertices_.size(); i++) {
00176       // Convert vertices into local coordinates
00177       Eigen::Vector3f local_point = inv_coords * vertices_[i];
00178       min_x = ::fmin(local_point[0], min_x);
00179       min_y = ::fmin(local_point[1], min_y);
00180       max_x = ::fmax(local_point[0], max_x);
00181       max_y = ::fmax(local_point[1], max_y);
00182     }
00183   }
00184   
00185   Eigen::Vector3f Polygon::randomSampleLocalPoint(boost::mt19937& random_generator)
00186   {
00187     // Compute min/max point
00188     double min_x, min_y, max_x, max_y;
00189     getLocalMinMax(min_x, min_y, max_x, max_y);
00190     std::vector<Polygon::Ptr> triangles = decomposeToTriangles();
00191     while (true) {
00192       double x = randomUniform(min_x, max_x, random_generator);
00193       double y = randomUniform(min_y, max_y, random_generator);
00194       Eigen::Vector3f local_v = Eigen::Vector3f(x, y, 0);
00195       Eigen::Vector3f v = coordinates() * local_v;
00196       // ROS_INFO_THROTTLE(1.0, "x: %f -- %f", min_x, max_x);
00197       // ROS_INFO_THROTTLE(1.0, "y: %f -- %f", min_y, max_y);
00198       // ROS_INFO_THROTTLE(1.0, "sampled point: [%f, %f]", x, y);
00199       // for (size_t i = 0; i < vertices_.size(); i++) {
00200       //   Eigen::Vector3f v = coordinates().inverse() * vertices_[i];
00201       //   ROS_INFO("v: [%f, %f, %f]", v[0], v[1], v[2]);
00202       // }
00203       if (isInside(v)) {
00204         return local_v;
00205       }
00206       else {
00207         // ROS_INFO_THROTTLE(1.0, "outside");
00208       }
00209     }
00210   }
00211 
00212   std::vector<Segment::Ptr> Polygon::edges() const
00213   {
00214     std::vector<Segment::Ptr> ret;
00215     ret.reserve(vertices_.size());
00216     for (size_t i = 0; i < vertices_.size() - 1; i++) {
00217       // edge between i and i+1
00218       ret.push_back(Segment::Ptr(new Segment(vertices_[i], vertices_[i+1])));
00219     }
00220     // edge between [-1] and [0]
00221     ret.push_back(Segment::Ptr(new Segment(vertices_[vertices_.size() - 1], vertices_[0])));
00222     return ret;
00223   }
00224   
00225   double Polygon::distance(const Eigen::Vector3f& point)
00226   {
00227     Eigen::Vector3f nearest_point;
00228     return Polygon::distance(point, nearest_point);
00229   }
00230 
00231   double Polygon::distance(const Eigen::Vector3f& point,
00232                            Eigen::Vector3f& nearest_point)
00233   {
00234     double distance;
00235     nearest_point = Polygon::nearestPoint(point, distance);
00236     return distance;
00237   }
00238 
00239   Eigen::Vector3f Polygon::nearestPoint(const Eigen::Vector3f& p,
00240                                         double& distance)
00241   {
00242     Eigen::Vector3f projected_p;
00243     Plane::project(p, projected_p);
00244     if (isInside(projected_p)) {
00245       distance = (p - projected_p).norm();
00246       return projected_p;
00247     }
00248     else {
00249       std::vector<Segment::Ptr> boundary_edges = edges();
00250       double min_distnace = DBL_MAX;
00251       Eigen::Vector3f nearest_point;
00252       // brute-force searching
00253       for (size_t i = 0; i < boundary_edges.size(); i++) {
00254         Segment::Ptr edge = boundary_edges[i];
00255         Eigen::Vector3f foot;
00256         double d = edge->distance(p, foot);
00257         if (min_distnace > d) {
00258           nearest_point = foot;
00259           min_distnace = d;
00260         }
00261       }
00262       distance = min_distnace;
00263       return nearest_point;
00264     }
00265   }
00266   
00267   size_t Polygon::getNumVertices() {
00268     return vertices_.size();
00269   }
00270   
00271   Eigen::Vector3f Polygon::getVertex(size_t i) {
00272     return vertices_[i];
00273   }
00274   
00275   Polygon::PtrPair Polygon::separatePolygon(size_t index)
00276   {
00277     PointIndexPair neighbor_index = getNeighborIndex(index);
00278     Vertices triangle_vertices;
00279     triangle_vertices.push_back(vertices_[index]);
00280     triangle_vertices.push_back(vertices_[neighbor_index.get<1>()]);
00281     triangle_vertices.push_back(vertices_[neighbor_index.get<0>()]);
00282     Polygon::Ptr triangle(new Polygon(triangle_vertices));
00283     Vertices rest_vertices;
00284     // do not add the points on the line
00285     for (size_t i = neighbor_index.get<1>(); i != index;) {
00286       // check the points on the line
00287       if (i == neighbor_index.get<1>()) {
00288         rest_vertices.push_back(vertices_[i]);
00289       }
00290       else {
00291         if (directionAtPoint(i).norm() != 0.0) {
00292           rest_vertices.push_back(vertices_[i]);
00293         }
00294         else {
00295           ROS_ERROR("removed: %lu", i);
00296         }
00297       }
00298       i = nextIndex(i);
00299     }
00300     Polygon::Ptr rest(new Polygon(rest_vertices));
00301     return boost::make_tuple<Polygon::Ptr, Polygon::Ptr>(
00302       triangle, rest);
00303   }
00304   
00305   bool Polygon::isPossibleToRemoveTriangleAtIndex(
00306     size_t index,
00307     const Eigen::Vector3f& direction)
00308   {
00309     Polygon::PtrPair candidate = separatePolygon(index);
00310     Polygon::Ptr triangle_candidate = candidate.get<0>();
00311     Polygon::Ptr rest_candidate = candidate.get<1>();
00312     // first check direction
00313     Eigen::Vector3f the_direction = directionAtPoint(index);
00314     //ROS_INFO("direction: [%f, %f, %f]", the_direction[0], the_direction[1], the_direction[2]);
00315     if (the_direction.norm() == 0.0) {
00316       ROS_ERROR("malformed polygon");
00317       exit(1);
00318     }
00319     if (direction.dot(the_direction) < 0) {
00320 #ifdef DEBUG_GEO_UTIL
00321       ROS_INFO("triangle is not same direction");
00322       ROS_INFO("direction: [%f, %f, %f]", direction[0], direction[1], direction[2]);
00323       ROS_INFO("the_direction: [%f, %f, %f]",
00324                the_direction[0],
00325                the_direction[1],
00326                the_direction[2]);
00327       for (size_t i = 0; i < vertices_.size(); i++) {
00328         Eigen::Vector3f v = directionAtPoint(i);
00329         ROS_INFO("the_direction[%lu]: [%f, %f, %f]",
00330                  i, v[0], v[1], v[2]);
00331       // other direction
00332       }
00333 #endif
00334       return false;
00335     }
00336     else {
00337       //return true;
00338       // second, check the triangle includes the rest of points or not
00339       for (size_t i = 0; i < rest_candidate->vertices_.size(); i++) {
00340         if (i == 0 || i == rest_candidate->vertices_.size() - 1) {
00341           continue;       // do not check the first and the last point
00342         }
00343         else {
00344           Eigen::Vector3f P = rest_candidate->getVertex(i);
00345           Eigen::Vector3f A = triangle_candidate->getVertex(0);
00346           Eigen::Vector3f B = triangle_candidate->getVertex(1);
00347           Eigen::Vector3f C = triangle_candidate->getVertex(2);
00348           Eigen::Vector3f CA = A - C;
00349           Eigen::Vector3f BC = C - B;
00350           Eigen::Vector3f AB = B - A;
00351           Eigen::Vector3f AP = P - A;
00352           Eigen::Vector3f BP = P - B;
00353           Eigen::Vector3f CP = P - C;
00354           Eigen::Vector3f Across = CA.normalized().cross(AP.normalized()).normalized();
00355           Eigen::Vector3f Bcross = AB.normalized().cross(BP.normalized()).normalized();
00356           Eigen::Vector3f Ccross = BC.normalized().cross(CP.normalized()).normalized();
00357 #ifdef DEBUG_GEO_UTIL
00358           ROS_INFO("P: [%f, %f, %f]", P[0], P[1], P[2]);
00359           ROS_INFO("A: [%f, %f, %f]", A[0], A[1], A[2]);
00360           ROS_INFO("B: [%f, %f, %f]", B[0], B[1], B[2]);
00361           ROS_INFO("C: [%f, %f, %f]", C[0], C[1], C[2]);
00362           ROS_INFO("Across: [%f, %f, %f]", Across[0], Across[1], Across[2]);
00363           ROS_INFO("Bcross: [%f, %f, %f]", Bcross[0], Bcross[1], Bcross[2]);
00364           ROS_INFO("Ccross: [%f, %f, %f]", Ccross[0], Ccross[1], Ccross[2]);
00365           ROS_INFO("Across-Bcross: %f", Across.dot(Bcross));
00366           ROS_INFO("Bcross-Ccross: %f", Bcross.dot(Ccross));
00367           ROS_INFO("Ccross-Across: %f", Ccross.dot(Across));
00368 #endif
00369           if (((Across.dot(Bcross) > 0 &&
00370                 Bcross.dot(Ccross) > 0 &&
00371                 Ccross.dot(Across) > 0) ||
00372                (Across.dot(Bcross) < 0 &&
00373                 Bcross.dot(Ccross) < 0 &&
00374                 Ccross.dot(Across) < 0))) {
00375             // ROS_ERROR("%lu -- %lu is inside", index, i);
00376             return false;
00377           }
00378           // ConvexPolygon convex_triangle(triangle_candidate->vertices_);
00379           // if (convex_triangle.isInside(v)) {
00380           //   //ROS_INFO("vertices is inside of the polygon");
00381           //   return false;
00382           // }
00383         }
00384       }
00385       return true;
00386     }
00387   }
00388 
00389   void Polygon::transformBy(const Eigen::Affine3d& transform)
00390   {
00391     Eigen::Affine3f float_affine;
00392     convertEigenAffine3(transform, float_affine);
00393     transformBy(float_affine);
00394   }
00395 
00396   void Polygon::transformBy(const Eigen::Affine3f& transform)
00397   {
00398     // 1. clear cached_triangles_
00399     // 2. transform vertices_
00400     // 3. update normal_ and d_
00401     // 3. update plane_coordinates_
00402     cached_triangles_.clear();
00403     for (size_t i = 0; i < vertices_.size(); i++) {
00404       vertices_[i] = transform * vertices_[i];
00405     }
00406     // compute normal and d
00407     normal_ = (vertices_[1] - vertices_[0]).cross(vertices_[2] - vertices_[0]).normalized();
00408     d_ = - normal_.dot(vertices_[0]) / normal_.norm();
00409     initializeCoordinates();
00410   }
00411   
00412   bool Polygon::maskImage(const jsk_recognition_utils::CameraDepthSensor& model,
00413                           cv::Mat& image) const
00414   {
00415     std::vector<cv::Point> projected_vertices
00416       = project3DPointstoPixel(model.getPinholeCameraModel(), vertices_);
00417     bool all_outside = true;
00418     // check some of vertices is inside of FoV
00419     for (size_t i = 0; i < projected_vertices.size(); i++) {
00420       if (model.isInside(projected_vertices[i])) {
00421         all_outside = false;
00422       }
00423     }
00424     image = model.image(CV_8UC1);
00425     // check all the v is positive
00426     for (size_t i = 0; i < vertices_.size(); i++) {
00427       if (vertices_[i][2] < 0) {
00428         return false;
00429       }
00430     }
00431     const cv::Point* element_points[1] = {&projected_vertices[0]};
00432     int number_of_points = (int)projected_vertices.size();
00433     // Is it should be cv::fillPoly?
00434     cv::fillPoly(image, 
00435                  element_points,
00436                  &number_of_points,
00437                  1,
00438                  cv::Scalar(255));
00439     return !all_outside;
00440   }
00441 
00442   void Polygon::drawLineToImage(const jsk_recognition_utils::CameraDepthSensor& model,
00443                                 cv::Mat& image,
00444                                 const cv::Scalar& color,
00445                                 const int line_width) const
00446   {
00447     std::vector<cv::Point> projected_vertices
00448       = project3DPointstoPixel(model.getPinholeCameraModel(), vertices_);
00449     
00450     for (size_t i = 0; i < projected_vertices.size() - 1; i++) {
00451       cv::Point from = projected_vertices[i];
00452       cv::Point to = projected_vertices[i+1];
00453       if (model.isInside(from) || model.isInside(to)) {
00454         cv::line(image, from, to, color, line_width);
00455       }
00456     }
00457     cv::Point from = projected_vertices[projected_vertices.size() - 1];
00458     cv::Point to = projected_vertices[0];
00459     if (model.isInside(from) || model.isInside(to)) {
00460       cv::line(image, from, to, color, line_width);
00461     }
00462   }
00463 
00464   bool Polygon::isConvex()
00465   {
00466 #ifdef DEBUG_GEO_UTIL
00467     for (size_t i = 0; i < getNumVertices(); i++) {
00468       Eigen::Vector3f n = directionAtPoint(i);
00469       ROS_INFO("n[%lu] [%f, %f, %f]", i, n[0], n[1], n[2]);
00470     }
00471 #endif
00472     Eigen::Vector3f n0 = directionAtPoint(0);
00473     for (size_t i = 1; i < getNumVertices(); i++) {
00474       Eigen::Vector3f n = directionAtPoint(i);
00475       if (n0.dot(n) < 0) {
00476         return false;
00477       }
00478     }
00479     return true;
00480   }
00481   
00482   std::vector<Polygon::Ptr> Polygon::decomposeToTriangles()
00483   {
00484     if (cached_triangles_.size() != 0) {
00485       return cached_triangles_;
00486     }
00487     std::vector<Polygon::Ptr> ret;
00488 
00489     // if this polygon is triangle, return immediately
00490     if (isTriangle()) {
00491       ret.push_back(Polygon::Ptr( new Polygon(*this)));
00492       return ret;
00493     }
00494 
00495     pcl::EarClippingPatched clip;
00496     // convert
00497     pcl::PolygonMesh::Ptr input_mesh (new pcl::PolygonMesh);
00498     pcl::PCLPointCloud2 mesh_cloud;
00499     pcl::PointCloud<pcl::PointXYZ> mesh_pcl_cloud;
00500     boundariesToPointCloud<pcl::PointXYZ>(mesh_pcl_cloud);
00501     std::vector<pcl::Vertices> mesh_vertices(1);
00502     for (size_t i = 0; i < vertices_.size(); i++) {
00503       mesh_vertices[0].vertices.push_back(i);
00504     }
00505     //mesh_vertices[0].vertices.push_back(0); // close
00506     mesh_pcl_cloud.height = 1;
00507     mesh_pcl_cloud.width = mesh_pcl_cloud.points.size();
00508     pcl::toPCLPointCloud2<pcl::PointXYZ>(mesh_pcl_cloud, mesh_cloud);
00509 
00510     input_mesh->polygons = mesh_vertices;
00511     input_mesh->cloud = mesh_cloud;
00512     clip.setInputMesh(input_mesh);
00513     pcl::PolygonMesh output;
00514     clip.process(output);
00515     assert(output.polygons.size() != 0);
00516     // convert to Polygon instances
00517     for (size_t i = 0; i < output.polygons.size(); i++) {
00518       pcl::Vertices output_polygon_vertices = output.polygons[i];
00519       Vertices vs(output_polygon_vertices.vertices.size());
00520       for (size_t j = 0; j < output_polygon_vertices.vertices.size(); j++) {
00521         pcl::PointXYZ p
00522           = mesh_pcl_cloud.points[output_polygon_vertices.vertices[j]];
00523         Eigen::Vector3f v;
00524         pointFromXYZToVector<pcl::PointXYZ, Eigen::Vector3f>(p, v);
00525         vs[j] = v;
00526       }
00527       ret.push_back(Polygon::Ptr(new Polygon(vs, toCoefficients())));
00528     }
00529     cached_triangles_ = ret;
00530     return ret;
00531   }
00532 
00533   Eigen::Vector3f Polygon::getNormalFromVertices()
00534   {
00535     if (vertices_.size() >= 3) {
00536       return (vertices_[1] - vertices_[0]).cross(vertices_[2] - vertices_[0]).normalized();
00537     }
00538     else {
00539       ROS_ERROR("the number of vertices is not enough");
00540       return Eigen::Vector3f(0, 0, 0);
00541     }
00542   }
00543 
00544   size_t Polygon::previousIndex(size_t i)
00545   {
00546     if (i == 0) {
00547       return vertices_.size() - 1;
00548     }
00549     else {
00550       return i - 1;
00551     }
00552   }
00553   
00554   size_t Polygon::nextIndex(size_t i)
00555   {
00556     if (i == vertices_.size() - 1) {
00557       return 0;
00558     }
00559     else {
00560       return i + 1;
00561     }
00562   }
00563 
00564   Polygon Polygon::fromROSMsg(const geometry_msgs::Polygon& polygon)
00565   {
00566     Vertices vertices;
00567     for (size_t i = 0; i < polygon.points.size(); i++) {
00568       Eigen::Vector3f v;
00569       pointFromXYZToVector<geometry_msgs::Point32, Eigen::Vector3f>(
00570         polygon.points[i], v);
00571       vertices.push_back(v);
00572     }
00573     return Polygon(vertices);
00574   }
00575 
00576   Polygon::Ptr Polygon::fromROSMsgPtr(const geometry_msgs::Polygon& polygon)
00577   {
00578     Vertices vertices;
00579     for (size_t i = 0; i < polygon.points.size(); i++) {
00580       Eigen::Vector3f v;
00581       pointFromXYZToVector<geometry_msgs::Point32, Eigen::Vector3f>(
00582         polygon.points[i], v);
00583       vertices.push_back(v);
00584     }
00585     return Polygon::Ptr(new Polygon(vertices));
00586   }
00587 
00588   std::vector<Polygon::Ptr> Polygon::fromROSMsg(const jsk_recognition_msgs::PolygonArray& msg,
00589                                                 const Eigen::Affine3f& offset)
00590   {
00591     std::vector<Polygon::Ptr> ret;
00592     for (size_t i = 0; i < msg.polygons.size(); i++) {
00593       Polygon::Ptr polygon = Polygon::fromROSMsgPtr(msg.polygons[i].polygon);
00594       polygon->transformBy(offset);
00595       ret.push_back(polygon);
00596     }
00597     return ret;
00598   }
00599   
00600   bool Polygon::isInside(const Eigen::Vector3f& p)
00601   {
00602     if (isTriangle()) {
00603       Eigen::Vector3f A = vertices_[0];
00604       Eigen::Vector3f B = vertices_[1];
00605       Eigen::Vector3f C = vertices_[2];
00606       // Eigen::Vector3f cross0 = (A - C).cross(p - A);
00607       // Eigen::Vector3f cross1 = (B - A).cross(p - B);
00608       // Eigen::Vector3f cross2 = (C - B).cross(p - C);
00609       
00610       Eigen::Vector3f cross0 = (B - A).cross(p - A);
00611       Eigen::Vector3f cross1 = (C - B).cross(p - B);
00612       Eigen::Vector3f cross2 = (A - C).cross(p - C);
00613       if (cross0.dot(cross1) >= 0 &&
00614           cross1.dot(cross2) >= 0) {
00615         return true;
00616       }
00617       else {
00618         return false;
00619       }
00620     }
00621     else {
00622       std::vector<Polygon::Ptr> triangles = decomposeToTriangles();
00623       for (size_t i = 0; i < triangles.size(); i++) {
00624         if (triangles[i]->isInside(p)) {
00625           return true;
00626         }
00627       }
00628       return false;
00629     }
00630   }
00631 
00632 
00633 }


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