grid_map.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 #define BOOST_PARAMETER_MAX_ARITY 7
37 #include <boost/make_shared.hpp>
38 #include <Eigen/Core>
41 #include <nodelet/nodelet.h>
43 #include <pcl/surface/convex_hull.h>
45 //#define DEBUG_GRID_MAP
46 
47 namespace jsk_recognition_utils
48 {
49  GridMap::GridMap(double resolution, const std::vector<float>& coefficients):
50  resolution_(resolution), vote_(0)
51  {
52  normal_[0] = -coefficients[0];
53  normal_[1] = -coefficients[1];
54  normal_[2] = -coefficients[2];
55  d_ = -coefficients[3];
56  if (normal_.norm() != 1.0) {
57  d_ = d_ / normal_.norm();
58  normal_.normalize();
59  }
60  O_ = - d_ * normal_;
61  // decide ex_ and ey_
62  Eigen::Vector3f u(1, 0, 0);
63  if (normal_ == u) {
64  u[0] = 0; u[1] = 1; u[2] = 0;
65  }
66  ey_ = normal_.cross(u).normalized();
67  ex_ = ey_.cross(normal_).normalized();
68  }
69 
71  {
72 
73  }
74 
75  GridIndex::Ptr GridMap::registerIndex(const int x, const int y)
76  {
77  ColumnIterator it = data_.find(x);
78  if (it != data_.end()) {
79  (it->second).insert(y);
80  }
81  else {
82  RowIndices new_row;
83  new_row.insert(y);
84  data_[x] = new_row;
85  }
86  GridIndex::Ptr ret(new GridIndex(x, y));
87  return ret;
88  }
89 
91  {
92  return registerIndex(index->x, index->y);
93  }
94 
95  void GridMap::registerPoint(const pcl::PointXYZRGB& point)
96  {
97  GridIndex::Ptr index (new GridIndex());
98  pointToIndex(point, index);
99  // check duplication
100  registerIndex(index);
101  }
102 
103  std::vector<GridIndex::Ptr> GridMap::registerLine(const pcl::PointXYZRGB& from,
104  const pcl::PointXYZRGB& to)
105  {
106 #ifdef DEBUG_GRID_MAP
107  std::cout << "newline" << std::endl;
108 #endif
109  std::vector<GridIndex::Ptr> added_indices;
110  //GridLine::Ptr new_line (new GridLine(from, to));
111  //lines_.push_back(new_line);
112  // count up all the grids which the line penetrates
113 
114  // 1. convert to y = ax + b style equation.
115  // 2. move x from the start index to the end index and count up the y range
116  // if it cannot be convert to y = ax + b style, it means the equation
117  // is represented as x = c style.
118  double from_x = from.getVector3fMap().dot(ex_) / resolution_;
119  double from_y = from.getVector3fMap().dot(ey_) / resolution_;
120  double to_x = to.getVector3fMap().dot(ex_) / resolution_;
121  double to_y = to.getVector3fMap().dot(ey_) / resolution_;
122 #ifdef DEBUG_GRID_MAP
123  std::cout << "registering (" << (int)from_x << ", " << (int)from_y << ")" << std::endl;
124  std::cout << "registering (" << (int)to_x << ", " << (int)to_y << ")" << std::endl;
125 #endif
126  added_indices.push_back(registerIndex(from_x, from_y));
127  added_indices.push_back(registerIndex(to_x, to_y));
128  if (from_x != to_x) {
129  double a = (to_y - from_y) / (to_x - from_x);
130  double b = - a * from_x + from_y;
131 #ifdef DEBUG_GRID_MAP
132  std::cout << "a: " << a << std::endl;
133 #endif
134  if (a == 0.0) {
135 #ifdef DEBUG_GRID_MAP
136  std::cout << "parallel to x" << std::endl;
137 #endif
138  int from_int_x = (int)from_x;
139  int to_int_x = (int)to_x;
140  int int_y = (int)from_y;
141  if (from_int_x > to_int_x) {
142  std::swap(from_int_x, to_int_x);
143  }
144  for (int ix = from_int_x; ix < to_int_x; ++ix) {
145  added_indices.push_back(registerIndex(ix, int_y));
146 #ifdef DEBUG_GRID_MAP
147  std::cout << "registering (" << ix << ", " << int_y << ")" << std::endl;
148 #endif
149  }
150  }
151  else if (fabs(a) < 1.0) {
152 #ifdef DEBUG_GRID_MAP
153  std::cout << "based on x" << std::endl;
154 #endif
155  int from_int_x = (int)from_x;
156  int to_int_x = (int)to_x;
157  if (from_int_x > to_int_x) {
158  std::swap(from_int_x, to_int_x);
159  }
160 
161  for (int ix = from_int_x; ix < to_int_x; ++ix) {
162  double y = a * ix + b;
163  added_indices.push_back(registerIndex(ix, (int)y));
164 #ifdef DEBUG_GRID_MAP
165  std::cout << "registering (" << ix << ", " << (int)y << ")" << std::endl;
166 #endif
167  }
168  }
169  else {
170 #ifdef DEBUG_GRID_MAP
171  std::cout << "based on y" << std::endl;
172 #endif
173  int from_int_y = (int)from_y;
174  int to_int_y = (int)to_y;
175  if (from_int_y > to_int_y) {
176  std::swap(from_int_y, to_int_y);
177  }
178 
179  for (int iy = from_int_y; iy < to_int_y; ++iy) {
180  double x = iy / a - b / a;
181  added_indices.push_back(registerIndex((int)x, iy));
182 #ifdef DEBUG_GRID_MAP
183  std::cout << "registering (" << (int)x << ", " << iy << ")" << std::endl;
184 #endif
185  }
186  }
187 
188  }
189  else {
190 #ifdef DEBUG_GRID_MAP
191  std::cout << "parallel to y" << std::endl;
192 #endif
193  // the line is parallel to y
194  int from_int_y = (int)from_y;
195  int to_int_y = (int)to_y;
196  int int_x = (int)from_x;
197  if (from_int_y > to_int_y) {
198  std::swap(from_int_y, to_int_y);
199  }
200  for (int iy = from_int_y; iy < to_int_y; ++iy) {
201  added_indices.push_back(registerIndex(int_x, iy));
202 #ifdef DEBUG_GRID_MAP
203  std::cout << "registering (" << int_x << ", " << (int)iy << ")" << std::endl;
204 #endif
205  }
206  }
207  return added_indices;
208  }
209 
210  void GridMap::registerPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
211  {
212  for (size_t i = 0; i < cloud->points.size(); i++) {
213  registerPoint(cloud->points[i]);
214  //ROS_INFO("registered point: [%f, %f, %f]", cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
215  }
216  }
217 
218  void GridMap::pointToIndex(const pcl::PointXYZRGB& point, GridIndex::Ptr index)
219  {
220  pointToIndex(point.getVector3fMap(), index);
221  }
222 
223  void GridMap::pointToIndex(const Eigen::Vector3f& p, GridIndex::Ptr index)
224  {
225  index->x = (p - O_).dot(ex_) / resolution_;
226  index->y = (p - O_).dot(ey_) / resolution_;
227  }
228 
229  void GridMap::gridToPoint(GridIndex::Ptr index, Eigen::Vector3f& pos)
230  {
231  gridToPoint(*index, pos);
232  }
233 
234  void GridMap::gridToPoint(const GridIndex& index, Eigen::Vector3f& pos)
235  {
236  //pos = resolution_ * (index.x * ex_ + index.y * ey_) + O_;
237  pos = resolution_ * ((index.x + 0.5) * ex_ + (index.y + 0.5) * ey_) + O_;
238  }
239 
240  void GridMap::gridToPoint2(const GridIndex& index, Eigen::Vector3f& pos)
241  {
242  //pos = resolution_ * ((index.x - 0.5) * ex_ + (index.y - 0.5) * ey_) + O_;
243  pos = resolution_ * ((index.x - 0.0) * ex_ + (index.y - 0.0) * ey_) + O_;
244  }
245 
246 
247  bool GridMap::getValue(const int x, const int y)
248  {
249  // check line
250  // for (size_t i = 0; i < lines_.size(); i++) {
251  // GridLine::Ptr line = lines_[i];
252  // Eigen::Vector3f A, B, C, D;
253  // gridToPoint2(GridIndex(x, y), A);
254  // gridToPoint2(GridIndex(x + 1, y), B);
255  // gridToPoint2(GridIndex(x + 1, y + 1), C);
256  // gridToPoint2(GridIndex(x, y + 1), D);
257  // bool penetrate = line->penetrateGrid(A, B, C, D);
258  // if (penetrate) {
259  // // // printf("(%lf, %lf, %lf) - (%lf, %lf, %lf) penetrate (%d, %d)\n",
260  // // // line->from[0],line->from[1],line->from[2],
261  // // // line->to[0],line->to[1],line->to[2],
262  // // // x, y);
263  // // //std::cout << "penetrate"
264  // return true;
265  // }
266  // }
267 
268  ColumnIterator it = data_.find(x);
269  if (it == data_.end()) {
270  return false;
271  }
272  else {
273  RowIndices c = it->second;
274  if (c.find(y) == c.end()) {
275  return false;
276  }
277  else {
278  return true;
279  }
280  }
281  }
282 
283  bool GridMap::getValue(const GridIndex& index)
284  {
285  return getValue(index.x, index.y);
286  }
287 
289  {
290  return getValue(*index);
291  }
292 
293  void GridMap::fillRegion(const GridIndex::Ptr start, std::vector<GridIndex::Ptr>& output)
294  {
295 #ifdef DEBUG_GRID_MAP
296  std::cout << "filling " << start->x << ", " << start->y << std::endl;
297 #endif
298  output.push_back(start);
299  registerIndex(start);
300 #ifdef DEBUG_GRID_MAP
301  if (abs(start->x) > 100 || abs(start->y) > 100) {
302  //exit(1);
303  std::cout << "force to quit" << std::endl;
304  for (size_t i = 0; i < lines_.size(); i++) {
305  GridLine::Ptr line = lines_[i];
306  Eigen::Vector3f from = line->from;
307  Eigen::Vector3f to = line->to;
308 #ifdef DEBUG_GRID_MAP
309  std::cout << "line[" << i << "]: "
310  << "[" << from[0] << ", " << from[1] << ", " << from[2] << "] -- "
311  << "[" << to[0] << ", " << to[1] << ", " << to[2] << std::endl;
312 #endif
313  }
314  return; // force to quit
315  }
316 #endif
317  GridIndex U(start->x, start->y + 1),
318  D(start->x, start->y - 1),
319  R(start->x + 1, start->y),
320  L(start->x - 1, start->y);
321 
322  if (!getValue(U)) {
323  fillRegion(boost::make_shared<GridIndex>(U), output);
324  }
325  if (!getValue(L)) {
326  fillRegion(boost::make_shared<GridIndex>(L), output);
327  }
328  if (!getValue(R)) {
329  fillRegion(boost::make_shared<GridIndex>(R), output);
330  }
331  if (!getValue(D)) {
332  fillRegion(boost::make_shared<GridIndex>(D), output);
333  }
334 
335  }
336 
337  void GridMap::fillRegion(const Eigen::Vector3f& start, std::vector<GridIndex::Ptr>& output)
338  {
339  GridIndex::Ptr start_index (new GridIndex);
340  pointToIndex(start, start_index);
341  fillRegion(start_index, output);
342  }
343 
344  void GridMap::indicesToPointCloud(const std::vector<GridIndex::Ptr>& indices,
345  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
346  {
347  for (size_t i = 0; i < indices.size(); i++) {
348  GridIndex::Ptr index = indices[i];
349  Eigen::Vector3f point;
350  pcl::PointXYZRGB new_point;
351  gridToPoint(index, point);
352  new_point.x = point[0];
353  new_point.y = point[1];
354  new_point.z = point[2];
355  cloud->points.push_back(new_point);
356  }
357  }
358 
359  void GridMap::originPose(Eigen::Affine3f& output)
360  {
361  Eigen::Matrix3f rot_mat;
362  rot_mat.col(0) = Eigen::Vector3f(ex_[0], ex_[1], ex_[2]);
363  rot_mat.col(1) = Eigen::Vector3f(ey_[0], ey_[1], ey_[2]);
364  rot_mat.col(2) = Eigen::Vector3f(normal_[0], normal_[1], normal_[2]);
365  ROS_DEBUG("O: [%f, %f, %f]", O_[0], O_[1], O_[2]);
366  ROS_DEBUG("ex: [%f, %f, %f]", ex_[0], ex_[1], ex_[2]);
367  ROS_DEBUG("ey: [%f, %f, %f]", ey_[0], ey_[1], ey_[2]);
368  ROS_DEBUG("normal: [%f, %f, %f]", normal_[0], normal_[1], normal_[2]);
369  output = Eigen::Translation3f(O_) * Eigen::Quaternionf(rot_mat);
370  }
371 
372  void GridMap::originPose(Eigen::Affine3d& output)
373  {
374  Eigen::Affine3f float_affine;
375  originPose(float_affine);
376  convertEigenAffine3(float_affine, output);
377  }
378 
379  void GridMap::toMsg(jsk_recognition_msgs::SparseOccupancyGrid& grid)
380  {
381  grid.resolution = resolution_;
382  // compute origin POSE from O and normal_, d_
383  Eigen::Affine3d plane_pose;
384  originPose(plane_pose);
385  tf::poseEigenToMsg(plane_pose, grid.origin_pose);
386  for (ColumnIterator it = data_.begin();
387  it != data_.end();
388  it++) {
389  int column_index = it->first;
390  RowIndices row_indices = it->second;
391  jsk_recognition_msgs::SparseOccupancyGridColumn ros_column;
392  ros_column.column_index = column_index;
393  for (RowIterator rit = row_indices.begin();
394  rit != row_indices.end();
395  rit++) {
396  jsk_recognition_msgs::SparseOccupancyGridCell cell;
397  cell.row_index = *rit;
398  cell.value = 1.0;
399  ros_column.cells.push_back(cell);
400  }
401  grid.columns.push_back(ros_column);
402  }
403  }
404 
406  {
407  return Plane(normal_, d_);
408  }
409 
411  {
412  Plane::Ptr ret (new Plane(normal_, d_));
413  return ret;
414  }
415 
416 
417  std::vector<float> GridMap::getCoefficients()
418  {
419  std::vector<float> output;
420  output.push_back(normal_[0]);
421  output.push_back(normal_[1]);
422  output.push_back(normal_[2]);
423  output.push_back(d_);
424  return output;
425  }
426 
428  {
429  ++vote_;
430  }
431 
432  unsigned int GridMap::getVoteNum()
433  {
434  return vote_;
435  }
436 
437  void GridMap::setGeneration(unsigned int generation) {
438  generation_ = generation;
439  }
440 
441  unsigned int GridMap::getGeneration()
442  {
443  return generation_;
444  }
445 
447  {
448  int x = index->x;
449  int y = index->y;
450  ColumnIterator it = data_.find(x);
451  if (it != data_.end()) {
452  RowIterator rit = (it->second).find(y);
453  if (rit != it->second.end()) {
454  it->second.erase(rit);
455  }
456  }
457  }
458  bool GridMap::isBinsOccupied(const Eigen::Vector3f& p)
459  {
460  GridIndex::Ptr ret (new GridIndex());
461  pointToIndex(p, ret);
462  //ROS_INFO("checking (%d, %d)", ret->x, ret->y);
463  return getValue(ret);
464  }
465 
466  boost::tuple<int, int> GridMap::minMaxX()
467  {
468  int min_x = INT_MAX;
469  int max_x = - INT_MAX;
470  for (ColumnIterator it = data_.begin();
471  it != data_.end();
472  ++it) {
473  int x = it->first;
474  if (min_x > x) {
475  min_x = x;
476  }
477  if (max_x < x) {
478  max_x = x;
479  }
480  }
481  return boost::make_tuple<int, int>(min_x, max_x);
482  }
483 
484  boost::tuple<int, int> GridMap::minMaxY()
485  {
486  int min_y = INT_MAX;
487  int max_y = - INT_MAX;
488  for (ColumnIterator it = data_.begin();
489  it != data_.end();
490  ++it) {
491  RowIndices row_indices = it->second;
492  for (RowIterator rit = row_indices.begin();
493  rit != row_indices.end();
494  rit++) {
495  int y = *rit;
496  if (min_y > y) {
497  min_y = y;
498  }
499  if (max_y < y) {
500  max_y = y;
501  }
502  }
503  }
504  return boost::make_tuple<int, int>(min_y, max_y);
505  }
506 
508  {
509  boost::tuple<int, int> min_max_x = minMaxX();
510  return min_max_x.get<1>() - min_max_x.get<0>();
511  }
512 
514  {
515  boost::tuple<int, int> min_max_y = minMaxY();
516  return min_max_y.get<1>() - min_max_y.get<0>();
517  }
518 
520  {
521  boost::tuple<int, int> min_max_x = minMaxX();
522  int min_x = min_max_x.get<0>();
523  return - min_x;
524  }
525 
527  {
528  boost::tuple<int, int> min_max_y = minMaxY();
529  int min_y = min_max_y.get<0>();
530  return - min_y;
531  }
532 
533  int GridMap::normalizedIndex(int width_offset, int height_offset,
534  int step,
535  int elem_size,
536  int original_x, int original_y)
537  {
538  int x = original_x + width_offset;
539  int y = original_y + height_offset;
540  return y * step + x * elem_size;
541  }
542 
543 
545  {
546  // initialize with black
547  int width = normalizedWidth();
548  int height = normalizedHeight();
549  int width_offset = widthOffset();
550  int height_offset = heightOffset();
551  cv::Mat m = cv::Mat(width, height, CV_8UC1) * 0;
552  // for all index
553  for (ColumnIterator it = data_.begin();
554  it != data_.end();
555  ++it) {
556  for (RowIterator rit = it->second.begin();
557  rit != it->second.end();
558  ++rit) {
559  m.data[normalizedIndex(width_offset, height_offset,
560  m.step, m.elemSize(),
561  it->first, *rit)] = 255;
562  }
563  }
564 
565  return m;
566  }
567 
568  bool GridMap::check4Neighbor(int x, int y) {
569  if (getValue(x + 1, y) &&
570  getValue(x + 1, y + 1) &&
571  getValue(x - 1, y) &&
572  getValue(x - 1, y - 1)) {
573  return true;
574  }
575  else {
576  return false;
577  }
578  }
579 
581  {
582  //Columns new_data;
584  for (ColumnIterator it = data_.begin();
585  it != data_.end();
586  it++) {
587  RowIndices row_indices = it->second;
588  int x = it->first;
589  for (RowIterator rit = row_indices.begin();
590  rit != row_indices.end();
591  rit++) {
592  int y = *rit;
593  if (check4Neighbor(x, y)) {
594  new_map->registerIndex(x, y);
595  }
596  }
597  }
598  data_ = new_map->data_;
599  }
600 
601  void GridMap::decrease(int i)
602  {
603  for (int ii = 0; ii < i; ii++) {
604  decreaseOne();
605  }
606  }
607 
608  void GridMap::add(GridMap& other)
609  {
610  for (ColumnIterator it = other.data_.begin();
611  it != other.data_.end();
612  it++) {
613  RowIndices row_indices = it->second;
614  int x = it->first;
615  for (RowIterator rit = row_indices.begin();
616  rit != row_indices.end();
617  rit++) {
618  int y = *rit;
619  Eigen::Vector3f pos;
620  GridIndex index(x, y);
621  other.gridToPoint(index, pos);
622  pcl::PointXYZRGB p;
623  pointFromVectorToXYZ<Eigen::Vector3f, pcl::PointXYZRGB>(pos, p);
624  registerPoint(p);
625  }
626  }
627  }
628 
629  pcl::PointCloud<pcl::PointXYZ>::Ptr GridMap::toPointCloud()
630  {
631  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
632  for (ColumnIterator it = data_.begin();
633  it != data_.end();
634  it++) {
635  RowIndices row_indices = it->second;
636  int x = it->first;
637  for (RowIterator rit = row_indices.begin();
638  rit != row_indices.end();
639  rit++) {
640  int y = *rit;
641  Eigen::Vector3f pos;
642  GridIndex index(x, y);
643  gridToPoint(index, pos);
644  pcl::PointXYZ p;
645  pointFromVectorToXYZ<Eigen::Vector3f, pcl::PointXYZ>(pos, p);
646  cloud->points.push_back(p);
647  }
648  }
649  return cloud;
650  }
651 
653  {
654  // 1. build pointcloud
655  // 2. compute convex hull
656  // 3. return it as ConvexPolygon
657  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = toPointCloud();
658  pcl::ConvexHull<pcl::PointXYZ> chull;
659  chull.setInputCloud(cloud);
660  chull.setDimension(2);
661  pcl::PointCloud<pcl::PointXYZ> chull_output;
662  chull.reconstruct(chull_output);
663  // convex chull_output to Vertices
664  Vertices vs;
665  for (size_t i = 0; i < chull_output.points.size(); i++) {
666  Eigen::Vector3f v = chull_output.points[i].getVector3fMap();
667  vs.push_back(v);
668  }
669  return ConvexPolygon::Ptr(new ConvexPolygon(vs));
670  }
671 
672 }
virtual boost::tuple< int, int > minMaxX()
Definition: grid_map.cpp:466
virtual void add(GridMap &other)
Definition: grid_map.cpp:608
GridMap(double resolution, const std::vector< float > &coefficients)
Definition: grid_map.cpp:49
virtual void registerPoint(const pcl::PointXYZRGB &point)
Definition: grid_map.cpp:95
virtual void decrease(int i)
Definition: grid_map.cpp:601
pos
virtual void indicesToPointCloud(const std::vector< GridIndex::Ptr > &indices, pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud)
Definition: grid_map.cpp:344
virtual std::vector< GridIndex::Ptr > registerLine(const pcl::PointXYZRGB &from, const pcl::PointXYZRGB &to)
Definition: grid_map.cpp:103
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
boost::shared_ptr< ConvexPolygon > Ptr
Columns::iterator ColumnIterator
Definition: grid_map.h:61
virtual void removeIndex(const GridIndex::Ptr &index)
Definition: grid_map.cpp:446
virtual bool getValue(const GridIndex::Ptr &index)
Definition: grid_map.cpp:288
virtual cv::Mat toImage()
Definition: grid_map.cpp:544
std::set< int >::iterator RowIterator
Definition: grid_map.h:62
virtual void pointToIndex(const pcl::PointXYZRGB &point, GridIndex::Ptr index)
Definition: grid_map.cpp:218
virtual pcl::PointCloud< pcl::PointXYZ >::Ptr toPointCloud()
Definition: grid_map.cpp:629
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
virtual void registerPointCloud(pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud)
Definition: grid_map.cpp:210
virtual bool check4Neighbor(int x, int y)
Definition: grid_map.cpp:568
virtual bool isBinsOccupied(const Eigen::Vector3f &p)
Definition: grid_map.cpp:458
virtual std::vector< float > getCoefficients()
Definition: grid_map.cpp:417
virtual void gridToPoint(GridIndex::Ptr index, Eigen::Vector3f &pos)
Definition: grid_map.cpp:229
virtual Plane::Ptr toPlanePtr()
Definition: grid_map.cpp:410
virtual void fillRegion(const Eigen::Vector3f &start, std::vector< GridIndex::Ptr > &output)
Definition: grid_map.cpp:337
virtual void setGeneration(unsigned int generation)
Definition: grid_map.cpp:437
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 ConvexPolygon::Ptr toConvexPolygon()
Definition: grid_map.cpp:652
virtual unsigned int getGeneration()
Definition: grid_map.cpp:441
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
p
virtual unsigned int getVoteNum()
Definition: grid_map.cpp:432
std::set< int > RowIndices
Definition: grid_map.h:59
virtual void originPose(Eigen::Affine3f &output)
Definition: grid_map.cpp:359
virtual void gridToPoint2(const GridIndex &index, Eigen::Vector3f &pos)
Definition: grid_map.cpp:240
virtual int normalizedIndex(int width_offset, int height_offset, int step, int elem_size, int original_x, int original_y)
Definition: grid_map.cpp:533
virtual void toMsg(jsk_recognition_msgs::SparseOccupancyGrid &grid)
Definition: grid_map.cpp:379
std::vector< GridLine::Ptr > lines_
Definition: grid_map.h:123
virtual GridIndex::Ptr registerIndex(const GridIndex::Ptr &index)
Definition: grid_map.cpp:90
#define ROS_DEBUG(...)
virtual boost::tuple< int, int > minMaxY()
Definition: grid_map.cpp:484


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