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 {
52  class ANNGridCell
53  {
54  public:
56  typedef boost::unordered_set<size_t> Indices;
57  ANNGridCell() {}
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:
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 {
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
jsk_footstep_planner::ANNGrid::pointToIndex
virtual Index pointToIndex(const pcl::PointNormal &p) const
Definition: ann_grid.h:138
boost::shared_ptr
i
int i
jsk_footstep_planner::ANNGrid::cells_
std::vector< std::vector< ANNGridCell::Ptr > > cells_
Definition: ann_grid.h:190
jsk_footstep_planner::ANNGrid::fillByBox
virtual IndexArray fillByBox(const Eigen::Vector3f &p0, const Eigen::Vector3f &p1, const Eigen::Vector3f &p2, const Eigen::Vector3f &p3)
Definition: ann_grid.cpp:177
jsk_footstep_planner::ANNGridCell::fill
virtual void fill(Indices &out)
Definition: ann_grid.h:132
ros.h
cloud
pcl::PointCloud< pcl::PointNormal >::Ptr cloud
Definition: footstep_projection_demo.cpp:49
jsk_footstep_planner::ANNGrid::min_point_
Eigen::Vector3f min_point_
Definition: ann_grid.h:192
jsk_footstep_planner::ANNGridCell::get
virtual Indices get()
Definition: ann_grid.h:127
jsk_footstep_planner::ANNGrid::getCell
ANNGridCell::Ptr getCell(size_t i, size_t j)
Definition: ann_grid.h:155
jsk_footstep_planner::ANNGrid::build
virtual void build(const pcl::PointCloud< pcl::PointNormal > &cloud)
Definition: ann_grid.cpp:75
jsk_footstep_planner::ANNGridCell::~ANNGridCell
virtual ~ANNGridCell()
Definition: ann_grid.h:122
jsk_footstep_planner::ANNGrid::ANNGrid
ANNGrid(const double grid_size)
Definition: ann_grid.h:131
jsk_footstep_planner::ANNGridCell::indices_
Indices indices_
Definition: ann_grid.h:145
jsk_footstep_planner::ANNGrid::box
virtual IndexArray box(const Eigen::Vector3f &p0, const Eigen::Vector3f &p1, const Eigen::Vector3f &p2, const Eigen::Vector3f &p3)
Definition: ann_grid.cpp:218
jsk_footstep_planner::ANNGrid::bresenham
virtual IndexArray bresenham(const Eigen::Vector3f &p0, const Eigen::Vector3f &p1)
Definition: ann_grid.cpp:133
jsk_footstep_planner::ANNGridCell
Definition: ann_grid.h:84
jsk_footstep_planner::ANNGrid
ANNGrid is a class to provide approximate near neighbors search based on 2.5-D representation....
Definition: ann_grid.h:125
jsk_footstep_planner
Definition: ann_grid.h:50
jsk_footstep_planner::ANNGrid::fill
virtual IndexArray fill(const IndexArray &filled)
Definition: ann_grid.cpp:193
jsk_footstep_planner::ANNGrid::toImage
virtual void toImage(cv::Mat &mat)
Definition: ann_grid.cpp:241
jsk_footstep_planner::ANNGrid::IndexArray
std::vector< Index > IndexArray
Definition: ann_grid.h:130
jsk_footstep_planner::ANNGrid::mat_
cv::Mat mat_
Definition: ann_grid.h:191
jsk_footstep_planner::ANNGrid::grid_size_
const double grid_size_
Definition: ann_grid.h:189
pose_array.p
p
Definition: pose_array.py:21
jsk_footstep_planner::ANNGridCell::add
virtual void add(size_t i)
Definition: ann_grid.h:123
jsk_footstep_planner::ANNGrid::~ANNGrid
virtual ~ANNGrid()
Definition: ann_grid.h:132
jsk_footstep_planner::ANNGridCell::Ptr
boost::shared_ptr< ANNGridCell > Ptr
Definition: ann_grid.h:119
jsk_footstep_planner::ANNGrid::approximateSearchInBox
virtual void approximateSearchInBox(const Eigen::Vector3f &p0, const Eigen::Vector3f &p1, const Eigen::Vector3f &p2, const Eigen::Vector3f &p3, pcl::PointIndices &indices)
Definition: ann_grid.cpp:109
jsk_footstep_planner::ANNGridCell::Indices
boost::unordered_set< size_t > Indices
Definition: ann_grid.h:120
jsk_footstep_planner::ANNGrid::Ptr
boost::shared_ptr< ANNGrid > Ptr
Definition: ann_grid.h:128
jsk_footstep_planner::ANNGrid::Index
cv::Point Index
Definition: ann_grid.h:129
size
size
jsk_footstep_planner::ANNGridCell::ANNGridCell
ANNGridCell()
Definition: ann_grid.h:121
v
GLfloat v[8][3]
jsk_footstep_planner::ANNGrid::approximateSearch
virtual void approximateSearch(const Eigen::Vector3f &v, pcl::PointIndices &indices)
Definition: ann_grid.cpp:126


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Wed Jan 24 2024 04:05:29