Point Cloud Library (PCL)
1.7.0
|
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 the copyright holder(s) 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$ 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