Go to the documentation of this file.
36 #ifndef JSK_RECOGNITION_UTILS_GEO_POLYGON_H_
37 #define JSK_RECOGNITION_UTILS_GEO_POLYGON_H_
39 #include <Eigen/Geometry>
40 #include <boost/shared_ptr.hpp>
41 #include <pcl/point_cloud.h>
42 #include <pcl/point_types.h>
43 #include <boost/random.hpp>
44 #include <geometry_msgs/Polygon.h>
45 #include <jsk_recognition_msgs/PolygonArray.h>
55 class Polygon:
public Plane
59 typedef boost::tuple<Ptr, Ptr>
PtrPair;
62 const std::vector<float>& coefficients);
73 double& max_x,
double& max_y);
74 template <
class Po
intT>
75 typename pcl::PointCloud<PointT>::Ptr
samplePoints(
double grid_size)
77 typename pcl::PointCloud<PointT>::Ptr
78 ret (
new pcl::PointCloud<PointT>);
79 double min_x, min_y, max_x, max_y;
88 for (
double x = min_x; x < max_x; x += grid_size) {
89 for (
double y = min_y; y < max_y; y += grid_size) {
90 Eigen::Vector3f candidate(x, y, 0);
91 Eigen::Vector3f candidate_global =
coordinates() * candidate;
94 for (
size_t i = 0;
i < triangles.size();
i++) {
95 if (triangles[
i]->
isInside(candidate_global)) {
102 p.x = candidate_global[0];
103 p.y = candidate_global[1];
104 p.z = candidate_global[2];
108 ret->points.push_back(
p);
119 std::vector<Segment::Ptr>
edges()
const;
125 double distance(
const Eigen::Vector3f& point);
132 double distance(
const Eigen::Vector3f& point,
133 Eigen::Vector3f& nearest_point);
154 virtual Eigen::Vector3f
nearestPoint(
const Eigen::Vector3f&
p,
159 virtual Eigen::Vector3f
getVertex(
size_t i);
162 virtual double area();
165 const Eigen::Vector3f& direction);
172 virtual bool isInside(
const Eigen::Vector3f&
p);
186 static std::vector<Polygon::Ptr>
fromROSMsg(
const jsk_recognition_msgs::PolygonArray& msg,
187 const Eigen::Affine3f& offset = Eigen::Affine3f::Identity());
210 cv::Mat& image)
const;
218 const cv::Scalar& color,
219 const int line_width = 1)
const;
223 pcl::PointCloud<PointT>& output) {
228 p.x = v[0];
p.y = v[1];
p.z = v[2];
229 output.points[
i] =
p;
232 output.width = output.points.size();
std::vector< Polygon::Ptr > cached_triangles_
virtual Plane transform(const Eigen::Affine3d &transform)
boost::tuple< size_t, size_t > PointIndexPair
double distance(const Eigen::Vector3f &point)
Compute distance between point and this polygon.
virtual size_t getFarestPointIndex(const Eigen::Vector3f &O)
virtual Vertices getVertices()
virtual Eigen::Vector3f getVertex(size_t i)
virtual void clearTriangleDecompositionCache()
virtual size_t getNumVertices()
virtual bool isPossibleToRemoveTriangleAtIndex(size_t index, const Eigen::Vector3f &direction)
static Polygon fromROSMsg(const geometry_msgs::Polygon &polygon)
virtual bool isInside(const Eigen::Vector3f &p)
return true if p is inside of polygon. p should be in global coordinates.
virtual bool isTriangle()
virtual void transformBy(const Eigen::Affine3d &transform)
transform Polygon with given transform. cached triangles is cleared.
virtual Eigen::Vector3f centroid()
virtual PointIndexPair getNeighborIndex(size_t index)
pcl::PointCloud< PointT >::Ptr samplePoints(double grid_size)
std::vector< Segment::Ptr > edges() const
get all the edges as point of Segment.
virtual bool maskImage(const jsk_recognition_utils::CameraDepthSensor &model, cv::Mat &image) const
generate mask image of the polygon. if all the points are outside of field-of-view,...
size_t previousIndex(size_t i)
boost::tuple< Ptr, Ptr > PtrPair
virtual Eigen::Vector3f directionAtPoint(size_t i)
Polygon(const Vertices &vertices)
size_t nextIndex(size_t i)
static Polygon createPolygonWithSkip(const Vertices &vertices)
virtual Eigen::Affine3f coordinates()
void boundariesToPointCloud(pcl::PointCloud< PointT > &output)
static Polygon::Ptr fromROSMsgPtr(const geometry_msgs::Polygon &polygon)
Eigen::Vector3f randomSampleLocalPoint(boost::mt19937 &random_generator)
virtual Eigen::Vector3f nearestPoint(const Eigen::Vector3f &p, double &distance)
Compute nearest point from p on this polygon.
virtual std::vector< Polygon::Ptr > decomposeToTriangles()
boost::shared_ptr< Polygon > Ptr
virtual void drawLineToImage(const jsk_recognition_utils::CameraDepthSensor &model, cv::Mat &image, const cv::Scalar &color, const int line_width=1) const
draw line of polygons on image.
virtual void getLocalMinMax(double &min_x, double &min_y, double &max_x, double &max_y)
virtual Eigen::Vector3f getNormalFromVertices()
virtual PtrPair separatePolygon(size_t index)
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices