convex_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 
40 
41 namespace jsk_recognition_utils
42 {
44  Polygon(vertices)
45  {
46 
47  }
48 
50  const std::vector<float>& coefficients):
51  Polygon(vertices, coefficients)
52  {
53 
54  }
55 
57  const Eigen::Vector3f& p, Eigen::Vector3f& output)
58  {
59  Plane::project(p, output);
60  }
61 
63  const Eigen::Affine3f& pose, Eigen::Affine3f& output)
64  {
65  Eigen::Vector3f p(pose.translation());
66  Eigen::Vector3f output_p;
67  projectOnPlane(p, output_p);
68  // ROS_INFO("[ConvexPolygon::projectOnPlane] p: [%f, %f, %f]",
69  // p[0], p[1], p[2]);
70  // ROS_INFO("[ConvexPolygon::projectOnPlane] output_p: [%f, %f, %f]",
71  // output_p[0], output_p[1], output_p[2]);
72  Eigen::Quaternionf rot;
73  rot.setFromTwoVectors(pose.rotation() * Eigen::Vector3f::UnitZ(),
74  coordinates().rotation() * Eigen::Vector3f::UnitZ());
75  Eigen::Quaternionf coords_rot(coordinates().rotation());
76  Eigen::Quaternionf pose_rot(pose.rotation());
77  // ROS_INFO("[ConvexPolygon::projectOnPlane] rot: [%f, %f, %f, %f]",
78  // rot.x(), rot.y(), rot.z(), rot.w());
79  // ROS_INFO("[ConvexPolygon::projectOnPlane] coords_rot: [%f, %f, %f, %f]",
80  // coords_rot.x(), coords_rot.y(), coords_rot.z(), coords_rot.w());
81  // ROS_INFO("[ConvexPolygon::projectOnPlane] pose_rot: [%f, %f, %f, %f]",
82  // pose_rot.x(), pose_rot.y(), pose_rot.z(), pose_rot.w());
83  // ROS_INFO("[ConvexPolygon::projectOnPlane] normal: [%f, %f, %f]", normal_[0], normal_[1], normal_[2]);
84  // Eigen::Affine3f::Identity() *
85  // output.translation() = Eigen::Translation3f(output_p);
86  // output.rotation() = rot * pose.rotation();
87  //output = Eigen::Translation3f(output_p) * rot * pose.rotation();
88  output = Eigen::Affine3f(rot * pose.rotation());
89  output.pretranslate(output_p);
90  // Eigen::Vector3f projected_point = output * Eigen::Vector3f(0, 0, 0);
91  // ROS_INFO("[ConvexPolygon::projectOnPlane] output: [%f, %f, %f]",
92  // projected_point[0], projected_point[1], projected_point[2]);
93  }
94 
96  {
97  Vertices new_vertices;
98  std::reverse_copy(vertices_.begin(), vertices_.end(),
99  std::back_inserter(new_vertices));
100  std::vector<float> reversed_coefficients(4);
101  reversed_coefficients[0] = - normal_[0];
102  reversed_coefficients[1] = - normal_[1];
103  reversed_coefficients[2] = - normal_[2];
104  reversed_coefficients[3] = - d_;
105 
106  return ConvexPolygon(new_vertices, reversed_coefficients);
107  }
108 
109  void ConvexPolygon::project(const Eigen::Vector3f& p, Eigen::Vector3f& output)
110  {
111  Eigen::Vector3f point_on_plane;
112  Plane::project(p, point_on_plane);
113  // check point_ is inside or not
114  if (isInside(point_on_plane)) {
115  output = point_on_plane;
116  }
117  else {
118  // find the minimum foot point
119  double min_distance = DBL_MAX;
120  Eigen::Vector3f min_point;
121  for (size_t i = 0; i < vertices_.size() - 1; i++) {
122  Segment seg(vertices_[i], vertices_[i + 1]);
123  Eigen::Vector3f foot;
124  double distance = seg.distanceToPoint(p, foot);
125  if (distance < min_distance) {
126  min_distance = distance;
127  min_point = foot;
128  }
129  }
130  output = min_point;
131  }
132  }
133 
134  void ConvexPolygon::project(const Eigen::Vector3d& p, Eigen::Vector3d& output)
135  {
136  Eigen::Vector3f output_f;
137  Eigen::Vector3f p_f(p[0], p[1], p[2]);
138  project(p_f, output_f);
139  pointFromVectorToVector<Eigen::Vector3f, Eigen::Vector3d>(output_f, output);
140  }
141 
142  void ConvexPolygon::project(const Eigen::Vector3d& p, Eigen::Vector3f& output)
143  {
144  Eigen::Vector3f p_f(p[0], p[1], p[2]);
145  project(p_f, output);
146  }
147 
148  void ConvexPolygon::project(const Eigen::Vector3f& p, Eigen::Vector3d& output)
149  {
150  Eigen::Vector3f output_f;
151  project(p, output_f);
152  pointFromVectorToVector<Eigen::Vector3f, Eigen::Vector3d>(output_f, output);
153  }
154 
155 
156  Eigen::Vector3f ConvexPolygon::getCentroid()
157  {
158  Eigen::Vector3f ret(0, 0, 0);
159  for (size_t i = 0; i < vertices_.size(); i++) {
160  ret = ret + vertices_[i];
161  }
162  return ret / vertices_.size();
163  }
164 
165  ConvexPolygon ConvexPolygon::fromROSMsg(const geometry_msgs::Polygon& polygon)
166  {
167  Vertices vertices;
168  for (size_t i = 0; i < polygon.points.size(); i++) {
169  Eigen::Vector3f p;
170  pointFromXYZToVector<geometry_msgs::Point32, Eigen::Vector3f>(
171  polygon.points[i], p);
172  vertices.push_back(p);
173  }
174  return ConvexPolygon(vertices);
175  }
176 
177  ConvexPolygon::Ptr ConvexPolygon::fromROSMsgPtr(const geometry_msgs::Polygon& polygon)
178  {
179  Vertices vertices;
180  for (size_t i = 0; i < polygon.points.size(); i++) {
181  Eigen::Vector3f p;
182  pointFromXYZToVector<geometry_msgs::Point32, Eigen::Vector3f>(
183  polygon.points[i], p);
184  vertices.push_back(p);
185  }
186  return ConvexPolygon::Ptr(new ConvexPolygon(vertices));
187  }
188 
189  bool ConvexPolygon::distanceSmallerThan(const Eigen::Vector3f& p,
190  double distance_threshold)
191  {
192  double dummy_distance;
193  return distanceSmallerThan(p, distance_threshold, dummy_distance);
194  }
195 
196  bool ConvexPolygon::distanceSmallerThan(const Eigen::Vector3f& p,
197  double distance_threshold,
198  double& output_distance)
199  {
200  // first check distance as Plane rather than Convex
201  double plane_distance = distanceToPoint(p);
202  if (plane_distance > distance_threshold) {
203  output_distance = plane_distance;
204  return false;
205  }
206 
207  Eigen::Vector3f foot_point;
208  project(p, foot_point);
209  double convex_distance = (p - foot_point).norm();
210  output_distance = convex_distance;
211  return convex_distance < distance_threshold;
212  }
213 
215  {
216  for (size_t i = 0; i < vertices_.size(); i++) {
217  Eigen::Vector3f p_k = vertices_[i];
218  Eigen::Vector3f p_k_1;
219  if (i == vertices_.size() - 1) {
220  p_k_1 = vertices_[0];
221  }
222  else {
223  p_k_1 = vertices_[i + 1];
224  }
225  if ((p_k - p_k_1).norm() < thr) {
226  return false;
227  }
228  }
229  return true;
230  }
231 
232  double ConvexPolygon::distanceFromVertices(const Eigen::Vector3f& p)
233  {
234  double min_distance = DBL_MAX;
235  for (size_t i = 0; i < vertices_.size(); i++) {
236  Eigen::Vector3f v = vertices_[i];
237  double d = (p - v).norm();
238  if (d < min_distance) {
239  min_distance = d;
240  }
241  }
242  return min_distance;
243  }
244 
246  {
247  // compute centroid
248  Eigen::Vector3f c = centroid();
249  Vertices new_vertices(vertices_.size());
250  for (size_t i = 0; i < vertices_.size(); i++) {
251  new_vertices[i] = (vertices_[i] - c).normalized() * distance + vertices_[i];
252  // ROS_INFO("old v: [%f, %f, %f]", vertices_[i][0], vertices_[i][1], vertices_[i][2]);
253  // ROS_INFO("new v: [%f, %f, %f]", new_vertices[i][0], new_vertices[i][1], new_vertices[i][2]);
254  // ROS_INFO("");
255  }
256 
257  ConvexPolygon::Ptr ret (new ConvexPolygon(new_vertices));
258  return ret;
259  }
260 
261  ConvexPolygon::Ptr ConvexPolygon::magnify(const double scale_factor)
262  {
263  // compute centroid
264  Eigen::Vector3f c = centroid();
265  Vertices new_vertices;
266  for (size_t i = 0; i < vertices_.size(); i++) {
267  new_vertices.push_back((vertices_[i] - c) * scale_factor + c);
268  }
269  ConvexPolygon::Ptr ret (new ConvexPolygon(new_vertices));
270  return ret;
271  }
272 
273  geometry_msgs::Polygon ConvexPolygon::toROSMsg()
274  {
275  geometry_msgs::Polygon polygon;
276  for (size_t i = 0; i < vertices_.size(); i++) {
277  geometry_msgs::Point32 ros_point;
278  ros_point.x = vertices_[i][0];
279  ros_point.y = vertices_[i][1];
280  ros_point.z = vertices_[i][2];
281  polygon.points.push_back(ros_point);
282  }
283  return polygon;
284  }
285 
286 
287  bool ConvexPolygon::isProjectableInside(const Eigen::Vector3f& p)
288  {
289  Eigen::Vector3f foot_point;
290  Plane::project(p, foot_point);
291  return isInside(foot_point);
292  }
293 
294 }
d
Definition: setup.py:9
bool distanceSmallerThan(const Eigen::Vector3f &p, double distance_threshold)
static ConvexPolygon fromROSMsg(const geometry_msgs::Polygon &polygon)
virtual ConvexPolygon flipConvex()
virtual Ptr magnify(const double scale_factor)
double distance(const Eigen::Vector3f &point)
Compute distance between point and this polygon.
Definition: polygon.cpp:225
boost::shared_ptr< ConvexPolygon > Ptr
virtual void project(const Eigen::Vector3f &p, Eigen::Vector3f &output)
virtual Eigen::Affine3f coordinates()
Definition: plane.cpp:282
Class to represent 3-D straight line which has finite length.
Definition: segment.h:50
static ConvexPolygon::Ptr fromROSMsgPtr(const geometry_msgs::Polygon &polygon)
virtual Ptr magnifyByDistance(const double distance)
virtual Eigen::Vector3f getCentroid()
TFSIMD_FORCE_INLINE Vector3 normalized() const
double distanceFromVertices(const Eigen::Vector3f &p)
virtual void projectOnPlane(const Eigen::Vector3f &p, Eigen::Vector3f &output)
ConvexPolygon(const Vertices &vertices)
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
virtual double distanceToPoint(const Eigen::Vector3f &from) const
compute a distance to a point
Definition: line.cpp:82
virtual void project(const Eigen::Vector3f &p, Eigen::Vector3f &output)
Definition: plane.cpp:171
virtual bool isProjectableInside(const Eigen::Vector3f &p)
p
Eigen::Vector3f normal_
Definition: plane.h:83
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
virtual double distanceToPoint(const Eigen::Vector4f p)
Definition: plane.cpp:122
virtual Eigen::Vector3f centroid()
Definition: polygon.cpp:62


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