Point Cloud Library (PCL)
1.3.1
|
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: pcl_base.h 3006 2011-10-31 23:25:06Z rusu $ 00037 * 00038 */ 00039 #ifndef PCL_PCL_BASE_H_ 00040 #define PCL_PCL_BASE_H_ 00041 00042 #include <cstddef> 00043 // Eigen includes 00044 #include <Eigen/StdVector> 00045 // STD includes 00046 #include <vector> 00047 00048 // Include PCL macros such as PCL_ERROR, etc 00049 #include "pcl/pcl_macros.h" 00050 00051 // Boost includes. Needed everywhere. 00052 #include <boost/shared_ptr.hpp> 00053 00054 // Point Cloud message includes. Needed everywhere. 00055 #include <sensor_msgs/PointCloud2.h> 00056 #include "pcl/point_cloud.h" 00057 #include "pcl/PointIndices.h" 00058 #include "pcl/win32_macros.h" 00059 00060 #include <pcl/console/print.h> 00061 00062 namespace pcl 00063 { 00064 // definitions used everywhere 00065 typedef boost::shared_ptr <std::vector<int> > IndicesPtr; 00066 typedef boost::shared_ptr <const std::vector<int> > IndicesConstPtr; 00067 00069 00070 template <typename PointT> 00071 class PCLBase 00072 { 00073 public: 00074 typedef pcl::PointCloud<PointT> PointCloud; 00075 typedef typename PointCloud::Ptr PointCloudPtr; 00076 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00077 00078 typedef PointIndices::Ptr PointIndicesPtr; 00079 typedef PointIndices::ConstPtr PointIndicesConstPtr; 00080 00082 PCLBase () : input_ (), indices_ (), use_indices_ (false), fake_indices_ (false) {} 00083 00085 virtual ~PCLBase() 00086 { 00087 input_.reset (); 00088 indices_.reset (); 00089 } 00090 00094 virtual inline void 00095 setInputCloud (const PointCloudConstPtr &cloud) { input_ = cloud; } 00096 00098 inline PointCloudConstPtr const 00099 getInputCloud () { return (input_); } 00100 00104 inline void 00105 setIndices (const IndicesPtr &indices) 00106 { 00107 indices_ = indices; 00108 fake_indices_ = false; 00109 use_indices_ = true; 00110 } 00111 00115 inline void 00116 setIndices (const PointIndicesConstPtr &indices) 00117 { 00118 indices_.reset (new std::vector<int> (indices->indices)); 00119 fake_indices_ = false; 00120 use_indices_ = true; 00121 } 00122 00131 inline void 00132 setIndices (size_t row_start, size_t col_start, size_t nb_rows, size_t nb_cols) 00133 { 00134 if ((nb_rows > input_->height) || (row_start > input_->height)) 00135 { 00136 PCL_ERROR ("[PCLBase::setIndices] cloud is only %d height", input_->height); 00137 return; 00138 } 00139 00140 if ((nb_cols > input_->width) || (col_start > input_->width)) 00141 { 00142 PCL_ERROR ("[PCLBase::setIndices] cloud is only %d width", input_->width); 00143 return; 00144 } 00145 00146 size_t row_end = row_start + nb_rows; 00147 if (row_end > input_->height) 00148 { 00149 PCL_ERROR ("[PCLBase::setIndices] %d is out of rows range %d", row_end, input_->height); 00150 return; 00151 } 00152 00153 size_t col_end = col_start + nb_cols; 00154 if (col_end > input_->width) 00155 { 00156 PCL_ERROR ("[PCLBase::setIndices] %d is out of columns range %d", col_end, input_->width); 00157 return; 00158 } 00159 00160 indices_.reset (new std::vector<int>); 00161 indices_->reserve (nb_cols * nb_rows); 00162 for(size_t i = row_start; i < row_end; i++) 00163 for(size_t j = col_start; j < col_end; j++) 00164 indices_->push_back ((i * input_->width) + j); 00165 fake_indices_ = false; 00166 use_indices_ = true; 00167 } 00168 00170 inline IndicesPtr const 00171 getIndices () { return (indices_); } 00172 00178 const PointT& operator[] (size_t pos) 00179 { 00180 return ((*input_)[(*indices_)[pos]]); 00181 } 00182 00183 protected: 00185 PointCloudConstPtr input_; 00186 00188 IndicesPtr indices_; 00189 00191 bool use_indices_; 00192 00194 bool fake_indices_; 00195 00208 inline bool 00209 initCompute () 00210 { 00211 // Check if input was set 00212 if (!input_) 00213 return (false); 00214 00215 // If no point indices have been given, construct a set of indices for the entire input point cloud 00216 if (!indices_) 00217 { 00218 fake_indices_ = true; 00219 indices_.reset (new std::vector<int>); 00220 try 00221 { 00222 indices_->resize (input_->points.size ()); 00223 } 00224 catch (std::bad_alloc) 00225 { 00226 PCL_ERROR ("[initCompute] Failed to allocate %lu indices.\n", (unsigned long)input_->points.size ()); 00227 } 00228 for (size_t i = 0; i < indices_->size (); ++i) { (*indices_)[i] = (int) i; } 00229 } 00230 00231 // If we have a set of fake indices, but they do not match the number of points in the cloud, update them 00232 if (fake_indices_ && indices_->size () != input_->points.size ()) 00233 { 00234 size_t indices_size = indices_->size (); 00235 indices_->resize (input_->points.size ()); 00236 for (size_t i = indices_size; i < indices_->size (); ++i) { (*indices_)[i] = (int) i; } 00237 } 00238 00239 return (true); 00240 } 00241 00245 inline bool 00246 deinitCompute () 00247 { 00248 return (true); 00249 } 00250 public: 00251 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00252 }; 00253 00255 template <> 00256 class PCL_EXPORTS PCLBase<sensor_msgs::PointCloud2> 00257 { 00258 public: 00259 typedef sensor_msgs::PointCloud2 PointCloud2; 00260 typedef PointCloud2::Ptr PointCloud2Ptr; 00261 typedef PointCloud2::ConstPtr PointCloud2ConstPtr; 00262 00263 typedef PointIndices::Ptr PointIndicesPtr; 00264 typedef PointIndices::ConstPtr PointIndicesConstPtr; 00265 00267 PCLBase () : input_ (), indices_ (), use_indices_ (false), fake_indices_ (false), 00268 x_field_name_ ("x"), y_field_name_ ("y"), z_field_name_ ("z") 00269 {}; 00270 00272 virtual ~PCLBase() 00273 { 00274 input_.reset (); 00275 indices_.reset (); 00276 } 00277 00281 void 00282 setInputCloud (const PointCloud2ConstPtr &cloud); 00283 00285 inline PointCloud2ConstPtr const 00286 getInputCloud () { return (input_); } 00287 00291 inline void 00292 setIndices (const IndicesPtr &indices) 00293 { 00294 indices_ = indices; 00295 fake_indices_ = false; 00296 use_indices_ = true; 00297 } 00298 00302 inline void 00303 setIndices (const PointIndicesConstPtr &indices) 00304 { 00305 indices_.reset (new std::vector<int> (indices->indices)); 00306 fake_indices_ = false; 00307 use_indices_ = true; 00308 } 00309 00311 inline IndicesPtr const 00312 getIndices () { return (indices_); } 00313 00314 protected: 00316 PointCloud2ConstPtr input_; 00317 00319 IndicesPtr indices_; 00320 00322 bool use_indices_; 00323 00325 bool fake_indices_; 00326 00328 std::vector<int> field_sizes_; 00329 00331 int x_idx_, y_idx_, z_idx_; 00332 00334 std::string x_field_name_, y_field_name_, z_field_name_; 00335 00336 bool initCompute (); 00337 bool deinitCompute (); 00338 public: 00339 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00340 }; 00341 } 00342 00343 #endif //#ifndef PCL_PCL_BASE_H_