Point Cloud Library (PCL)  1.11.1-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 #pragma once
41 
42 #include <pcl/common/spring.h>
43 
44 
45 namespace pcl
46 {
47 
48 namespace common
49 {
50 
51 template <typename PointT> void
53  const PointT& val, const std::size_t& amount)
54 {
55  if (amount <= 0)
56  PCL_THROW_EXCEPTION (InitFailedException,
57  "[pcl::common::expandColumns] error: amount must be ]0.."
58  << (input.width/2) << "] !");
59 
60  if (!input.isOrganized () || amount > (input.width/2))
61  PCL_THROW_EXCEPTION (InitFailedException,
62  "[pcl::common::expandColumns] error: "
63  << "columns expansion requires organised point cloud");
64 
65  std::uint32_t old_height = input.height;
66  std::uint32_t old_width = input.width;
67  std::uint32_t new_width = old_width + 2*amount;
68  if (&input != &output)
69  output = input;
70  output.reserve (new_width * old_height);
71  for (int j = 0; j < output.height; ++j)
72  {
73  typename PointCloud<PointT>::iterator start = output.begin() + (j * new_width);
74  output.insert (start, amount, val);
75  start = output.begin() + (j * new_width) + old_width + amount;
76  output.insert (start, amount, val);
77  output.height = old_height;
78  }
79  output.width = new_width;
80  output.height = old_height;
81 }
82 
83 template <typename PointT> void
85  const PointT& val, const std::size_t& amount)
86 {
87  if (amount <= 0)
88  PCL_THROW_EXCEPTION (InitFailedException,
89  "[pcl::common::expandRows] error: amount must be ]0.."
90  << (input.height/2) << "] !");
91 
92  std::uint32_t old_height = input.height;
93  std::uint32_t new_height = old_height + 2*amount;
94  std::uint32_t old_width = input.width;
95  if (&input != &output)
96  output = input;
97  output.reserve (new_height * old_width);
98  output.insert (output.begin (), amount * old_width, val);
99  output.insert (output.end (), amount * old_width, val);
100  output.width = old_width;
101  output.height = new_height;
102 }
103 
104 template <typename PointT> void
106  const std::size_t& amount)
107 {
108  if (amount <= 0)
109  PCL_THROW_EXCEPTION (InitFailedException,
110  "[pcl::common::duplicateColumns] error: amount must be ]0.."
111  << (input.width/2) << "] !");
112 
113  if (!input.isOrganized () || amount > (input.width/2))
114  PCL_THROW_EXCEPTION (InitFailedException,
115  "[pcl::common::duplicateColumns] error: "
116  << "columns expansion requires organised point cloud");
117 
118  std::size_t old_height = input.height;
119  std::size_t old_width = input.width;
120  std::size_t new_width = old_width + 2*amount;
121  if (&input != &output)
122  output = input;
123  output.reserve (new_width * old_height);
124  for (std::size_t j = 0; j < old_height; ++j)
125  for(std::size_t i = 0; i < amount; ++i)
126  {
127  typename PointCloud<PointT>::iterator start = output.begin () + (j * new_width);
128  output.insert (start, *start);
129  start = output.begin () + (j * new_width) + old_width + i;
130  output.insert (start, *start);
131  }
132 
133  output.width = new_width;
134  output.height = old_height;
135 }
136 
137 template <typename PointT> void
139  const std::size_t& amount)
140 {
141  if (amount <= 0 || amount > (input.height/2))
142  PCL_THROW_EXCEPTION (InitFailedException,
143  "[pcl::common::duplicateRows] error: amount must be ]0.."
144  << (input.height/2) << "] !");
145 
146  std::uint32_t old_height = input.height;
147  std::uint32_t new_height = old_height + 2*amount;
148  std::uint32_t old_width = input.width;
149  if (&input != &output)
150  output = input;
151  output.reserve (new_height * old_width);
152  for(std::size_t i = 0; i < amount; ++i)
153  {
154  output.insert (output.begin (), output.begin (), output.begin () + old_width);
155  output.insert (output.end (), output.end () - old_width, output.end ());
156  }
157 
158  output.width = old_width;
159  output.height = new_height;
160 }
161 
162 template <typename PointT> void
164  const std::size_t& amount)
165 {
166  if (amount <= 0)
167  PCL_THROW_EXCEPTION (InitFailedException,
168  "[pcl::common::mirrorColumns] error: amount must be ]0.."
169  << (input.width/2) << "] !");
170 
171  if (!input.isOrganized () || amount > (input.width/2))
172  PCL_THROW_EXCEPTION (InitFailedException,
173  "[pcl::common::mirrorColumns] error: "
174  << "columns expansion requires organised point cloud");
175 
176  std::size_t old_height = input.height;
177  std::size_t old_width = input.width;
178  std::size_t new_width = old_width + 2*amount;
179  if (&input != &output)
180  output = input;
181  output.reserve (new_width * old_height);
182  for (std::size_t j = 0; j < old_height; ++j)
183  for(std::size_t i = 0; i < amount; ++i)
184  {
185  typename PointCloud<PointT>::iterator start = output.begin () + (j * new_width);
186  output.insert (start, *(start + 2*i));
187  start = output.begin () + (j * new_width) + old_width + 2*i;
188  output.insert (start+1, *(start - 2*i));
189  }
190  output.width = new_width;
191  output.height = old_height;
192 }
193 
194 template <typename PointT> void
196  const std::size_t& amount)
197 {
198  if (amount <= 0 || amount > (input.height/2))
199  PCL_THROW_EXCEPTION (InitFailedException,
200  "[pcl::common::mirrorRows] error: amount must be ]0.."
201  << (input.height/2) << "] !");
202 
203  std::uint32_t old_height = input.height;
204  std::uint32_t new_height = old_height + 2*amount;
205  std::uint32_t old_width = input.width;
206  if (&input != &output)
207  output = input;
208  output.reserve (new_height * old_width);
209  for(std::size_t i = 0; i < amount; i++)
210  {
211  const auto extra_odd = output.height % 2;
212  auto up = output.begin () + (2*i + extra_odd) * old_width;
213  output.insert (output.begin (), up, up + old_width);
214  auto bottom = output.end () - (2*i+1) * old_width;
215  output.insert (output.end (), bottom, bottom + old_width);
216  }
217  output.width = old_width;
218  output.height = new_height;
219 }
220 
221 template <typename PointT> void
223  const std::size_t& amount)
224 {
225  if (amount <= 0 || amount > (input.height/2))
226  PCL_THROW_EXCEPTION (InitFailedException,
227  "[pcl::common::deleteRows] error: amount must be ]0.."
228  << (input.height/2) << "] !");
229 
230  std::uint32_t old_height = input.height;
231  std::uint32_t old_width = input.width;
232  output.erase (output.begin (), output.begin () + amount * old_width);
233  output.erase (output.end () - amount * old_width, output.end ());
234  output.height = old_height - 2*amount;
235  output.width = old_width;
236 }
237 
238 template <typename PointT> void
240  const std::size_t& amount)
241 {
242  if (amount <= 0 || amount > (input.width/2))
243  PCL_THROW_EXCEPTION (InitFailedException,
244  "[pcl::common::deleteCols] error: amount must be in ]0.."
245  << (input.width/2) << "] !");
246 
247  if (!input.isOrganized ())
248  PCL_THROW_EXCEPTION (InitFailedException,
249  "[pcl::common::deleteCols] error: "
250  << "columns delete requires organised point cloud");
251 
252  std::uint32_t old_height = input.height;
253  std::uint32_t old_width = input.width;
254  std::uint32_t new_width = old_width - 2 * amount;
255  for(std::size_t j = 0; j < old_height; j++)
256  {
257  typename PointCloud<PointT>::iterator start = output.begin () + j * new_width;
258  output.erase (start, start + amount);
259  start = output.begin () + (j+1) * new_width;
260  output.erase (start, start + amount);
261  }
262  output.height = old_height;
263  output.width = new_width;
264 }
265 
266 } // namespace common
267 } // namespace pcl
268 
pcl
Definition: convolution.h:46
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:394
pcl::common::deleteCols
void deleteCols(const PointCloud< PointT > &input, PointCloud< PointT > &output, const std::size_t &amount)
delete amount columns in top and bottom of point cloud
Definition: spring.hpp:239
pcl::common::expandColumns
void expandColumns(const PointCloud< PointT > &input, PointCloud< PointT > &output, const PointT &val, const std::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:52
pcl::PointCloud::begin
iterator begin() noexcept
Definition: point_cloud.h:423
pcl::common::mirrorRows
void mirrorRows(const PointCloud< PointT > &input, PointCloud< PointT > &output, const std::size_t &amount)
expand point cloud mirroring amount top and bottom rows.
Definition: spring.hpp:195
pcl::PointCloud::erase
iterator erase(iterator position)
Erase a point in the cloud.
Definition: point_cloud.h:799
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::common::duplicateRows
void duplicateRows(const PointCloud< PointT > &input, PointCloud< PointT > &output, const std::size_t &amount)
expand point cloud duplicating the amount top and bottom rows times.
Definition: spring.hpp:138
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::common::mirrorColumns
void mirrorColumns(const PointCloud< PointT > &input, PointCloud< PointT > &output, const std::size_t &amount)
expand point cloud mirroring amount right and left columns.
Definition: spring.hpp:163
pcl::common::deleteRows
void deleteRows(const PointCloud< PointT > &input, PointCloud< PointT > &output, const std::size_t &amount)
delete amount rows in top and bottom of point cloud
Definition: spring.hpp:222
pcl::PointCloud::isOrganized
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
Definition: point_cloud.h:310
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:392
pcl::common::duplicateColumns
void duplicateColumns(const PointCloud< PointT > &input, PointCloud< PointT > &output, const std::size_t &amount)
expand point cloud duplicating the amount right and left columns times.
Definition: spring.hpp:105
pcl::common::expandRows
void expandRows(const PointCloud< PointT > &input, PointCloud< PointT > &output, const PointT &val, const std::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:84
pcl::PointCloud::end
iterator end() noexcept
Definition: point_cloud.h:424
pcl::PointCloud::reserve
void reserve(std::size_t n)
Definition: point_cloud.h:439
pcl::PointCloud::insert
iterator insert(iterator position, const PointT &pt)
Insert a new point in the cloud, given an iterator.
Definition: point_cloud.h:692
pcl::InitFailedException
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition: exceptions.h:193
pcl::PointCloud::iterator
typename VectorType::iterator iterator
Definition: point_cloud.h:419