polygon.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 #define BOOST_PARAMETER_MAX_ARITY 7
37 
43 
44 namespace jsk_recognition_utils
45 {
47  {
48  const double thr = 0.01;
49  Polygon not_skipped_polygon(vertices);
50  Vertices skipped_vertices;
51  for (size_t i = 0; i < vertices.size(); i++) {
52  size_t next_i = not_skipped_polygon.nextIndex(i);
53  Eigen::Vector3f v0 = vertices[i];
54  Eigen::Vector3f v1 = vertices[next_i];
55  if ((v1 - v0).norm() > thr) {
56  skipped_vertices.push_back(vertices[i]);
57  }
58  }
59  return Polygon(skipped_vertices);
60  }
61 
62  Eigen::Vector3f Polygon::centroid()
63  {
64  Eigen::Vector3f c(0, 0, 0);
65  if (vertices_.size() == 0) {
66  return c;
67  }
68  else {
69  for (size_t i = 0; i < vertices_.size(); i++) {
70  c = c + vertices_[i];
71  }
72  return c / vertices_.size();
73  }
74  }
75 
76  std::vector<Plane::Ptr> convertToPlanes(
77  std::vector<pcl::ModelCoefficients::Ptr> coefficients)
78  {
79  std::vector<Plane::Ptr> ret;
80  for (size_t i = 0; i < coefficients.size(); i++) {
81  ret.push_back(Plane::Ptr (new Plane(coefficients[i]->values)));
82  }
83  return ret;
84  }
85 
86 
87  Polygon::Polygon(const Vertices& vertices):
88  Plane((vertices[1] - vertices[0]).cross(vertices[2] - vertices[0]).normalized(), vertices[0]),
89  vertices_(vertices)
90  {
91 
92  }
93 
94  Polygon::Polygon(const Vertices& vertices,
95  const std::vector<float>& coefficients):
96  Plane(coefficients), vertices_(vertices)
97  {
98 
99  }
100 
102  {
103 
104  }
105 
106  size_t Polygon::getFarestPointIndex(const Eigen::Vector3f& O)
107  {
108  double max_distance = - DBL_MAX;
109  size_t max_index = 0;
110  for (size_t i = 0; i < vertices_.size(); i++) {
111  Eigen::Vector3f v = vertices_[i];
112  double d = (O - v).norm();
113  if (max_distance < d) {
114  max_distance = d;
115  max_index = i;
116  }
117  }
118  return max_index;
119  }
120 
122  {
123  return boost::make_tuple<size_t, size_t>(
124  previousIndex(index), nextIndex(index));
125  }
126 
127  double Polygon::area()
128  {
129  if (isTriangle()) {
130  return (vertices_[1] - vertices_[0]).cross(vertices_[2] - vertices_[0]).norm() / 2.0;
131  }
132  else {
133  std::vector<Polygon::Ptr> triangles = decomposeToTriangles();
134  double sum = 0;
135  for (size_t i = 0; i < triangles.size(); i++) {
136  sum += triangles[i]->area();
137  }
138  return sum;
139  }
140  }
141 
142  Eigen::Vector3f Polygon::directionAtPoint(size_t i)
143  {
144  Eigen::Vector3f O = vertices_[i];
145  Eigen::Vector3f A = vertices_[previousIndex(i)];
146  Eigen::Vector3f B = vertices_[nextIndex(i)];
147  Eigen::Vector3f OA = A - O;
148  Eigen::Vector3f OB = B - O;
149  Eigen::Vector3f n = (OA.normalized()).cross(OB.normalized());
150  if (n.norm() == 0) {
151  // ROS_ERROR("normal is 0");
152  // ROS_ERROR("O: [%f, %f, %f]", O[0], O[1], O[2]);
153  // ROS_ERROR("A: [%f, %f, %f]", A[0], A[1], A[2]);
154  // ROS_ERROR("B: [%f, %f, %f]", B[0], B[1], B[2]);
155  // ROS_ERROR("OA: [%f, %f, %f]", OA[0], OA[1], OA[2]);
156  // ROS_ERROR("OB: [%f, %f, %f]", OB[0], OB[1], OB[2]);
157  //exit(1);
158  }
159  return n.normalized();
160  }
161 
163  return vertices_.size() == 3;
164  }
165 
166  void Polygon::getLocalMinMax(double& min_x, double& min_y,
167  double& max_x, double& max_y)
168  {
169  min_x = DBL_MAX;
170  min_y = DBL_MAX;
171  max_x = - DBL_MAX;
172  max_y = - DBL_MAX;
173 
174  Eigen::Affine3f inv_coords = coordinates().inverse();
175  for (size_t i = 0; i < vertices_.size(); i++) {
176  // Convert vertices into local coordinates
177  Eigen::Vector3f local_point = inv_coords * vertices_[i];
178  min_x = ::fmin(local_point[0], min_x);
179  min_y = ::fmin(local_point[1], min_y);
180  max_x = ::fmax(local_point[0], max_x);
181  max_y = ::fmax(local_point[1], max_y);
182  }
183  }
184 
185  Eigen::Vector3f Polygon::randomSampleLocalPoint(boost::mt19937& random_generator)
186  {
187  // Compute min/max point
188  double min_x, min_y, max_x, max_y;
189  getLocalMinMax(min_x, min_y, max_x, max_y);
190  std::vector<Polygon::Ptr> triangles = decomposeToTriangles();
191  while (true) {
192  double x = randomUniform(min_x, max_x, random_generator);
193  double y = randomUniform(min_y, max_y, random_generator);
194  Eigen::Vector3f local_v = Eigen::Vector3f(x, y, 0);
195  Eigen::Vector3f v = coordinates() * local_v;
196  // ROS_INFO_THROTTLE(1.0, "x: %f -- %f", min_x, max_x);
197  // ROS_INFO_THROTTLE(1.0, "y: %f -- %f", min_y, max_y);
198  // ROS_INFO_THROTTLE(1.0, "sampled point: [%f, %f]", x, y);
199  // for (size_t i = 0; i < vertices_.size(); i++) {
200  // Eigen::Vector3f v = coordinates().inverse() * vertices_[i];
201  // ROS_INFO("v: [%f, %f, %f]", v[0], v[1], v[2]);
202  // }
203  if (isInside(v)) {
204  return local_v;
205  }
206  else {
207  // ROS_INFO_THROTTLE(1.0, "outside");
208  }
209  }
210  }
211 
212  std::vector<Segment::Ptr> Polygon::edges() const
213  {
214  std::vector<Segment::Ptr> ret;
215  ret.reserve(vertices_.size());
216  for (size_t i = 0; i < vertices_.size() - 1; i++) {
217  // edge between i and i+1
218  ret.push_back(Segment::Ptr(new Segment(vertices_[i], vertices_[i+1])));
219  }
220  // edge between [-1] and [0]
221  ret.push_back(Segment::Ptr(new Segment(vertices_[vertices_.size() - 1], vertices_[0])));
222  return ret;
223  }
224 
225  double Polygon::distance(const Eigen::Vector3f& point)
226  {
227  Eigen::Vector3f nearest_point;
228  return Polygon::distance(point, nearest_point);
229  }
230 
231  double Polygon::distance(const Eigen::Vector3f& point,
232  Eigen::Vector3f& nearest_point)
233  {
234  double distance;
235  nearest_point = Polygon::nearestPoint(point, distance);
236  return distance;
237  }
238 
239  Eigen::Vector3f Polygon::nearestPoint(const Eigen::Vector3f& p,
240  double& distance)
241  {
242  Eigen::Vector3f projected_p;
243  Plane::project(p, projected_p);
244  if (isInside(projected_p)) {
245  distance = (p - projected_p).norm();
246  return projected_p;
247  }
248  else {
249  std::vector<Segment::Ptr> boundary_edges = edges();
250  double min_distnace = DBL_MAX;
251  Eigen::Vector3f nearest_point;
252  // brute-force searching
253  for (size_t i = 0; i < boundary_edges.size(); i++) {
254  Segment::Ptr edge = boundary_edges[i];
255  Eigen::Vector3f foot;
256  double d = edge->distance(p, foot);
257  if (min_distnace > d) {
258  nearest_point = foot;
259  min_distnace = d;
260  }
261  }
262  distance = min_distnace;
263  return nearest_point;
264  }
265  }
266 
268  return vertices_.size();
269  }
270 
271  Eigen::Vector3f Polygon::getVertex(size_t i) {
272  return vertices_[i];
273  }
274 
276  {
277  PointIndexPair neighbor_index = getNeighborIndex(index);
278  Vertices triangle_vertices;
279  triangle_vertices.push_back(vertices_[index]);
280  triangle_vertices.push_back(vertices_[neighbor_index.get<1>()]);
281  triangle_vertices.push_back(vertices_[neighbor_index.get<0>()]);
282  Polygon::Ptr triangle(new Polygon(triangle_vertices));
283  Vertices rest_vertices;
284  // do not add the points on the line
285  for (size_t i = neighbor_index.get<1>(); i != index;) {
286  // check the points on the line
287  if (i == neighbor_index.get<1>()) {
288  rest_vertices.push_back(vertices_[i]);
289  }
290  else {
291  if (directionAtPoint(i).norm() != 0.0) {
292  rest_vertices.push_back(vertices_[i]);
293  }
294  else {
295  ROS_ERROR("removed: %lu", i);
296  }
297  }
298  i = nextIndex(i);
299  }
300  Polygon::Ptr rest(new Polygon(rest_vertices));
301  return boost::make_tuple<Polygon::Ptr, Polygon::Ptr>(
302  triangle, rest);
303  }
304 
306  size_t index,
307  const Eigen::Vector3f& direction)
308  {
309  Polygon::PtrPair candidate = separatePolygon(index);
310  Polygon::Ptr triangle_candidate = candidate.get<0>();
311  Polygon::Ptr rest_candidate = candidate.get<1>();
312  // first check direction
313  Eigen::Vector3f the_direction = directionAtPoint(index);
314  //ROS_INFO("direction: [%f, %f, %f]", the_direction[0], the_direction[1], the_direction[2]);
315  if (the_direction.norm() == 0.0) {
316  ROS_ERROR("malformed polygon");
317  exit(1);
318  }
319  if (direction.dot(the_direction) < 0) {
320 #ifdef DEBUG_GEO_UTIL
321  ROS_INFO("triangle is not same direction");
322  ROS_INFO("direction: [%f, %f, %f]", direction[0], direction[1], direction[2]);
323  ROS_INFO("the_direction: [%f, %f, %f]",
324  the_direction[0],
325  the_direction[1],
326  the_direction[2]);
327  for (size_t i = 0; i < vertices_.size(); i++) {
328  Eigen::Vector3f v = directionAtPoint(i);
329  ROS_INFO("the_direction[%lu]: [%f, %f, %f]",
330  i, v[0], v[1], v[2]);
331  // other direction
332  }
333 #endif
334  return false;
335  }
336  else {
337  //return true;
338  // second, check the triangle includes the rest of points or not
339  for (size_t i = 0; i < rest_candidate->vertices_.size(); i++) {
340  if (i == 0 || i == rest_candidate->vertices_.size() - 1) {
341  continue; // do not check the first and the last point
342  }
343  else {
344  Eigen::Vector3f P = rest_candidate->getVertex(i);
345  Eigen::Vector3f A = triangle_candidate->getVertex(0);
346  Eigen::Vector3f B = triangle_candidate->getVertex(1);
347  Eigen::Vector3f C = triangle_candidate->getVertex(2);
348  Eigen::Vector3f CA = A - C;
349  Eigen::Vector3f BC = C - B;
350  Eigen::Vector3f AB = B - A;
351  Eigen::Vector3f AP = P - A;
352  Eigen::Vector3f BP = P - B;
353  Eigen::Vector3f CP = P - C;
354  Eigen::Vector3f Across = CA.normalized().cross(AP.normalized()).normalized();
355  Eigen::Vector3f Bcross = AB.normalized().cross(BP.normalized()).normalized();
356  Eigen::Vector3f Ccross = BC.normalized().cross(CP.normalized()).normalized();
357 #ifdef DEBUG_GEO_UTIL
358  ROS_INFO("P: [%f, %f, %f]", P[0], P[1], P[2]);
359  ROS_INFO("A: [%f, %f, %f]", A[0], A[1], A[2]);
360  ROS_INFO("B: [%f, %f, %f]", B[0], B[1], B[2]);
361  ROS_INFO("C: [%f, %f, %f]", C[0], C[1], C[2]);
362  ROS_INFO("Across: [%f, %f, %f]", Across[0], Across[1], Across[2]);
363  ROS_INFO("Bcross: [%f, %f, %f]", Bcross[0], Bcross[1], Bcross[2]);
364  ROS_INFO("Ccross: [%f, %f, %f]", Ccross[0], Ccross[1], Ccross[2]);
365  ROS_INFO("Across-Bcross: %f", Across.dot(Bcross));
366  ROS_INFO("Bcross-Ccross: %f", Bcross.dot(Ccross));
367  ROS_INFO("Ccross-Across: %f", Ccross.dot(Across));
368 #endif
369  if (((Across.dot(Bcross) > 0 &&
370  Bcross.dot(Ccross) > 0 &&
371  Ccross.dot(Across) > 0) ||
372  (Across.dot(Bcross) < 0 &&
373  Bcross.dot(Ccross) < 0 &&
374  Ccross.dot(Across) < 0))) {
375  // ROS_ERROR("%lu -- %lu is inside", index, i);
376  return false;
377  }
378  // ConvexPolygon convex_triangle(triangle_candidate->vertices_);
379  // if (convex_triangle.isInside(v)) {
380  // //ROS_INFO("vertices is inside of the polygon");
381  // return false;
382  // }
383  }
384  }
385  return true;
386  }
387  }
388 
389  void Polygon::transformBy(const Eigen::Affine3d& transform)
390  {
391  Eigen::Affine3f float_affine;
392  convertEigenAffine3(transform, float_affine);
393  transformBy(float_affine);
394  }
395 
396  void Polygon::transformBy(const Eigen::Affine3f& transform)
397  {
398  // 1. clear cached_triangles_
399  // 2. transform vertices_
400  // 3. update normal_ and d_
401  // 3. update plane_coordinates_
402  cached_triangles_.clear();
403  for (size_t i = 0; i < vertices_.size(); i++) {
404  vertices_[i] = transform * vertices_[i];
405  }
406  // compute normal and d
407  normal_ = (vertices_[1] - vertices_[0]).cross(vertices_[2] - vertices_[0]).normalized();
408  d_ = - normal_.dot(vertices_[0]) / normal_.norm();
410  }
411 
413  cv::Mat& image) const
414  {
415  std::vector<cv::Point> projected_vertices
417  bool all_outside = true;
418  // check some of vertices is inside of FoV
419  for (size_t i = 0; i < projected_vertices.size(); i++) {
420  if (model.isInside(projected_vertices[i])) {
421  all_outside = false;
422  }
423  }
424  image = model.image(CV_8UC1);
425  // check all the v is positive
426  for (size_t i = 0; i < vertices_.size(); i++) {
427  if (vertices_[i][2] < 0) {
428  return false;
429  }
430  }
431  const cv::Point* element_points[1] = {&projected_vertices[0]};
432  int number_of_points = (int)projected_vertices.size();
433  // Is it should be cv::fillPoly?
434  cv::fillPoly(image,
435  element_points,
436  &number_of_points,
437  1,
438  cv::Scalar(255));
439  return !all_outside;
440  }
441 
443  cv::Mat& image,
444  const cv::Scalar& color,
445  const int line_width) const
446  {
447  std::vector<cv::Point> projected_vertices
449 
450  for (size_t i = 0; i < projected_vertices.size() - 1; i++) {
451  cv::Point from = projected_vertices[i];
452  cv::Point to = projected_vertices[i+1];
453  if (model.isInside(from) || model.isInside(to)) {
454  cv::line(image, from, to, color, line_width);
455  }
456  }
457  cv::Point from = projected_vertices[projected_vertices.size() - 1];
458  cv::Point to = projected_vertices[0];
459  if (model.isInside(from) || model.isInside(to)) {
460  cv::line(image, from, to, color, line_width);
461  }
462  }
463 
465  {
466 #ifdef DEBUG_GEO_UTIL
467  for (size_t i = 0; i < getNumVertices(); i++) {
468  Eigen::Vector3f n = directionAtPoint(i);
469  ROS_INFO("n[%lu] [%f, %f, %f]", i, n[0], n[1], n[2]);
470  }
471 #endif
472  Eigen::Vector3f n0 = directionAtPoint(0);
473  for (size_t i = 1; i < getNumVertices(); i++) {
474  Eigen::Vector3f n = directionAtPoint(i);
475  if (n0.dot(n) < 0) {
476  return false;
477  }
478  }
479  return true;
480  }
481 
482  std::vector<Polygon::Ptr> Polygon::decomposeToTriangles()
483  {
484  if (cached_triangles_.size() != 0) {
485  return cached_triangles_;
486  }
487  std::vector<Polygon::Ptr> ret;
488 
489  // if this polygon is triangle, return immediately
490  if (isTriangle()) {
491  ret.push_back(Polygon::Ptr( new Polygon(*this)));
492  return ret;
493  }
494 
496  // convert
497  pcl::PolygonMesh::Ptr input_mesh (new pcl::PolygonMesh);
498  pcl::PCLPointCloud2 mesh_cloud;
499  pcl::PointCloud<pcl::PointXYZ> mesh_pcl_cloud;
500  boundariesToPointCloud<pcl::PointXYZ>(mesh_pcl_cloud);
501  std::vector<pcl::Vertices> mesh_vertices(1);
502  for (size_t i = 0; i < vertices_.size(); i++) {
503  mesh_vertices[0].vertices.push_back(i);
504  }
505  //mesh_vertices[0].vertices.push_back(0); // close
506  mesh_pcl_cloud.height = 1;
507  mesh_pcl_cloud.width = mesh_pcl_cloud.points.size();
508  pcl::toPCLPointCloud2<pcl::PointXYZ>(mesh_pcl_cloud, mesh_cloud);
509 
510  input_mesh->polygons = mesh_vertices;
511  input_mesh->cloud = mesh_cloud;
512  clip.setInputMesh(input_mesh);
513  pcl::PolygonMesh output;
514  clip.process(output);
515  assert(output.polygons.size() != 0);
516  // convert to Polygon instances
517  for (size_t i = 0; i < output.polygons.size(); i++) {
518  pcl::Vertices output_polygon_vertices = output.polygons[i];
519  Vertices vs(output_polygon_vertices.vertices.size());
520  for (size_t j = 0; j < output_polygon_vertices.vertices.size(); j++) {
521  pcl::PointXYZ p
522  = mesh_pcl_cloud.points[output_polygon_vertices.vertices[j]];
523  Eigen::Vector3f v;
524  pointFromXYZToVector<pcl::PointXYZ, Eigen::Vector3f>(p, v);
525  vs[j] = v;
526  }
527  ret.push_back(Polygon::Ptr(new Polygon(vs, toCoefficients())));
528  }
529  cached_triangles_ = ret;
530  return ret;
531  }
532 
534  {
535  if (vertices_.size() >= 3) {
536  return (vertices_[1] - vertices_[0]).cross(vertices_[2] - vertices_[0]).normalized();
537  }
538  else {
539  ROS_ERROR("the number of vertices is not enough");
540  return Eigen::Vector3f(0, 0, 0);
541  }
542  }
543 
544  size_t Polygon::previousIndex(size_t i)
545  {
546  if (i == 0) {
547  return vertices_.size() - 1;
548  }
549  else {
550  return i - 1;
551  }
552  }
553 
554  size_t Polygon::nextIndex(size_t i)
555  {
556  if (i == vertices_.size() - 1) {
557  return 0;
558  }
559  else {
560  return i + 1;
561  }
562  }
563 
564  Polygon Polygon::fromROSMsg(const geometry_msgs::Polygon& polygon)
565  {
566  Vertices vertices;
567  for (size_t i = 0; i < polygon.points.size(); i++) {
568  Eigen::Vector3f v;
569  pointFromXYZToVector<geometry_msgs::Point32, Eigen::Vector3f>(
570  polygon.points[i], v);
571  vertices.push_back(v);
572  }
573  return Polygon(vertices);
574  }
575 
576  Polygon::Ptr Polygon::fromROSMsgPtr(const geometry_msgs::Polygon& polygon)
577  {
578  Vertices vertices;
579  for (size_t i = 0; i < polygon.points.size(); i++) {
580  Eigen::Vector3f v;
581  pointFromXYZToVector<geometry_msgs::Point32, Eigen::Vector3f>(
582  polygon.points[i], v);
583  vertices.push_back(v);
584  }
585  return Polygon::Ptr(new Polygon(vertices));
586  }
587 
588  std::vector<Polygon::Ptr> Polygon::fromROSMsg(const jsk_recognition_msgs::PolygonArray& msg,
589  const Eigen::Affine3f& offset)
590  {
591  std::vector<Polygon::Ptr> ret;
592  for (size_t i = 0; i < msg.polygons.size(); i++) {
593  Polygon::Ptr polygon = Polygon::fromROSMsgPtr(msg.polygons[i].polygon);
594  polygon->transformBy(offset);
595  ret.push_back(polygon);
596  }
597  return ret;
598  }
599 
600  bool Polygon::isInside(const Eigen::Vector3f& p)
601  {
602  if (isTriangle()) {
603  Eigen::Vector3f A = vertices_[0];
604  Eigen::Vector3f B = vertices_[1];
605  Eigen::Vector3f C = vertices_[2];
606  // Eigen::Vector3f cross0 = (A - C).cross(p - A);
607  // Eigen::Vector3f cross1 = (B - A).cross(p - B);
608  // Eigen::Vector3f cross2 = (C - B).cross(p - C);
609 
610  Eigen::Vector3f cross0 = (B - A).cross(p - A);
611  Eigen::Vector3f cross1 = (C - B).cross(p - B);
612  Eigen::Vector3f cross2 = (A - C).cross(p - C);
613  if (cross0.dot(cross1) >= 0 &&
614  cross1.dot(cross2) >= 0) {
615  return true;
616  }
617  else {
618  return false;
619  }
620  }
621  else {
622  std::vector<Polygon::Ptr> triangles = decomposeToTriangles();
623  for (size_t i = 0; i < triangles.size(); i++) {
624  if (triangles[i]->isInside(p)) {
625  return true;
626  }
627  }
628  return false;
629  }
630  }
631 
632 
633 }
d
Definition: setup.py:9
size_t nextIndex(size_t i)
Definition: polygon.cpp:554
std::vector< Plane::Ptr > convertToPlanes(std::vector< pcl::ModelCoefficients::Ptr >)
Definition: polygon.cpp:76
virtual Eigen::Vector3f directionAtPoint(size_t i)
Definition: polygon.cpp:142
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, returns false.
Definition: polygon.cpp:412
rest
std::vector< double > values
TFSIMD_FORCE_INLINE Vector3 cross(const Vector3 &v) const
virtual void transformBy(const Eigen::Affine3d &transform)
transform Polygon with given transform. cached triangles is cleared.
Definition: polygon.cpp:389
static Polygon createPolygonWithSkip(const Vertices &vertices)
Definition: polygon.cpp:46
double distance(const Eigen::Vector3f &point)
Compute distance between point and this polygon.
Definition: polygon.cpp:225
virtual Plane transform(const Eigen::Affine3d &transform)
Definition: plane.cpp:225
virtual bool isInside(const cv::Point &p) const
return true if point p is inside of field-of-view.
virtual cv::Mat image(const int type) const
return an image from internal camera parameter.
boost::shared_ptr< Polygon > Ptr
Definition: polygon.h:58
static Polygon fromROSMsg(const geometry_msgs::Polygon &polygon)
Definition: polygon.cpp:564
virtual Eigen::Affine3f coordinates()
Definition: plane.cpp:282
Class to represent 3-D straight line which has finite length.
Definition: segment.h:50
The ear clipping triangulation algorithm. The code is inspired by Flavien Brebion implementation...
virtual image_geometry::PinholeCameraModel getPinholeCameraModel() const
get instance of image_geometry::PinholeCameraModel.
virtual void initializeCoordinates()
Definition: plane.cpp:264
virtual void getLocalMinMax(double &min_x, double &min_y, double &max_x, double &max_y)
Definition: polygon.cpp:166
Eigen::Vector3f randomSampleLocalPoint(boost::mt19937 &random_generator)
Definition: polygon.cpp:185
virtual PtrPair separatePolygon(size_t index)
Definition: polygon.cpp:275
Plane(const std::vector< float > &coefficients)
Definition: plane.cpp:44
virtual bool isPossibleToRemoveTriangleAtIndex(size_t index, const Eigen::Vector3f &direction)
Definition: polygon.cpp:305
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.
Definition: polygon.cpp:442
TFSIMD_FORCE_INLINE Vector3 normalized() const
static Polygon::Ptr fromROSMsgPtr(const geometry_msgs::Polygon &polygon)
Definition: polygon.cpp:576
virtual Eigen::Vector3f nearestPoint(const Eigen::Vector3f &p, double &distance)
Compute nearest point from p on this polygon.
Definition: polygon.cpp:239
virtual PointIndexPair getNeighborIndex(size_t index)
Definition: polygon.cpp:121
virtual std::vector< float > toCoefficients()
Definition: plane.cpp:239
#define ROS_INFO(...)
virtual bool isInside(const Eigen::Vector3f &p)
return true if p is inside of polygon. p should be in global coordinates.
Definition: polygon.cpp:600
boost::tuple< size_t, size_t > PointIndexPair
Definition: types.h:50
virtual Eigen::Vector3f getNormalFromVertices()
Definition: polygon.cpp:533
void convertEigenAffine3(const Eigen::Affine3d &from, Eigen::Affine3f &to)
x
y
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
Definition: types.h:48
virtual void project(const Eigen::Vector3f &p, Eigen::Vector3f &output)
Definition: plane.cpp:171
size_t previousIndex(size_t i)
Definition: polygon.cpp:544
std::vector< Segment::Ptr > edges() const
get all the edges as point of Segment.
Definition: polygon.cpp:212
virtual size_t getFarestPointIndex(const Eigen::Vector3f &O)
Definition: polygon.cpp:106
p
virtual Eigen::Vector3f getVertex(size_t i)
Definition: polygon.cpp:271
boost::tuple< Ptr, Ptr > PtrPair
Definition: polygon.h:59
Eigen::Vector3f normal_
Definition: plane.h:83
std::vector< Polygon::Ptr > cached_triangles_
Definition: polygon.h:237
#define ROS_ERROR(...)
std::vector< cv::Point > project3DPointstoPixel(const image_geometry::PinholeCameraModel &model, const Vertices &vertices)
Project array of 3d point represented in Eigen::Vector3f to 2d point using model. ...
virtual std::vector< Polygon::Ptr > decomposeToTriangles()
Definition: polygon.cpp:482
virtual size_t getNumVertices()
Definition: polygon.cpp:267
Polygon(const Vertices &vertices)
Definition: polygon.cpp:87
double randomUniform(double min, double max, boost::mt19937 &gen)
Return a random value according to uniform distribution.
Definition: random_util.cpp:55
virtual Eigen::Vector3f centroid()
Definition: polygon.cpp:62


jsk_recognition_utils
Author(s):
autogenerated on Wed May 27 2020 03:59:48