ann_grid.h
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, 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 
37 #ifndef JSK_FOOTSTEP_PLANNER_ANN_GRID_H_
38 #define JSK_FOOTSTEP_PLANNER_ANN_GRID_H_
39 
40 #include <boost/tuple/tuple.hpp>
41 #include <boost/unordered/unordered_set.hpp>
42 #include <pcl/point_types.h>
43 #include <pcl/point_cloud.h>
44 #include <pcl/PointIndices.h>
45 
46 #include <opencv2/opencv.hpp>
47 
48 #include <ros/ros.h>
49 
51 {
53  {
54  public:
56  typedef boost::unordered_set<size_t> Indices;
58  virtual ~ANNGridCell() {}
59  virtual void add(size_t i)
60  {
61  indices_.insert(i);
62  }
63  virtual Indices get()
64  {
65  return indices_;
66  }
67 
68  virtual void fill(Indices& out)
69  {
70  for (Indices::iterator it = indices_.begin(); it != indices_.end(); ++it) {
71  out.insert(*it);
72  }
73  }
74  virtual void fill(std::vector<int>& out)
75  {
76  for (Indices::iterator it = indices_.begin(); it != indices_.end(); ++it) {
77  out.push_back(*it);
78  }
79  }
80  protected:
81  Indices indices_;
82  private:
83 
84  };
85 
86 
93  class ANNGrid
94  {
95  public:
97  typedef cv::Point Index;
98  typedef std::vector<Index> IndexArray;
99  ANNGrid(const double grid_size): grid_size_(grid_size) {}
100  virtual ~ANNGrid()
101  {
102  }
103 
104  virtual void build(const pcl::PointCloud<pcl::PointNormal>& cloud);
105 
106  virtual Index pointToIndex(const pcl::PointNormal& p) const
107  {
108  Eigen::Vector3f v = p.getVector3fMap();
109  Eigen::Vector3f translated_point = v - min_point_;
110  int i = (int)std::floor(translated_point[0] / grid_size_);
111  int j = (int)std::floor(translated_point[1] / grid_size_);
112  return cv::Point(i, j);
113  }
114 
115  virtual Index pointToIndex(const Eigen::Vector3f& p) const
116  {
117  Eigen::Vector3f translated_point = p - min_point_;
118  int i = (int)std::floor(translated_point[0] / grid_size_);
119  int j = (int)std::floor(translated_point[1] / grid_size_);
120  return cv::Point(i, j);
121  }
122 
123  inline ANNGridCell::Ptr getCell(size_t i, size_t j)
124  {
125  // TODO: is this check correct?
126  if (cells_.size() > i) {
127  if (cells_[i].size() > j) {
128  return cells_[i][j];
129  }
130  else {
131  return ANNGridCell::Ptr();
132  }
133  }
134  else {
135  return ANNGridCell::Ptr();
136  }
137  }
138 
139  virtual void approximateSearch(const Eigen::Vector3f& v, pcl::PointIndices& indices);
140  virtual IndexArray bresenham(const Eigen::Vector3f& p0, const Eigen::Vector3f& p1);
141  // fill method is not mt safe
142  virtual IndexArray fill(const IndexArray& filled);
143  virtual IndexArray fillByBox(
144  const Eigen::Vector3f& p0, const Eigen::Vector3f& p1,
145  const Eigen::Vector3f& p2, const Eigen::Vector3f& p3);
146  virtual void approximateSearchInBox(
147  const Eigen::Vector3f& p0, const Eigen::Vector3f& p1,
148  const Eigen::Vector3f& p2, const Eigen::Vector3f& p3,
149  pcl::PointIndices& indices);
150 
151  virtual IndexArray box(const Eigen::Vector3f& p0, const Eigen::Vector3f& p1,
152  const Eigen::Vector3f& p2, const Eigen::Vector3f& p3);
153  virtual void toImage(cv::Mat& mat);
154  virtual void toImage(cv::Mat& mat, const IndexArray& pixels);
155  protected:
156 
157  const double grid_size_;
158  std::vector<std::vector<ANNGridCell::Ptr> > cells_;
159  cv::Mat mat_;
160  Eigen::Vector3f min_point_;
161  private:
162 
163  };
164 }
165 
166 #endif
ANNGrid is a class to provide approximate near neighbors search based on 2.5-D representation. All the z values of pointcloud is ignored and it sorted as 2-D array.
Definition: ann_grid.h:93
Eigen::Vector3f min_point_
Definition: ann_grid.h:160
p1
boost::shared_ptr< ANNGridCell > Ptr
Definition: ann_grid.h:55
virtual void fill(std::vector< int > &out)
Definition: ann_grid.h:74
virtual void fill(Indices &out)
Definition: ann_grid.h:68
virtual Index pointToIndex(const Eigen::Vector3f &p) const
Definition: ann_grid.h:115
size
boost::unordered_set< size_t > Indices
Definition: ann_grid.h:56
virtual void add(size_t i)
Definition: ann_grid.h:59
p2
pcl::PointCloud< pcl::PointNormal >::Ptr cloud
virtual Index pointToIndex(const pcl::PointNormal &p) const
Definition: ann_grid.h:106
boost::shared_ptr< ANNGrid > Ptr
Definition: ann_grid.h:96
box
std::vector< Index > IndexArray
Definition: ann_grid.h:98
ANNGrid(const double grid_size)
Definition: ann_grid.h:99
GLfloat v[8][3]
ANNGridCell::Ptr getCell(size_t i, size_t j)
Definition: ann_grid.h:123
std::vector< std::vector< ANNGridCell::Ptr > > cells_
Definition: ann_grid.h:158


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Thu Nov 14 2019 03:53:28