ann_grid.cpp
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 
37 
38 #include <pcl/common/common.h>
39 #include <jsk_topic_tools/log_utils.h>
40 
41 namespace jsk_footstep_planner
42 {
43  void ANNGrid::build(const pcl::PointCloud<pcl::PointNormal>& cloud)
44  {
45  cells_.clear();
46  pcl::PointNormal min_pt, max_pt;
47  pcl::getMinMax3D(cloud, min_pt, max_pt);
48  min_point_ = min_pt.getVector3fMap();
49  Eigen::Vector3f diff = max_pt.getVector3fMap() - min_pt.getVector3fMap();
50 
51  size_t x_num = std::ceil(diff[0] / grid_size_) + 1;
52  size_t y_num = std::ceil(diff[1] / grid_size_) + 1;
53  cells_.resize(x_num);
54  mat_ = cv::Mat(y_num, x_num, CV_8U);
55  for (size_t xi = 0; xi < x_num; xi++) {
56  cells_[xi].resize(y_num);
57  for (size_t yi = 0; yi < y_num; yi++) {
58  cells_[xi][yi] = ANNGridCell::Ptr(new ANNGridCell);
59  }
60  }
61  for (size_t i = 0; i < cloud.points.size(); i++) {
62  pcl::PointNormal p = cloud.points[i];
63  if (isnan(p.x) || isnan(p.y) || isnan(p.z)) {
64  continue;
65  }
67  if (index.x >= x_num) {
68  ROS_FATAL("index.x exceeds x_num: %d > %lu", index.x, x_num);
69  }
70  if (index.y >= y_num) {
71  ROS_FATAL("indey.y eyceeds y_num: %d > %lu", index.y, y_num);
72  }
73  cells_[index.x][index.y]->add(i);
74  }
75  }
76 
78  const Eigen::Vector3f& p0, const Eigen::Vector3f& p1,
79  const Eigen::Vector3f& p2, const Eigen::Vector3f& p3,
80  pcl::PointIndices& out_indices)
81  {
82  IndexArray cell_indices = fillByBox(p0, p1, p2, p3);
83  //ANNGridCell::Indices point_indices;
84  out_indices.indices.clear();
85  for (size_t i = 0; i < cell_indices.size(); i++) {
86  ANNGridCell::Ptr cell = getCell(cell_indices[i].x, cell_indices[i].y);
87  if (cell) {
88  cell->fill(out_indices.indices);
89  }
90  }
91  //out_indices.indices = std::vector<int>(point_indices.begin(), point_indices.end());
92  }
93 
94  void ANNGrid::approximateSearch(const Eigen::Vector3f& v, pcl::PointIndices& indices)
95  {
97  ANNGridCell::Indices cell_indices = getCell(index.x, index.y)->get();
98  indices.indices = std::vector<int>(cell_indices.begin(), cell_indices.end());
99  }
100 
102  const Eigen::Vector3f& p0, const Eigen::Vector3f& p1)
103  {
104  IndexArray ret;
105  Index i0 = pointToIndex(p0);
106  Index i1 = pointToIndex(p1);
107 
108  int x0 = i0.x;
109  int y0 = i0.y;
110  int x1 = i1.x;
111  int y1 = i1.y;
112  int dx = std::abs(x1 - x0);
113  int dy = std::abs(y1 - y0);
114  int sx, sy;
115  if (x0 < x1) {
116  sx = 1;
117  }
118  else {
119  sx = -1;
120  }
121  if (y0 < y1) {
122  sy = 1;
123  }
124  else {
125  sy = -1;
126  }
127  int err = dx - dy;
128  while (1) {
129  ret.push_back(cv::Point(x0, y0));
130  if (x0 == x1 && y0 == y1) {
131  return ret;
132  }
133  int e2 = 2 * err;
134  if (e2 > -dy) {
135  err = err - dy;
136  x0 = x0 + sx;
137  }
138  if (e2 < dx) {
139  err = err + dx;
140  y0 = y0 + sy;
141  }
142  }
143  }
144 
146  const Eigen::Vector3f& p0, const Eigen::Vector3f& p1,
147  const Eigen::Vector3f& p2, const Eigen::Vector3f& p3)
148  {
149  IndexArray box_indices(4);
150  Index i0 = pointToIndex(p0);
151  Index i1 = pointToIndex(p1);
152  Index i2 = pointToIndex(p2);
153  Index i3 = pointToIndex(p3);
154  box_indices[0] = i0;
155  box_indices[1] = i1;
156  box_indices[2] = i2;
157  box_indices[3] = i3;
158  return fill(box_indices);
159  }
160 
161  ANNGrid::IndexArray ANNGrid::fill(const IndexArray& filled)
162  {
163  // clear by 0
164  mat_ = cv::Scalar::all(0);
165  std::vector<cv::Point> pts(filled.size());
166  for (size_t i = 0; i < filled.size(); i++) {
167  pts[i] = cv::Point(filled[i].x, filled[i].y);
168  }
169  cv::fillConvexPoly(mat_, pts, cv::Scalar(255));
170  // Compute bounding box
171  cv::Rect bbox = cv::boundingRect(cv::Mat(pts));
172  IndexArray ret;
173  ret.reserve(filled.size());
174  for (size_t j = 0; j <= bbox.height; j++) {
175  int y = bbox.y + j;
176  for (size_t i = 0; i <= bbox.width; i++) {
177  int x = bbox.x + i;
178  if (mat_.at<unsigned char>(y, x) == 255) {
179  ret.push_back(cv::Point(x, y));
180  }
181  }
182  }
183  return ret;
184  }
185 
186  ANNGrid::IndexArray ANNGrid::box(const Eigen::Vector3f& p0, const Eigen::Vector3f& p1,
187  const Eigen::Vector3f& p2, const Eigen::Vector3f& p3)
188  {
189  IndexArray a = bresenham(p0, p1);
190  IndexArray b = bresenham(p1, p2);
191  IndexArray c = bresenham(p2, p3);
192  IndexArray d = bresenham(p3, p0);
193  IndexArray ret(a.size() + b.size() + c.size() + d.size());
194  for (size_t i = 0; i < a.size(); i++) {
195  ret[i] = a[i];
196  }
197  for (size_t i = 0; i < b.size(); i++) {
198  ret[i + a.size()] = b[i];
199  }
200  for (size_t i = 0; i < c.size(); i++) {
201  ret[i + a.size() + b.size()] = c[i];
202  }
203  for (size_t i = 0; i < d.size(); i++) {
204  ret[i+ a.size() + b.size() + c.size()] = d[i];
205  }
206  return ret;
207  }
208 
209  void ANNGrid::toImage(cv::Mat& mat)
210  {
211  mat = cv::Mat::zeros(cells_[0].size(), cells_.size(), CV_8U);
212  }
213 
214  void ANNGrid::toImage(cv::Mat& mat, const IndexArray& pixels)
215  {
216  // Initialize by black
217  toImage(mat);
218  for (size_t i = 0; i < pixels.size(); i++) {
219  Index id = pixels[i];
220  mat.at<char>(id.y, id.x) = char(255);
221  }
222  }
223 }
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
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::ANNGrid::getCell
ANNGridCell::Ptr getCell(size_t i, size_t j)
Definition: ann_grid.h:155
plot_bench.dy
int dy
Definition: plot_bench.py:23
jsk_footstep_planner::ANNGrid::build
virtual void build(const pcl::PointCloud< pcl::PointNormal > &cloud)
Definition: ann_grid.cpp:75
p2
p2
c
c
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
plot_bench.dx
int dx
Definition: plot_bench.py:22
jsk_footstep_planner::ANNGrid::bresenham
virtual IndexArray bresenham(const Eigen::Vector3f &p0, const Eigen::Vector3f &p1)
Definition: ann_grid.cpp:133
p1
p1
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
y
double y
jsk_footstep_planner::ANNGrid::grid_size_
const double grid_size_
Definition: ann_grid.h:189
pose_array.p
p
Definition: pose_array.py:21
d
d
jsk_footstep_planner::ANNGridCell::Ptr
boost::shared_ptr< ANNGridCell > Ptr
Definition: ann_grid.h:119
ROS_FATAL
#define ROS_FATAL(...)
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
cell
index
unsigned int index
jsk_footstep_planner::ANNGrid::Index
cv::Point Index
Definition: ann_grid.h:129
x
double x
a
char a[26]
size
size
ann_grid.h
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