convex_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/convex_polygon.h"
00039 #include "jsk_recognition_utils/geo_util.h"
00040 
00041 namespace jsk_recognition_utils
00042 {
00043   ConvexPolygon::ConvexPolygon(const Vertices& vertices):
00044     Polygon(vertices)
00045   {
00046 
00047   }
00048 
00049   ConvexPolygon::ConvexPolygon(const Vertices& vertices,
00050                                const std::vector<float>& coefficients):
00051     Polygon(vertices, coefficients)
00052   {
00053 
00054   }
00055   
00056   void ConvexPolygon::projectOnPlane(
00057     const Eigen::Vector3f& p, Eigen::Vector3f& output)
00058   {
00059     Plane::project(p, output);
00060   }
00061 
00062   void ConvexPolygon::projectOnPlane(
00063     const Eigen::Affine3f& pose, Eigen::Affine3f& output)
00064   {
00065     Eigen::Vector3f p(pose.translation());
00066     Eigen::Vector3f output_p;
00067     projectOnPlane(p, output_p);
00068     // ROS_INFO("[ConvexPolygon::projectOnPlane] p: [%f, %f, %f]",
00069     //          p[0], p[1], p[2]);
00070     // ROS_INFO("[ConvexPolygon::projectOnPlane] output_p: [%f, %f, %f]",
00071     //          output_p[0], output_p[1], output_p[2]);
00072     Eigen::Quaternionf rot;
00073     rot.setFromTwoVectors(pose.rotation() * Eigen::Vector3f::UnitZ(),
00074                           coordinates().rotation() * Eigen::Vector3f::UnitZ());
00075     Eigen::Quaternionf coords_rot(coordinates().rotation());
00076     Eigen::Quaternionf pose_rot(pose.rotation());
00077     // ROS_INFO("[ConvexPolygon::projectOnPlane] rot: [%f, %f, %f, %f]",
00078     //          rot.x(), rot.y(), rot.z(), rot.w());
00079     // ROS_INFO("[ConvexPolygon::projectOnPlane] coords_rot: [%f, %f, %f, %f]",
00080     //          coords_rot.x(), coords_rot.y(), coords_rot.z(), coords_rot.w());
00081     // ROS_INFO("[ConvexPolygon::projectOnPlane] pose_rot: [%f, %f, %f, %f]",
00082     //          pose_rot.x(), pose_rot.y(), pose_rot.z(), pose_rot.w());
00083     // ROS_INFO("[ConvexPolygon::projectOnPlane] normal: [%f, %f, %f]", normal_[0], normal_[1], normal_[2]);
00084     // Eigen::Affine3f::Identity() *
00085     // output.translation() = Eigen::Translation3f(output_p);
00086     // output.rotation() = rot * pose.rotation();
00087     //output = Eigen::Translation3f(output_p) * rot * pose.rotation();
00088     output = Eigen::Affine3f(rot * pose.rotation());
00089     output.pretranslate(output_p);
00090     // Eigen::Vector3f projected_point = output * Eigen::Vector3f(0, 0, 0);
00091     // ROS_INFO("[ConvexPolygon::projectOnPlane] output: [%f, %f, %f]",
00092     //          projected_point[0], projected_point[1], projected_point[2]);
00093   }
00094 
00095   ConvexPolygon ConvexPolygon::flipConvex()
00096   {
00097     Vertices new_vertices;
00098     std::reverse_copy(vertices_.begin(), vertices_.end(),
00099                       std::back_inserter(new_vertices));
00100     std::vector<float> reversed_coefficients(4);
00101     reversed_coefficients[0] = - normal_[0];
00102     reversed_coefficients[1] = - normal_[1];
00103     reversed_coefficients[2] = - normal_[2];
00104     reversed_coefficients[3] = - d_;
00105     
00106     return ConvexPolygon(new_vertices, reversed_coefficients);
00107   }
00108   
00109   void ConvexPolygon::project(const Eigen::Vector3f& p, Eigen::Vector3f& output)
00110   {
00111     Eigen::Vector3f point_on_plane;
00112     Plane::project(p, point_on_plane);
00113     // check point_ is inside or not
00114     if (isInside(point_on_plane)) {
00115       output = point_on_plane;
00116     }
00117     else {
00118       // find the minimum foot point
00119       double min_distance = DBL_MAX;
00120       Eigen::Vector3f min_point;
00121       for (size_t i = 0; i < vertices_.size() - 1; i++) {
00122         Segment seg(vertices_[i], vertices_[i + 1]);
00123         Eigen::Vector3f foot;
00124         double distance = seg.distanceToPoint(p, foot);
00125         if (distance < min_distance) {
00126           min_distance = distance;
00127           min_point = foot;
00128         }
00129       }
00130       output = min_point;
00131     }
00132   }
00133 
00134   void ConvexPolygon::project(const Eigen::Vector3d& p, Eigen::Vector3d& output)
00135   {
00136     Eigen::Vector3f output_f;
00137     Eigen::Vector3f p_f(p[0], p[1], p[2]);
00138     project(p_f, output_f);
00139     pointFromVectorToVector<Eigen::Vector3f, Eigen::Vector3d>(output_f, output);
00140   }
00141   
00142   void ConvexPolygon::project(const Eigen::Vector3d& p, Eigen::Vector3f& output)
00143   {
00144     Eigen::Vector3f p_f(p[0], p[1], p[2]);
00145     project(p_f, output);
00146   }
00147   
00148   void ConvexPolygon::project(const Eigen::Vector3f& p, Eigen::Vector3d& output)
00149   {
00150     Eigen::Vector3f output_f;
00151     project(p, output_f);
00152     pointFromVectorToVector<Eigen::Vector3f, Eigen::Vector3d>(output_f, output);
00153   }
00154   
00155 
00156   Eigen::Vector3f ConvexPolygon::getCentroid()
00157   {
00158     Eigen::Vector3f ret(0, 0, 0);
00159     for (size_t i = 0; i < vertices_.size(); i++) {
00160       ret = ret + vertices_[i];
00161     }
00162     return ret / vertices_.size();
00163   }
00164 
00165   ConvexPolygon ConvexPolygon::fromROSMsg(const geometry_msgs::Polygon& polygon)
00166   {
00167     Vertices vertices;
00168     for (size_t i = 0; i < polygon.points.size(); i++) {
00169       Eigen::Vector3f p;
00170       pointFromXYZToVector<geometry_msgs::Point32, Eigen::Vector3f>(
00171         polygon.points[i], p);
00172       vertices.push_back(p);
00173     }
00174     return ConvexPolygon(vertices);
00175   }
00176   
00177   ConvexPolygon::Ptr ConvexPolygon::fromROSMsgPtr(const geometry_msgs::Polygon& polygon)
00178   {
00179     Vertices vertices;
00180     for (size_t i = 0; i < polygon.points.size(); i++) {
00181       Eigen::Vector3f p;
00182       pointFromXYZToVector<geometry_msgs::Point32, Eigen::Vector3f>(
00183         polygon.points[i], p);
00184       vertices.push_back(p);
00185     }
00186     return ConvexPolygon::Ptr(new ConvexPolygon(vertices));
00187   }
00188 
00189   bool ConvexPolygon::distanceSmallerThan(const Eigen::Vector3f& p,
00190                                           double distance_threshold)
00191   {
00192     double dummy_distance;
00193     return distanceSmallerThan(p, distance_threshold, dummy_distance);
00194   }
00195   
00196   bool ConvexPolygon::distanceSmallerThan(const Eigen::Vector3f& p,
00197                                           double distance_threshold,
00198                                           double& output_distance)
00199   {
00200     // first check distance as Plane rather than Convex
00201     double plane_distance = distanceToPoint(p);
00202     if (plane_distance > distance_threshold) {
00203       output_distance = plane_distance;
00204       return false;
00205     }
00206 
00207     Eigen::Vector3f foot_point;
00208     project(p, foot_point);
00209     double convex_distance = (p - foot_point).norm();
00210     output_distance = convex_distance;
00211     return convex_distance < distance_threshold;
00212   }
00213 
00214   bool ConvexPolygon::allEdgesLongerThan(double thr)
00215   {
00216     for (size_t i = 0; i < vertices_.size(); i++) {
00217       Eigen::Vector3f p_k = vertices_[i];
00218       Eigen::Vector3f p_k_1;
00219       if (i == vertices_.size() - 1) {
00220         p_k_1 = vertices_[0];
00221       }
00222       else {
00223         p_k_1 = vertices_[i + 1];
00224       }
00225       if ((p_k - p_k_1).norm() < thr) {
00226         return false;
00227       }
00228     }
00229     return true;
00230   }
00231 
00232   double ConvexPolygon::distanceFromVertices(const Eigen::Vector3f& p)
00233   {
00234     double min_distance = DBL_MAX;
00235     for (size_t i = 0; i < vertices_.size(); i++) {
00236       Eigen::Vector3f v = vertices_[i];
00237       double d = (p - v).norm();
00238       if (d < min_distance) {
00239         min_distance = d;
00240       }
00241     }
00242     return min_distance;
00243   }
00244   
00245   ConvexPolygon::Ptr ConvexPolygon::magnifyByDistance(const double distance)
00246   {
00247     // compute centroid
00248     Eigen::Vector3f c = centroid();
00249     Vertices new_vertices(vertices_.size());
00250     for (size_t i = 0; i < vertices_.size(); i++) {
00251       new_vertices[i] = (vertices_[i] - c).normalized() * distance + vertices_[i];
00252       // ROS_INFO("old v: [%f, %f, %f]", vertices_[i][0], vertices_[i][1], vertices_[i][2]);
00253       // ROS_INFO("new v: [%f, %f, %f]", new_vertices[i][0], new_vertices[i][1], new_vertices[i][2]);
00254       // ROS_INFO("");
00255     }
00256     
00257     ConvexPolygon::Ptr ret (new ConvexPolygon(new_vertices));
00258     return ret;
00259   }
00260   
00261   ConvexPolygon::Ptr ConvexPolygon::magnify(const double scale_factor)
00262   {
00263     // compute centroid
00264     Eigen::Vector3f c = centroid();
00265     Vertices new_vertices;
00266     for (size_t i = 0; i < vertices_.size(); i++) {
00267       new_vertices.push_back((vertices_[i] - c) * scale_factor + c);
00268     }
00269     ConvexPolygon::Ptr ret (new ConvexPolygon(new_vertices));
00270     return ret;
00271   }
00272 
00273   geometry_msgs::Polygon ConvexPolygon::toROSMsg()
00274   {
00275     geometry_msgs::Polygon polygon;
00276     for (size_t i = 0; i < vertices_.size(); i++) {
00277       geometry_msgs::Point32 ros_point;
00278       ros_point.x = vertices_[i][0];
00279       ros_point.y = vertices_[i][1];
00280       ros_point.z = vertices_[i][2];
00281       polygon.points.push_back(ros_point);
00282     }
00283     return polygon;
00284   }
00285 
00286 
00287   bool ConvexPolygon::isProjectableInside(const Eigen::Vector3f& p)
00288   {
00289     Eigen::Vector3f foot_point;
00290     Plane::project(p, foot_point);
00291     return isInside(foot_point);
00292   }
00293 
00294 }


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