grid_plane.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
39 #include <boost/tuple/tuple_comparison.hpp>
40 
41 namespace jsk_recognition_utils
42 {
43  GridPlane::GridPlane(ConvexPolygon::Ptr plane, const double resolution):
44  convex_(plane), resolution_(resolution)
45  {
46 
47  }
48 
50  {
51 
52  }
53 
55  const Eigen::Vector3f& p)
56  {
57  double offset_x = p[0] + 0.5 * resolution_;
58  double offset_y = p[1] + 0.5 * resolution_;
59  // return boost::make_tuple<int, int>(std::floor(offset_x / resolution_),
60  // std::floor(offset_y / resolution_));
61  return boost::make_tuple<int, int>(boost::math::round(p[0] / resolution_),
62  boost::math::round(p[1] / resolution_));
63  }
64 
66  {
67  cells_.insert(pair);
68  }
69 
71  {
73  for (std::set<IndexPair>::iterator it = cells_.begin();
74  it != cells_.end();
75  ++it) {
76  IndexPair the_index = *it;
77  for (int xi = - num; xi <= num; xi++) {
78  for (int yi = - num; yi <= num; yi++) {
79  if (abs(xi) + abs(yi) <= num) {
80  IndexPair new_pair = boost::make_tuple<int, int>(
81  the_index.get<0>() + xi,
82  the_index.get<1>() + yi);
83  ret->cells_.insert(new_pair);
84  }
85  }
86  }
87  }
88  return ret;
89  }
90 
92  {
94  for (std::set<IndexPair>::iterator it = cells_.begin();
95  it != cells_.end();
96  ++it) {
97  IndexPair the_index = *it;
98  bool should_removed = false;
99  for (int xi = - num; xi <= num; xi++) {
100  for (int yi = - num; yi <= num; yi++) {
101  if (abs(xi) + abs(yi) <= num) {
102  IndexPair check_pair = boost::make_tuple<int, int>(
103  the_index.get<0>() + xi,
104  the_index.get<1>() + yi);
105  if (!isOccupied(check_pair)) {
106  should_removed = true;
107  }
108  }
109  }
110  }
111  if (!should_removed) {
112  ret->cells_.insert(the_index);
113  }
114  }
115  return ret;
116  }
117 
119  {
120  bool result = cells_.find(pair) != cells_.end();
121  // Verbosing for debug
122  // ROS_INFO("Checking index pair (%d, %d)", pair.get<0>(), pair.get<1>());
123  // ROS_INFO("Result: %d", result);
124  // ROS_INFO("cells are:");
125  // for (IndexPairSet::iterator it = cells_.begin();
126  // it != cells_.end();
127  // ++it) {
128  // ROS_INFO(" (%d, %d)", it->get<0>(), it->get<1>());
129  // }
130  return result;
131  }
132 
133  bool GridPlane::isOccupied(const Eigen::Vector3f& p)
134  {
136  return isOccupied(pair);
137  }
138 
139  bool GridPlane::isOccupiedGlobal(const Eigen::Vector3f& p)
140  {
141  return isOccupied(convex_->coordinates().inverse() * p);
142  }
143 
145  {
147  ret->cells_ = cells_;
148  return ret;
149  }
150 
152  const IndexPair& pair)
153  {
154  return Eigen::Vector3f(pair.get<0>() * resolution_,
155  pair.get<1>() * resolution_,
156  0);
157  }
158 
160  const IndexPair& pair)
161  {
162  Eigen::Vector3f local_point = unprojectIndexPairAsLocalPoint(pair);
163  return convex_->coordinates() * local_point;
164  }
165 
167  {
168  ConvexPolygon::Ptr intersect_polygon = cube.intersectConvexPolygon(*convex_);
169  // 1. transform vertices into local coordinates
170  // 2. compute min-max
171  // 3. compute candidates
172  // 4. filter candidates
173 
174  // 1. transform vertices into local coordinates
175  Vertices local_vertices;
176  Vertices global_vertices = intersect_polygon->getVertices();
177  Eigen::Affine3f inv_coords = convex_->coordinates().inverse();
178  for (size_t i = 0; i < global_vertices.size(); i++) {
179  local_vertices.push_back(inv_coords * global_vertices[i]);
180  }
181 
182  // 2. compute min-max
183  double min_x = DBL_MAX;
184  double min_y = DBL_MAX;
185  double max_x = - DBL_MAX;
186  double max_y = - DBL_MAX;
187  for (size_t i = 0; i < local_vertices.size(); i++) {
188  min_x = ::fmin(min_x, local_vertices[i][0]);
189  min_y = ::fmin(min_y, local_vertices[i][1]);
190  max_x = ::fmax(max_x, local_vertices[i][0]);
191  max_y = ::fmax(max_y, local_vertices[i][1]);
192  }
193  // ROS_INFO("x: [%f~%f]", min_x, max_x);
194  // ROS_INFO("y: [%f~%f]", min_y, max_y);
195  // 3. compute candidates
196  std::vector<Polygon::Ptr> triangles
197  = intersect_polygon->decomposeToTriangles();
198  for (double x = min_x; x <= max_x; x += resolution_) {
199  for (double y = min_y; y <= max_y; y += resolution_) {
200  Eigen::Vector3f local_p(x, y, 0);
201  Eigen::Vector3f p = convex_->coordinates() * local_p;
202  // 4. filter candidates
203  bool insidep = false;
204  for (size_t i = 0; i < triangles.size(); i++) {
205  if (triangles[i]->isInside(p)) {
206  insidep = true;
207  break;
208  }
209  }
210  if (insidep) {
211  IndexPair pair = projectLocalPointAsIndexPair(local_p);
212  addIndexPair(pair);
213  }
214  }
215  }
216  }
217 
219  pcl::PointCloud<pcl::PointNormal>::Ptr& cloud,
220  double distance_threshold,
221  std::set<int>& non_plane_indices)
222  {
224  cloud, distance_threshold, M_PI / 2.0, non_plane_indices);
225  }
226 
228  pcl::PointCloud<pcl::PointNormal>::Ptr& cloud,
229  double distance_threshold,
230  double normal_threshold,
231  std::set<int>& non_plane_indices)
232  {
233  Eigen::Affine3f local_coordinates = convex_->coordinates();
234  Eigen::Affine3f inv_local_coordinates = local_coordinates.inverse();
235  //std::vector<Polygon::Ptr> triangles = convex_->decomposeToTriangles();
236 
237  pcl::ExtractPolygonalPrismData<pcl::PointNormal> prism_extract;
238  pcl::PointCloud<pcl::PointNormal>::Ptr
239  hull_cloud (new pcl::PointCloud<pcl::PointNormal>);
240  pcl::PointCloud<pcl::PointNormal>::Ptr
241  hull_output (new pcl::PointCloud<pcl::PointNormal>);
242  pcl::PointCloud<pcl::PointNormal>::Ptr
243  rehull_cloud (new pcl::PointCloud<pcl::PointNormal>);
244  convex_->boundariesToPointCloud<pcl::PointNormal>(*hull_cloud);
245  // pcl::ConvexHull<pcl::PointNormal> chull;
246  // chull.setDimension(2);
247  // chull.setInputCloud (hull_cloud);
248  // chull.reconstruct(*hull_output);
249 
250  // it's important to make it sure to close the loop of
251  // convex hull
252  hull_cloud->points.push_back(hull_cloud->points[0]);
253 
254  prism_extract.setInputCloud(cloud);
255  prism_extract.setHeightLimits(-distance_threshold, distance_threshold);
256  prism_extract.setInputPlanarHull(hull_cloud);
257  //prism_extract.setInputPlanarHull(hull_output);
258  // output_indices is set of indices which are on plane
259  pcl::PointIndices output_indices;
260  prism_extract.segment(output_indices);
261  std::set<int> output_set(output_indices.indices.begin(),
262  output_indices.indices.end());
263  Eigen::Vector3f n = convex_->getNormal();
264  for (size_t i = 0; i < cloud->points.size(); i++) {
265  if (output_set.find(i) != output_set.end()) {
266  // check normal
267  pcl::PointNormal p = cloud->points[i];
268  Eigen::Vector3f n_p = p.getNormalVector3fMap();
269  if (std::abs(n.dot(n_p)) > cos(normal_threshold)) {
270  non_plane_indices.insert(i);
271  }
272  }
273  }
274 
275 
276  for (size_t i = 0; i < output_indices.indices.size(); i++) {
277  //for (size_t i = 0; i < cloud->points.size(); i++) {
278  int point_index = output_indices.indices[i];
279  pcl::PointNormal p = cloud->points[point_index];
280  Eigen::Vector3f ep = p.getVector3fMap();
281  Eigen::Vector3f local_ep = inv_local_coordinates * ep;
282  IndexPair pair = projectLocalPointAsIndexPair(local_ep);
283  addIndexPair(pair);
284  }
285  return output_indices.indices.size();
286  }
287 
289  pcl::PointCloud<pcl::PointNormal>::Ptr& cloud,
290  double distance_threshold)
291  {
292  std::set<int> dummy;
293  return fillCellsFromPointCloud(cloud, distance_threshold, dummy);
294  }
295 
296  jsk_recognition_msgs::SimpleOccupancyGrid GridPlane::toROSMsg()
297  {
298  jsk_recognition_msgs::SimpleOccupancyGrid ros_msg;
299  std::vector<float> coeff;
300  convex_->toCoefficients(coeff);
301  //ROS_INFO("coef: [%f, %f, %f, %f]", coeff[0], coeff[1], coeff[2], coeff[3]);
302  ros_msg.coefficients[0] = coeff[0];
303  ros_msg.coefficients[1] = coeff[1];
304  ros_msg.coefficients[2] = coeff[2];
305  ros_msg.coefficients[3] = coeff[3];
306  ros_msg.resolution = resolution_;
307  for (std::set<IndexPair>::iterator it = cells_.begin();
308  it != cells_.end();
309  ++it) {
310  IndexPair pair = *it;
311  Eigen::Vector3f c = unprojectIndexPairAsLocalPoint(pair);
312  geometry_msgs::Point p;
313  pointFromVectorToXYZ<Eigen::Vector3f, geometry_msgs::Point>(
314  c, p);
315  ros_msg.cells.push_back(p);
316  }
317  return ros_msg;
318  }
319 
321  const jsk_recognition_msgs::SimpleOccupancyGrid& rosmsg,
322  const Eigen::Affine3f& offset = Eigen::Affine3f::Identity())
323  {
324  boost::mutex::scoped_lock lock(global_chull_mutex);
325  Plane plane = Plane(rosmsg.coefficients).transform(offset);
326  // ROS_INFO("[GridPlane::fromROSMsg] c: [%f, %f, %f, %f]",
327  // rosmsg.coefficients[0],
328  // rosmsg.coefficients[1],
329  // rosmsg.coefficients[2],
330  // rosmsg.coefficients[3]);
331  // ROS_INFO("[GridPlane::fromROSMsg] transformed c: [%f, %f, %f, %f]",
332  // plane.toCoefficients()[0],
333  // plane.toCoefficients()[1],
334  // plane.toCoefficients()[2],
335  // plane.toCoefficients()[3]);
336  Eigen::Affine3f plane_coords = plane.coordinates();
337  Eigen::Vector3f plane_origin(plane_coords.translation());
338  // ROS_INFO_EIGEN_VECTOR3("[GridPlane::fromROSMsg] plane_origin",
339  // plane_origin);
340  pcl::PointCloud<pcl::PointNormal>::Ptr
341  vertices (new pcl::PointCloud<pcl::PointNormal>);
342  for (size_t i = 0; i < rosmsg.cells.size(); i++) {
343  Eigen::Vector3f local_p(rosmsg.cells[i].x, rosmsg.cells[i].y, 0);
344  Eigen::Vector3f global_p = plane.coordinates() * local_p;
345  pcl::PointNormal p;
346  p.x = global_p[0];
347  p.y = global_p[1];
348  p.z = global_p[2];
349  // ROS_INFO("[%f, %f, %f] => [%f, %f, %f]",
350  // local_p[0], local_p[1], local_p[2],
351  // global_p[0], global_p[1], global_p[2]);
352  vertices->points.push_back(p);
353  }
354  pcl::ConvexHull<pcl::PointNormal> chull;
355  //chull.setDimension(2);
356  chull.setInputCloud (vertices);
357  pcl::PointCloud<pcl::PointNormal>::Ptr
358  convex_vertices_cloud (new pcl::PointCloud<pcl::PointNormal>);
359  chull.reconstruct (*convex_vertices_cloud);
360 
361  Vertices convex_vertices
362  = pointCloudToVertices<pcl::PointNormal>(*convex_vertices_cloud);
363  ConvexPolygon::Ptr convex(new ConvexPolygon(convex_vertices));
364  // Check orientation
365  if (!convex->isSameDirection(plane)) {
366  // ROS_INFO("[GridPlane::fromROSMsg] flip convex");
367  //convex = boost::make_shared<ConvexPolygon>(convex->flipConvex());
368  Vertices reversed_convex_vertices;
369  std::reverse_copy(convex_vertices.begin(), convex_vertices.end(),
370  std::back_inserter(reversed_convex_vertices));
371  convex.reset(new ConvexPolygon(reversed_convex_vertices));
372  }
373  Eigen::Vector3f convex_origin(convex->coordinates().translation());
374  Eigen::Vector3f convex_normal = convex->getNormal();
375  // ROS_INFO_EIGEN_VECTOR3("[GridPlane::fromROSMsg] convex_origin",
376  // convex_origin);
377  // ROS_INFO_EIGEN_VECTOR3("[GridPlane::fromROSMsg] convex_normal",
378  // convex_normal);
379  GridPlane ret(convex, rosmsg.resolution);
380  //ROS_INFO("resolution: %f", ret.resolution_);
381  ret.fillCellsFromPointCloud(vertices, 1000.0);
382  // ROS_INFO("cell size: %lu", ret.cells_.size());
383  // ROS_INFO("original cell size: %lu", rosmsg.cells.size());
384  return ret;
385  }
386 
387 }
GridPlane(ConvexPolygon::Ptr plane, const double resolution)
Definition: grid_plane.cpp:43
virtual IndexPair projectLocalPointAsIndexPair(const Eigen::Vector3f &p)
Project 3-D point to GridPlane::IndexPair. p should be represented in local coordinates.
Definition: grid_plane.cpp:54
virtual GridPlane::Ptr erode(int num)
Erode grid cells with specified number of pixels.
Definition: grid_plane.cpp:91
virtual Eigen::Vector3f unprojectIndexPairAsLocalPoint(const IndexPair &pair)
Unproject GridPlane::IndexPair to 3-D local point.
Definition: grid_plane.cpp:151
boost::mutex global_chull_mutex
Definition: pcl_util.cpp:43
virtual Plane transform(const Eigen::Affine3d &transform)
Definition: plane.cpp:225
virtual size_t fillCellsFromPointCloud(pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, double distance_threshold)
Definition: grid_plane.cpp:288
virtual GridPlane::Ptr clone()
Definition: grid_plane.cpp:144
virtual Eigen::Affine3f coordinates()
Definition: plane.cpp:282
ConvexPolygon::Ptr intersectConvexPolygon(Plane &plane)
Definition: cube.cpp:174
virtual Eigen::Vector3f unprojectIndexPairAsGlobalPoint(const IndexPair &pair)
Unproject GridPlane::IndexPair to 3-D global point.
Definition: grid_plane.cpp:159
virtual bool isOccupied(const IndexPair &pair)
Definition: grid_plane.cpp:118
Grid based representation of planar region.
Definition: grid_plane.h:73
x
y
virtual bool isOccupiedGlobal(const Eigen::Vector3f &p)
p should be global coordinate
Definition: grid_plane.cpp:139
static GridPlane fromROSMsg(const jsk_recognition_msgs::SimpleOccupancyGrid &rosmsg, const Eigen::Affine3f &offset)
Construct GridPlane object from jsk_recognition_msgs::SimpleOccupancyGrid.
Definition: grid_plane.cpp:320
virtual void fillCellsFromCube(Cube &cube)
Definition: grid_plane.cpp:166
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
Definition: types.h:48
ConvexPolygon::Ptr convex_
Definition: grid_plane.h:162
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
p
virtual void addIndexPair(IndexPair pair)
Add IndexPair to this instance.
Definition: grid_plane.cpp:65
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
virtual jsk_recognition_msgs::SimpleOccupancyGrid toROSMsg()
Definition: grid_plane.cpp:296
boost::tuple< int, int > IndexPair
Definition: grid_plane.h:77
virtual GridPlane::Ptr dilate(int num)
Dilate grid cells with specified number of pixels.
Definition: grid_plane.cpp:70


jsk_recognition_utils
Author(s):
autogenerated on Fri Dec 6 2019 04:02:51