Point Cloud Library (PCL)  1.9.0-dev
spring.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 #ifndef PCL_POINT_CLOUD_SPRING_IMPL_HPP_
41 #define PCL_POINT_CLOUD_SPRING_IMPL_HPP_
42 
43 template <typename PointT> void
45  const PointT& val, const size_t& amount)
46 {
47  if (amount <= 0)
48  PCL_THROW_EXCEPTION (InitFailedException,
49  "[pcl::common::expandColumns] error: amount must be ]0.."
50  << (input.width/2) << "] !");
51 
52  if (!input.isOrganized () || amount > (input.width/2))
53  PCL_THROW_EXCEPTION (InitFailedException,
54  "[pcl::common::expandColumns] error: "
55  << "columns expansion requires organised point cloud");
56 
57  uint32_t old_height = input.height;
58  uint32_t old_width = input.width;
59  uint32_t new_width = old_width + 2*amount;
60  if (&input != &output)
61  output = input;
62  output.reserve (new_width * old_height);
63  for (int j = 0; j < output.height; ++j)
64  {
65  typename PointCloud<PointT>::iterator start = output.begin() + (j * new_width);
66  output.insert (start, amount, val);
67  start = output.begin() + (j * new_width) + old_width + amount;
68  output.insert (start, amount, val);
69  output.height = old_height;
70  }
71  output.width = new_width;
72  output.height = old_height;
73 }
74 
75 template <typename PointT> void
77  const PointT& val, const size_t& amount)
78 {
79  if (amount <= 0)
80  PCL_THROW_EXCEPTION (InitFailedException,
81  "[pcl::common::expandRows] error: amount must be ]0.."
82  << (input.height/2) << "] !");
83 
84  uint32_t old_height = input.height;
85  uint32_t new_height = old_height + 2*amount;
86  uint32_t old_width = input.width;
87  if (&input != &output)
88  output = input;
89  output.reserve (new_height * old_width);
90  output.insert (output.begin (), amount * old_width, val);
91  output.insert (output.end (), amount * old_width, val);
92  output.width = old_width;
93  output.height = new_height;
94 }
95 
96 template <typename PointT> void
98  const size_t& amount)
99 {
100  if (amount <= 0)
101  PCL_THROW_EXCEPTION (InitFailedException,
102  "[pcl::common::duplicateColumns] error: amount must be ]0.."
103  << (input.width/2) << "] !");
104 
105  if (!input.isOrganized () || amount > (input.width/2))
106  PCL_THROW_EXCEPTION (InitFailedException,
107  "[pcl::common::duplicateColumns] error: "
108  << "columns expansion requires organised point cloud");
109 
110  size_t old_height = input.height;
111  size_t old_width = input.width;
112  size_t new_width = old_width + 2*amount;
113  if (&input != &output)
114  output = input;
115  output.reserve (new_width * old_height);
116  for (size_t j = 0; j < old_height; ++j)
117  for(size_t i = 0; i < amount; ++i)
118  {
119  typename PointCloud<PointT>::iterator start = output.begin () + (j * new_width);
120  output.insert (start, *start);
121  start = output.begin () + (j * new_width) + old_width + i;
122  output.insert (start, *start);
123  }
124 
125  output.width = new_width;
126  output.height = old_height;
127 }
128 
129 template <typename PointT> void
131  const size_t& amount)
132 {
133  if (amount <= 0 || amount > (input.height/2))
134  PCL_THROW_EXCEPTION (InitFailedException,
135  "[pcl::common::duplicateRows] error: amount must be ]0.."
136  << (input.height/2) << "] !");
137 
138  uint32_t old_height = input.height;
139  uint32_t new_height = old_height + 2*amount;
140  uint32_t old_width = input.width;
141  if (&input != &output)
142  output = input;
143  output.reserve (new_height * old_width);
144  for(size_t i = 0; i < amount; ++i)
145  {
146  output.insert (output.begin (), output.begin (), output.begin () + old_width);
147  output.insert (output.end (), output.end () - old_width, output.end ());
148  }
149 
150  output.width = old_width;
151  output.height = new_height;
152 }
153 
154 template <typename PointT> void
156  const size_t& amount)
157 {
158  if (amount <= 0)
159  PCL_THROW_EXCEPTION (InitFailedException,
160  "[pcl::common::mirrorColumns] error: amount must be ]0.."
161  << (input.width/2) << "] !");
162 
163  if (!input.isOrganized () || amount > (input.width/2))
164  PCL_THROW_EXCEPTION (InitFailedException,
165  "[pcl::common::mirrorColumns] error: "
166  << "columns expansion requires organised point cloud");
167 
168  size_t old_height = input.height;
169  size_t old_width = input.width;
170  size_t new_width = old_width + 2*amount;
171  if (&input != &output)
172  output = input;
173  output.reserve (new_width * old_height);
174  for (size_t j = 0; j < old_height; ++j)
175  for(size_t i = 0; i < amount; ++i)
176  {
177  typename PointCloud<PointT>::iterator start = output.begin () + (j * new_width);
178  output.insert (start, *(start + 2*i));
179  start = output.begin () + (j * new_width) + old_width + 2*i;
180  output.insert (start+1, *(start - 2*i));
181  }
182  output.width = new_width;
183  output.height = old_height;
184 }
185 
186 template <typename PointT> void
188  const size_t& amount)
189 {
190  if (amount <= 0 || amount > (input.height/2))
191  PCL_THROW_EXCEPTION (InitFailedException,
192  "[pcl::common::mirrorRows] error: amount must be ]0.."
193  << (input.height/2) << "] !");
194 
195  uint32_t old_height = input.height;
196  uint32_t new_height = old_height + 2*amount;
197  uint32_t old_width = input.width;
198  if (&input != &output)
199  output = input;
200  output.reserve (new_height * old_width);
201  for(size_t i = 0; i < amount; i++)
202  {
203  typename PointCloud<PointT>::iterator up;
204  if (output.height % 2 == 0)
205  up = output.begin () + (2*i) * old_width;
206  else
207  up = output.begin () + (2*i+1) * old_width;
208  output.insert (output.begin (), up, up + old_width);
209  typename PointCloud<PointT>::iterator bottom = output.end () - (2*i+1) * old_width;
210  output.insert (output.end (), bottom, bottom + old_width);
211  }
212  output.width = old_width;
213  output.height = new_height;
214 }
215 
216 template <typename PointT> void
218  const size_t& amount)
219 {
220  if (amount <= 0 || amount > (input.height/2))
221  PCL_THROW_EXCEPTION (InitFailedException,
222  "[pcl::common::deleteRows] error: amount must be ]0.."
223  << (input.height/2) << "] !");
224 
225  uint32_t old_height = input.height;
226  uint32_t old_width = input.width;
227  output.erase (output.begin (), output.begin () + amount * old_width);
228  output.erase (output.end () - amount * old_width, output.end ());
229  output.height = old_height - 2*amount;
230  output.width = old_width;
231 }
232 
233 template <typename PointT> void
235  const size_t& amount)
236 {
237  if (amount <= 0 || amount > (input.width/2))
238  PCL_THROW_EXCEPTION (InitFailedException,
239  "[pcl::common::deleteCols] error: amount must be in ]0.."
240  << (input.width/2) << "] !");
241 
242  if (!input.isOrganized ())
243  PCL_THROW_EXCEPTION (InitFailedException,
244  "[pcl::common::deleteCols] error: "
245  << "columns delete requires organised point cloud");
246 
247  uint32_t old_height = input.height;
248  uint32_t old_width = input.width;
249  uint32_t new_width = old_width - 2 * amount;
250  for(size_t j = 0; j < old_height; j++)
251  {
252  typename PointCloud<PointT>::iterator start = output.begin () + j * new_width;
253  output.erase (start, start + amount);
254  start = output.begin () + (j+1) * new_width;
255  output.erase (start, start + amount);
256  }
257  output.height = old_height;
258  output.width = new_width;
259 }
260 
261 #endif
void deleteRows(const PointCloud< PointT > &input, PointCloud< PointT > &output, const size_t &amount)
delete amount rows in top and bottom of point cloud
Definition: spring.hpp:217
iterator erase(iterator position)
Erase a point in the cloud.
Definition: point_cloud.h:536
void reserve(size_t n)
Definition: point_cloud.h:449
iterator end()
Definition: point_cloud.h:443
void mirrorColumns(const PointCloud< PointT > &input, PointCloud< PointT > &output, const size_t &amount)
expand point cloud mirroring amount right and left columns.
Definition: spring.hpp:155
VectorType::iterator iterator
Definition: point_cloud.h:440
void expandRows(const PointCloud< PointT > &input, PointCloud< PointT > &output, const PointT &val, const size_t &amount)
expand point cloud inserting amount rows at the top and the bottom of a point cloud and filling them ...
Definition: spring.hpp:76
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
void deleteCols(const PointCloud< PointT > &input, PointCloud< PointT > &output, const size_t &amount)
delete amount columns in top and bottom of point cloud
Definition: spring.hpp:234
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition: exceptions.h:194
void duplicateRows(const PointCloud< PointT > &input, PointCloud< PointT > &output, const size_t &amount)
expand point cloud duplicating the amount top and bottom rows times.
Definition: spring.hpp:130
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void expandColumns(const PointCloud< PointT > &input, PointCloud< PointT > &output, const PointT &val, const size_t &amount)
expand point cloud inserting amount columns at the right and the left of a point cloud and filling th...
Definition: spring.hpp:44
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
Definition: point_cloud.h:331
iterator begin()
Definition: point_cloud.h:442
void mirrorRows(const PointCloud< PointT > &input, PointCloud< PointT > &output, const size_t &amount)
expand point cloud mirroring amount top and bottom rows.
Definition: spring.hpp:187
A point structure representing Euclidean xyz coordinates, and the RGB color.
void duplicateColumns(const PointCloud< PointT > &input, PointCloud< PointT > &output, const size_t &amount)
expand point cloud duplicating the amount right and left columns times.
Definition: spring.hpp:97
iterator insert(iterator position, const PointT &pt)
Insert a new point in the cloud, given an iterator.
Definition: point_cloud.h:494