pointcloud_model_generator.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 #include <boost/random.hpp>
38 
39 namespace jsk_footstep_planner
40 {
42  const std::string& model_name,
43  pcl::PointCloud<PointT>& output,
44  double hole_rate)
45  {
46  output.points.clear();
47  if (model_name == "flat") {
48  flat(output, hole_rate);
49  }
50  else if (model_name == "stairs") {
51  stairs(output, hole_rate);
52  }
53  else if (model_name == "hills") {
54  hills(output, hole_rate);
55  }
56  else if (model_name == "gaussian") {
57  gaussian(output, hole_rate);
58  }
59  else if (model_name == "flat_pole") {
60  flatPole(output, hole_rate);
61  }
62  }
63 
64  void PointCloudModelGenerator::flat(pcl::PointCloud<PointT>& output, double hole_rate)
65  {
66  boost::mt19937 gen( static_cast<unsigned long>(time(0)) );
67  boost::uniform_real<> dst( 0, 100 );
68  boost::variate_generator<
69  boost::mt19937&, boost::uniform_real<>
70  > rand( gen, dst );
71 
72  for (double y = -4; y < 4; y = y + 0.01) {
73  for (double x = -4; x < 4; x = x + 0.01) {
74  if (rand() >= hole_rate) {
75  pcl::PointNormal p;
76  p.x = x;
77  p.y = y;
78  output.points.push_back(p);
79  }
80  }
81  }
82  }
83 
84  void PointCloudModelGenerator::addPole(pcl::PointCloud<PointT>& output,
85  const Eigen::Vector3f& center,
86  const double width,
87  const double height)
88  {
89  double y0 = center[1] + width / 2.0;
90  double y1 = center[1] - width / 2.0;
91  double x0 = center[0] + width / 2.0;
92  double x1 = center[0] - width / 2.0;
93  for (double x = x0; x > x1; x = x - 0.01) {
94  for (double z = 0; z < height; z = z + 0.01) {
95  pcl::PointNormal p;
96  p.x = x;
97  p.y = y0;
98  p.z = z;
99  output.points.push_back(p);
100  }
101  }
102  for (double y = y0; y > y1; y = y - 0.01) {
103  for (double z = 0; z < height; z = z + 0.01) {
104  pcl::PointNormal p;
105  p.x = x1;
106  p.y = y;
107  p.z = z;
108  output.points.push_back(p);
109  }
110  }
111  for (double x = x0; x > x1; x = x - 0.01) {
112  for (double z = 0; z < height; z = z + 0.01) {
113  pcl::PointNormal p;
114  p.x = x;
115  p.y = y1;
116  p.z = z;
117  output.points.push_back(p);
118  }
119  }
120  for (double y = y0; y > y1; y = y - 0.01) {
121  for (double z = 0; z < height; z = z + 0.01) {
122  pcl::PointNormal p;
123  p.x = x0;
124  p.y = y;
125  p.z = z;
126  output.points.push_back(p);
127  }
128  }
129  }
130 
131  void PointCloudModelGenerator::flatPole(pcl::PointCloud<PointT>& output, double hole_rate)
132  {
133  boost::mt19937 gen( static_cast<unsigned long>(time(0)) );
134  boost::uniform_real<> dst( 0, 100 );
135  boost::variate_generator<
136  boost::mt19937&, boost::uniform_real<>
137  > rand( gen, dst );
138 
139  for (double y = -4; y < 4; y = y + 0.01) {
140  for (double x = -4; x < 4; x = x + 0.01) {
141  if (rand() >= hole_rate) {
142  pcl::PointNormal p;
143  p.x = x;
144  p.y = y;
145  output.points.push_back(p);
146  }
147  }
148  }
149  for (double y = -4; y < 4; y = y + 2.0) {
150  for (double x = -4; x < 4; x = x + 2.0) {
151  if (x != 0.0 || y != 0.0) {
152  addPole(output, Eigen::Vector3f(x, y, 0), 0.2, 2.0);
153  }
154  }
155  }
156  }
157 
158  void PointCloudModelGenerator::hills(pcl::PointCloud<PointT>& output, double hole_rate)
159  {
160  boost::mt19937 gen( static_cast<unsigned long>(time(0)) );
161  boost::uniform_real<> dst( 0, 100 );
162  boost::variate_generator<
163  boost::mt19937&, boost::uniform_real<>
164  > rand( gen, dst );
165 
166  const double height = 0.1;
167  for (double y = -4; y < 4; y = y + 0.01) {
168  for (double x = -4; x < 4; x = x + 0.01) {
169  if (rand() >= hole_rate) {
170  pcl::PointNormal p;
171  p.x = x;
172  p.y = y;
173  p.z = height * sin(x * 2) * sin(y * 2);
174  output.points.push_back(p);
175  }
176  }
177  }
178  }
179 
180  void PointCloudModelGenerator::gaussian(pcl::PointCloud<PointT>& output, double hole_rate)
181  {
182  boost::mt19937 gen( static_cast<unsigned long>(time(0)) );
183  boost::uniform_real<> dst( 0, 100 );
184  boost::variate_generator<
185  boost::mt19937&, boost::uniform_real<>
186  > rand( gen, dst );
187  const double height = 1.0;
188  const double sigma = 0.3;
189  for (double y = -4; y < 4; y = y + 0.01) {
190  for (double x = -4; x < 4; x = x + 0.01) {
191  if (rand() >= hole_rate) {
192  pcl::PointNormal p;
193  p.x = x;
194  p.y = y;
195  //p.z = height * sin(x * 2) * sin(y * 2);
196  p.z = height * exp(-x*x / (2 * sigma * 2)) * exp(-y*y / (2 * sigma * 2));
197  output.points.push_back(p);
198  }
199  }
200  }
201  }
202 
203  void PointCloudModelGenerator::stairs(pcl::PointCloud<PointT>& output, double hole_rate)
204  {
205  boost::mt19937 gen( static_cast<unsigned long>(time(0)) );
206  boost::uniform_real<> dst( 0, 100 );
207  boost::variate_generator<
208  boost::mt19937&, boost::uniform_real<>
209  > rand( gen, dst );
210 
211  for (double y = -4; y < 4; y = y + 0.01) {
212  // for (double x = -4; x < 0; x = x + 0.01) {
213  // if (rand() >= hole_rate) {
214  // pcl::PointNormal p;
215  // p.x = x;
216  // p.y = y;
217  // p.z = 0;
218  // output.points.push_back(p);
219  // }
220  // }
221  for (double x = -4; x < 5; x = x + 0.01) {
222  if (rand() >= hole_rate) {
223  pcl::PointNormal p;
224  p.x = x;
225  p.y = y;
226  if (x > 0) {
227  p.z = floor(x * 3) * 0.1;
228  }
229  else {
230  p.z = ceil(x * 3) * 0.1;
231  }
232  output.points.push_back(p);
233  }
234  }
235  }
236  }
237 
238 }
pointcloud_model_generator.h
jsk_footstep_planner::PointCloudModelGenerator::addPole
virtual void addPole(pcl::PointCloud< PointT > &output, const Eigen::Vector3f &center, const double width, const double height)
Definition: pointcloud_model_generator.cpp:116
sin
double sin()
jsk_footstep_planner::PointCloudModelGenerator::generate
virtual void generate(const std::string &model_name, pcl::PointCloud< PointT > &output, double hole_rate=0.0)
Definition: pointcloud_model_generator.cpp:73
jsk_footstep_planner::PointCloudModelGenerator::flatPole
virtual void flatPole(pcl::PointCloud< PointT > &output, double hole_rate)
Definition: pointcloud_model_generator.cpp:163
jsk_footstep_planner
Definition: ann_grid.h:50
y
double y
jsk_footstep_planner::PointCloudModelGenerator::flat
virtual void flat(pcl::PointCloud< PointT > &output, double hole_rate)
Definition: pointcloud_model_generator.cpp:96
pose_array.p
p
Definition: pose_array.py:21
hole_rate
double hole_rate
Definition: pointcloud_model_generator_node.cpp:44
jsk_footstep_planner::PointCloudModelGenerator::gaussian
virtual void gaussian(pcl::PointCloud< PointT > &output, double hole_rate)
Definition: pointcloud_model_generator.cpp:212
width
width
jsk_footstep_planner::PointCloudModelGenerator::stairs
virtual void stairs(pcl::PointCloud< PointT > &output, double hole_rate)
Definition: pointcloud_model_generator.cpp:235
jsk_footstep_planner::PointCloudModelGenerator::hills
virtual void hills(pcl::PointCloud< PointT > &output, double hole_rate)
Definition: pointcloud_model_generator.cpp:190
sigma
sigma
x
double x
height
height
z
double z


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