Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * $Id: point_cloud_handlers.hpp 3299 2011-12-01 23:29:36Z rusu $ 00035 * 00036 */ 00037 00038 #include <pcl/console/time.h> 00039 #include <pcl/win32_macros.h> 00040 00042 template <typename PointT> void 00043 pcl::visualization::PointCloudColorHandlerCustom<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const 00044 { 00045 if (!capable_) 00046 return; 00047 00048 if (!scalars) 00049 scalars = vtkSmartPointer<vtkUnsignedCharArray>::New (); 00050 scalars->SetNumberOfComponents (3); 00051 00052 vtkIdType nr_points = cloud_->points.size (); 00053 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points); 00054 00055 // Get a random color 00056 unsigned char* colors = new unsigned char[nr_points * 3]; 00057 00058 // Color every point 00059 for (vtkIdType cp = 0; cp < nr_points; ++cp) 00060 { 00061 colors[cp * 3 + 0] = r_; 00062 colors[cp * 3 + 1] = g_; 00063 colors[cp * 3 + 2] = b_; 00064 } 00065 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0); 00066 } 00067 00069 template <typename PointT> void 00070 pcl::visualization::PointCloudColorHandlerRandom<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const 00071 { 00072 if (!capable_) 00073 return; 00074 00075 if (!scalars) 00076 scalars = vtkSmartPointer<vtkUnsignedCharArray>::New (); 00077 scalars->SetNumberOfComponents (3); 00078 00079 vtkIdType nr_points = cloud_->points.size (); 00080 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points); 00081 00082 // Get a random color 00083 unsigned char* colors = new unsigned char[nr_points * 3]; 00084 double r, g, b; 00085 pcl::visualization::getRandomColors (r, g, b); 00086 00087 int r_ = pcl_lrint (r * 255.0), g_ = pcl_lrint (g * 255.0), b_ = pcl_lrint (b * 255.0); 00088 00089 // Color every point 00090 for (vtkIdType cp = 0; cp < nr_points; ++cp) 00091 { 00092 colors[cp * 3 + 0] = r_; 00093 colors[cp * 3 + 1] = g_; 00094 colors[cp * 3 + 2] = b_; 00095 } 00096 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0); 00097 } 00098 00100 template <typename PointT> 00101 pcl::visualization::PointCloudColorHandlerRGBField<PointT>::PointCloudColorHandlerRGBField (const PointCloudConstPtr &cloud) : 00102 pcl::visualization::PointCloudColorHandler<PointT>::PointCloudColorHandler (cloud) 00103 { 00104 // Handle the 24-bit packed RGB values 00105 field_idx_ = pcl::getFieldIndex (*cloud, "rgb", fields_); 00106 if (field_idx_ != -1) 00107 { 00108 capable_ = true; 00109 return; 00110 } 00111 else 00112 { 00113 field_idx_ = pcl::getFieldIndex (*cloud, "rgba", fields_); 00114 if (field_idx_ != -1) 00115 capable_ = true; 00116 else 00117 capable_ = false; 00118 } 00119 } 00120 00122 template <typename PointT> void 00123 pcl::visualization::PointCloudColorHandlerRGBField<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const 00124 { 00125 if (!capable_) 00126 return; 00127 00128 if (!scalars) 00129 scalars = vtkSmartPointer<vtkUnsignedCharArray>::New (); 00130 scalars->SetNumberOfComponents (3); 00131 00132 vtkIdType nr_points = cloud_->points.size (); 00133 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points); 00134 unsigned char* colors = reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->GetPointer (0); 00135 00136 int j = 0; 00137 // If XYZ present, check if the points are invalid 00138 int x_idx = -1; 00139 for (size_t d = 0; d < fields_.size (); ++d) 00140 if (fields_[d].name == "x") 00141 x_idx = (int) d; 00142 00143 if (x_idx != -1) 00144 { 00145 // Color every point 00146 for (vtkIdType cp = 0; cp < nr_points; ++cp) 00147 { 00148 // Copy the value at the specified field 00149 if (!pcl_isfinite (cloud_->points[cp].x) || 00150 !pcl_isfinite (cloud_->points[cp].y) || 00151 !pcl_isfinite (cloud_->points[cp].z)) 00152 continue; 00153 00154 int idx = j * 3; 00155 colors[idx ] = cloud_->points[cp].r; 00156 colors[idx + 1] = cloud_->points[cp].g; 00157 colors[idx + 2] = cloud_->points[cp].b; 00158 j++; 00159 } 00160 } 00161 else 00162 { 00163 // Color every point 00164 for (vtkIdType cp = 0; cp < nr_points; ++cp) 00165 { 00166 int idx = cp * 3; 00167 colors[idx ] = cloud_->points[cp].r; 00168 colors[idx + 1] = cloud_->points[cp].g; 00169 colors[idx + 2] = cloud_->points[cp].b; 00170 } 00171 } 00172 } 00173 00175 template <typename PointT> 00176 pcl::visualization::PointCloudColorHandlerGenericField<PointT>::PointCloudColorHandlerGenericField (const PointCloudConstPtr &cloud, 00177 const std::string &field_name) : pcl::visualization::PointCloudColorHandler<PointT>::PointCloudColorHandler (cloud) 00178 { 00179 field_name_ = field_name; 00180 field_idx_ = pcl::getFieldIndex (*cloud, field_name, fields_); 00181 if (field_idx_ != -1) 00182 capable_ = true; 00183 else 00184 capable_ = false; 00185 } 00186 00188 template <typename PointT> void 00189 pcl::visualization::PointCloudColorHandlerGenericField<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const 00190 { 00191 if (!capable_) 00192 return; 00193 00194 if (!scalars) 00195 scalars = vtkSmartPointer<vtkFloatArray>::New (); 00196 scalars->SetNumberOfComponents (1); 00197 00198 vtkIdType nr_points = cloud_->points.size (); 00199 reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetNumberOfTuples (nr_points); 00200 00201 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00202 00203 float* colors = new float[nr_points]; 00204 float field_data; 00205 00206 int j = 0; 00207 // If XYZ present, check if the points are invalid 00208 int x_idx = -1; 00209 for (size_t d = 0; d < fields_.size (); ++d) 00210 if (fields_[d].name == "x") 00211 x_idx = d; 00212 00213 if (x_idx != -1) 00214 { 00215 // Color every point 00216 for (vtkIdType cp = 0; cp < nr_points; ++cp) 00217 { 00218 // Copy the value at the specified field 00219 if (!pcl_isfinite (cloud_->points[cp].x) || !pcl_isfinite (cloud_->points[cp].y) || !pcl_isfinite (cloud_->points[cp].z)) 00220 continue; 00221 00222 uint8_t* pt_data = (uint8_t*)&cloud_->points[cp]; 00223 //memcpy (&field_data, pt_data + fields_[field_idx_].offset, sizeof (float)); 00224 memcpy (&field_data, pt_data + fields_[field_idx_].offset, pcl::getFieldSize (fields_[field_idx_].datatype)); 00225 00226 if (!pcl_isfinite (field_data)) 00227 continue; 00228 00229 colors[j] = field_data; 00230 j++; 00231 } 00232 } 00233 else 00234 { 00235 // Color every point 00236 for (vtkIdType cp = 0; cp < nr_points; ++cp) 00237 { 00238 uint8_t* pt_data = (uint8_t*)&cloud_->points[cp]; 00239 //memcpy (&field_data, pt_data + fields_[field_idx_].offset, sizeof (float)); 00240 memcpy (&field_data, pt_data + fields_[field_idx_].offset, pcl::getFieldSize (fields_[field_idx_].datatype)); 00241 00242 if (!pcl_isfinite (field_data)) 00243 continue; 00244 00245 colors[j] = field_data; 00246 j++; 00247 } 00248 } 00249 reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetArray (colors, j, 0); 00250 } 00251 00253 template <typename PointT> 00254 pcl::visualization::PointCloudGeometryHandlerXYZ<PointT>::PointCloudGeometryHandlerXYZ (const PointCloudConstPtr &cloud) 00255 : pcl::visualization::PointCloudGeometryHandler<PointT>::PointCloudGeometryHandler (cloud) 00256 { 00257 field_x_idx_ = pcl::getFieldIndex (*cloud, "x", fields_); 00258 if (field_x_idx_ == -1) 00259 return; 00260 field_y_idx_ = pcl::getFieldIndex (*cloud, "y", fields_); 00261 if (field_y_idx_ == -1) 00262 return; 00263 field_z_idx_ = pcl::getFieldIndex (*cloud, "z", fields_); 00264 if (field_z_idx_ == -1) 00265 return; 00266 capable_ = true; 00267 } 00268 00270 template <typename PointT> void 00271 pcl::visualization::PointCloudGeometryHandlerXYZ<PointT>::getGeometry (vtkSmartPointer<vtkPoints> &points) const 00272 { 00273 if (!capable_) 00274 return; 00275 00276 if (!points) 00277 points = vtkSmartPointer<vtkPoints>::New (); 00278 points->SetDataTypeToFloat (); 00279 00280 vtkSmartPointer<vtkFloatArray> data = vtkSmartPointer<vtkFloatArray>::New (); 00281 data->SetNumberOfComponents (3); 00282 vtkIdType nr_points = cloud_->points.size (); 00283 00284 // Add all points 00285 vtkIdType j = 0; // true point index 00286 float* pts = new float[nr_points * 3]; 00287 00288 // If the dataset has no invalid values, just copy all of them 00289 if (cloud_->is_dense) 00290 { 00291 for (vtkIdType i = 0; i < nr_points; ++i) 00292 { 00293 pts[i * 3 + 0] = cloud_->points[i].x; 00294 pts[i * 3 + 1] = cloud_->points[i].y; 00295 pts[i * 3 + 2] = cloud_->points[i].z; 00296 } 00297 data->SetArray (&pts[0], nr_points * 3, 0); 00298 points->SetData (data); 00299 } 00300 // Need to check for NaNs, Infs, ec 00301 else 00302 { 00303 for (vtkIdType i = 0; i < nr_points; ++i) 00304 { 00305 // Check if the point is invalid 00306 if (!pcl_isfinite (cloud_->points[i].x) || !pcl_isfinite (cloud_->points[i].y) || !pcl_isfinite (cloud_->points[i].z)) 00307 continue; 00308 00309 pts[j * 3 + 0] = cloud_->points[i].x; 00310 pts[j * 3 + 1] = cloud_->points[i].y; 00311 pts[j * 3 + 2] = cloud_->points[i].z; 00312 // Set j and increment 00313 j++; 00314 } 00315 data->SetArray (&pts[0], j * 3, 0); 00316 points->SetData (data); 00317 } 00318 } 00319 00321 template <typename PointT> 00322 pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<PointT>::PointCloudGeometryHandlerSurfaceNormal (const PointCloudConstPtr &cloud) 00323 : pcl::visualization::PointCloudGeometryHandler<PointT>::PointCloudGeometryHandler (cloud) 00324 { 00325 field_x_idx_ = pcl::getFieldIndex (*cloud, "normal_x", fields_); 00326 if (field_x_idx_ == -1) 00327 return; 00328 field_y_idx_ = pcl::getFieldIndex (*cloud, "normal_y", fields_); 00329 if (field_y_idx_ == -1) 00330 return; 00331 field_z_idx_ = pcl::getFieldIndex (*cloud, "normal_z", fields_); 00332 if (field_z_idx_ == -1) 00333 return; 00334 capable_ = true; 00335 } 00336 00338 template <typename PointT> void 00339 pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<PointT>::getGeometry (vtkSmartPointer<vtkPoints> &points) const 00340 { 00341 if (!capable_) 00342 return; 00343 00344 if (!points) 00345 points = vtkSmartPointer<vtkPoints>::New (); 00346 points->SetDataTypeToFloat (); 00347 points->SetNumberOfPoints (cloud_->points.size ()); 00348 00349 // Add all points 00350 double p[3]; 00351 for (vtkIdType i = 0; i < (int)cloud_->points.size (); ++i) 00352 { 00353 p[0] = cloud_->points[i].normal[0]; 00354 p[1] = cloud_->points[i].normal[1]; 00355 p[2] = cloud_->points[i].normal[2]; 00356 00357 points->SetPoint (i, p); 00358 } 00359 } 00360 00362 template <typename PointT> 00363 pcl::visualization::PointCloudGeometryHandlerCustom<PointT>::PointCloudGeometryHandlerCustom (const PointCloudConstPtr &cloud, 00364 const std::string &x_field_name, const std::string &y_field_name, const std::string &z_field_name) : pcl::visualization::PointCloudGeometryHandler<PointT>::PointCloudGeometryHandler (cloud) 00365 { 00366 field_x_idx_ = pcl::getFieldIndex (*cloud, x_field_name, fields_); 00367 if (field_x_idx_ == -1) 00368 return; 00369 field_y_idx_ = pcl::getFieldIndex (*cloud, y_field_name, fields_); 00370 if (field_y_idx_ == -1) 00371 return; 00372 field_z_idx_ = pcl::getFieldIndex (*cloud, z_field_name, fields_); 00373 if (field_z_idx_ == -1) 00374 return; 00375 field_name_ = x_field_name + y_field_name + z_field_name; 00376 capable_ = true; 00377 } 00378 00380 template <typename PointT> void 00381 pcl::visualization::PointCloudGeometryHandlerCustom<PointT>::getGeometry (vtkSmartPointer<vtkPoints> &points) const 00382 { 00383 if (!capable_) 00384 return; 00385 00386 if (!points) 00387 points = vtkSmartPointer<vtkPoints>::New (); 00388 points->SetDataTypeToFloat (); 00389 points->SetNumberOfPoints (cloud_->points.size ()); 00390 00391 float data; 00392 // Add all points 00393 double p[3]; 00394 for (vtkIdType i = 0; i < (int)cloud_->points.size (); ++i) 00395 { 00396 // Copy the value at the specified field 00397 uint8_t* pt_data = (uint8_t*)&cloud_->points[i]; 00398 memcpy (&data, pt_data + fields_[field_x_idx_].offset, sizeof (float)); 00399 p[0] = data; 00400 00401 memcpy (&data, pt_data + fields_[field_y_idx_].offset, sizeof (float)); 00402 p[1] = data; 00403 00404 memcpy (&data, pt_data + fields_[field_z_idx_].offset, sizeof (float)); 00405 p[2] = data; 00406 00407 points->SetPoint (i, p); 00408 } 00409 } 00410