spring.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: spring.hpp 4251 2012-02-04 00:33:47Z nizar $
00037  *
00038  */
00039 
00040 #ifndef PCL_POINT_CLOUD_SPRING_IMPL_HPP_
00041 #define PCL_POINT_CLOUD_SPRING_IMPL_HPP_
00042 
00043 template <typename PointT> void 
00044 pcl::common::expandColumns (const PointCloud<PointT>& input, PointCloud<PointT>& output, 
00045                             const PointT& val, const size_t& amount)
00046 {
00047   if (amount <= 0)
00048     PCL_THROW_EXCEPTION (InitFailedException,
00049                          "[pcl::common::expandColumns] error: amount must be ]0.."
00050                          << (input.width/2) << "] !");
00051 
00052   if (!input.isOrganized () || amount > (input.width/2))
00053     PCL_THROW_EXCEPTION (InitFailedException,
00054                          "[pcl::common::expandColumns] error: " 
00055                          << "columns expansion requires organised point cloud");
00056 
00057   uint32_t old_height = input.height;
00058   uint32_t old_width = input.width;
00059   uint32_t new_width = old_width + 2*amount;
00060   if (&input != &output)
00061     output = input;
00062   output.reserve (new_width * old_height);
00063   for (int j = 0; j < output.height; ++j)
00064   {
00065     typename PointCloud<PointT>::iterator start = output.begin() + (j * new_width);
00066     output.insert (start, amount, val);
00067     start = output.begin() + (j * new_width) + old_width + amount;
00068     output.insert (start, amount, val);
00069     output.height = old_height;
00070   }
00071   output.width = new_width;
00072   output.height = old_height;
00073 }
00074 
00075 template <typename PointT> void 
00076 pcl::common::expandRows (const PointCloud<PointT>& input, PointCloud<PointT>& output,
00077                          const PointT& val, const size_t& amount)
00078 {
00079   if (amount <= 0)
00080     PCL_THROW_EXCEPTION (InitFailedException,
00081                          "[pcl::common::expandRows] error: amount must be ]0.."
00082                          << (input.height/2) << "] !");
00083 
00084   uint32_t old_height = input.height;
00085   uint32_t new_height = old_height + 2*amount;
00086   uint32_t old_width = input.width;
00087   if (&input != &output)
00088     output = input;
00089   output.reserve (new_height * old_width);
00090   output.insert (output.begin (), amount * old_width, val);
00091   output.insert (output.end (), amount * old_width, val);
00092   output.width = old_width;
00093   output.height = new_height;
00094 }
00095 
00096 template <typename PointT> void 
00097 pcl::common::duplicateColumns (const PointCloud<PointT>& input, PointCloud<PointT>& output,
00098                                const size_t& amount)
00099 {
00100   if (amount <= 0)
00101     PCL_THROW_EXCEPTION (InitFailedException,
00102                          "[pcl::common::duplicateColumns] error: amount must be ]0.."
00103                          << (input.width/2) << "] !");
00104 
00105   if (!input.isOrganized () || amount > (input.width/2))
00106     PCL_THROW_EXCEPTION (InitFailedException,
00107                          "[pcl::common::duplicateColumns] error: " 
00108                          << "columns expansion requires organised point cloud");
00109 
00110   size_t old_height = input.height;
00111   size_t old_width = input.width;
00112   size_t new_width = old_width + 2*amount;
00113   if (&input != &output)
00114     output = input;
00115   output.reserve (new_width * old_height);
00116   for (size_t j = 0; j < old_height; ++j)
00117     for(size_t i = 0; i < amount; ++i)
00118     {
00119       typename PointCloud<PointT>::iterator start = output.begin () + (j * new_width);
00120       output.insert (start, *start);
00121       start = output.begin () + (j * new_width) + old_width + i;
00122       output.insert (start, *start);
00123     }
00124 
00125   output.width = new_width;
00126   output.height = old_height;
00127 }
00128 
00129 template <typename PointT> void 
00130 pcl::common::duplicateRows (const PointCloud<PointT>& input, PointCloud<PointT>& output,
00131                             const size_t& amount)
00132 {
00133   if (amount <= 0 || amount > (input.height/2))
00134     PCL_THROW_EXCEPTION (InitFailedException,
00135                          "[pcl::common::duplicateRows] error: amount must be ]0.." 
00136                          << (input.height/2) << "] !");
00137 
00138   uint32_t old_height = input.height;
00139   uint32_t new_height = old_height + 2*amount;
00140   uint32_t old_width = input.width;
00141   if (&input != &output)
00142     output = input;
00143   output.reserve (new_height * old_width);
00144   for(size_t i = 0; i < amount; ++i)
00145   {
00146     output.insert (output.begin (), output.begin (), output.begin () + old_width);
00147     output.insert (output.end (), output.end () - old_width, output.end ());
00148   }
00149 
00150   output.width = old_width;
00151   output.height = new_height;
00152 }
00153 
00154 template <typename PointT> void 
00155 pcl::common::mirrorColumns (const PointCloud<PointT>& input, PointCloud<PointT>& output,
00156                                   const size_t& amount)
00157 {
00158   if (amount <= 0)
00159     PCL_THROW_EXCEPTION (InitFailedException,
00160                          "[pcl::common::mirrorColumns] error: amount must be ]0.."
00161                          << (input.width/2) << "] !");
00162 
00163   if (!input.isOrganized () || amount > (input.width/2))
00164     PCL_THROW_EXCEPTION (InitFailedException,
00165                          "[pcl::common::mirrorColumns] error: " 
00166                          << "columns expansion requires organised point cloud");
00167 
00168   size_t old_height = input.height;
00169   size_t old_width = input.width;
00170   size_t new_width = old_width + 2*amount;
00171   if (&input != &output)
00172     output = input;
00173   output.reserve (new_width * old_height);
00174   for (size_t j = 0; j < old_height; ++j)
00175     for(size_t i = 0; i < amount; ++i)
00176     {
00177       typename PointCloud<PointT>::iterator start = output.begin () + (j * new_width);
00178       output.insert (start, *(start + 2*i));
00179       start = output.begin () + (j * new_width) + old_width + 2*i;
00180       output.insert (start+1, *(start - 2*i));
00181     }
00182   output.width = new_width;
00183   output.height = old_height;
00184 }
00185 
00186 template <typename PointT> void 
00187 pcl::common::mirrorRows (const PointCloud<PointT>& input, PointCloud<PointT>& output,
00188                          const size_t& amount)
00189 {
00190   if (amount <= 0 || amount > (input.height/2))
00191     PCL_THROW_EXCEPTION (InitFailedException,
00192                          "[pcl::common::mirrorRows] error: amount must be ]0.." 
00193                          << (input.height/2) << "] !");
00194 
00195   uint32_t old_height = input.height;
00196   uint32_t new_height = old_height + 2*amount;
00197   uint32_t old_width = input.width;
00198   if (&input != &output)
00199     output = input;
00200   output.reserve (new_height * old_width);
00201   for(size_t i = 0; i < amount; i++)
00202   {
00203     typename PointCloud<PointT>::iterator up;
00204     if (output.height % 2 ==  0)
00205       up = output.begin () + (2*i) * old_width;
00206     else
00207       up = output.begin () + (2*i+1) * old_width;
00208     output.insert (output.begin (), up, up + old_width);
00209     typename PointCloud<PointT>::iterator bottom = output.end () - (2*i+1) * old_width;
00210     output.insert (output.end (), bottom, bottom + old_width);
00211   }
00212   output.width = old_width;
00213   output.height = new_height;
00214 }
00215 
00216 template <typename PointT> void 
00217 pcl::common::deleteRows (const PointCloud<PointT>& input, PointCloud<PointT>& output,
00218                          const size_t& amount)
00219 {
00220   if (amount <= 0 || amount > (input.height/2))
00221     PCL_THROW_EXCEPTION (InitFailedException,
00222                          "[pcl::common::deleteRows] error: amount must be ]0.." 
00223                          << (input.height/2) << "] !");
00224 
00225   uint32_t old_height = input.height;
00226   uint32_t old_width = input.width;
00227   output.erase (output.begin (), output.begin () + amount * old_width);
00228   output.erase (output.end () - amount * old_width, output.end ());
00229   output.height = old_height - 2*amount;
00230   output.width = old_width;
00231 }
00232 
00233 template <typename PointT> void 
00234 pcl::common::deleteCols (const PointCloud<PointT>& input, PointCloud<PointT>& output,
00235                          const size_t& amount)
00236 {
00237   if (amount <= 0 || amount > (input.width/2))
00238     PCL_THROW_EXCEPTION (InitFailedException,
00239                          "[pcl::common::deleteCols] error: amount must be in ]0.."
00240                          << (input.width/2) << "] !");
00241 
00242   if (!input.isOrganized ())
00243     PCL_THROW_EXCEPTION (InitFailedException,
00244                          "[pcl::common::deleteCols] error: " 
00245                          << "columns delete requires organised point cloud");
00246 
00247   uint32_t old_height = input.height;
00248   uint32_t old_width = input.width;
00249   uint32_t new_width = old_width - 2 * amount;
00250   for(size_t j = 0; j < old_height; j++)
00251   {
00252     typename PointCloud<PointT>::iterator start = output.begin () + j * new_width;
00253     output.erase (start, start + amount);
00254     start = output.begin () + (j+1) * new_width;
00255     output.erase (start, start + amount);    
00256   }
00257   output.height = old_height;
00258   output.width = new_width;
00259 }
00260 
00261 #endif


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:18:02