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) 2012-, Open Perception, 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: point_cloud_handlers.hpp 7678 2012-10-22 20:54:04Z rusu $ 00037 * 00038 */ 00039 #ifndef PCL_POINT_CLOUD_GEOMETRY_HANDLERS_IMPL_HPP_ 00040 #define PCL_POINT_CLOUD_GEOMETRY_HANDLERS_IMPL_HPP_ 00041 00042 #include <pcl/pcl_macros.h> 00043 00044 /////////////////////////////////////////////////////////////////////////////////////////// 00045 template <typename PointT> 00046 pcl::visualization::PointCloudGeometryHandlerXYZ<PointT>::PointCloudGeometryHandlerXYZ (const PointCloudConstPtr &cloud) 00047 : pcl::visualization::PointCloudGeometryHandler<PointT>::PointCloudGeometryHandler (cloud) 00048 { 00049 field_x_idx_ = pcl::getFieldIndex (*cloud, "x", fields_); 00050 if (field_x_idx_ == -1) 00051 return; 00052 field_y_idx_ = pcl::getFieldIndex (*cloud, "y", fields_); 00053 if (field_y_idx_ == -1) 00054 return; 00055 field_z_idx_ = pcl::getFieldIndex (*cloud, "z", fields_); 00056 if (field_z_idx_ == -1) 00057 return; 00058 capable_ = true; 00059 } 00060 00061 /////////////////////////////////////////////////////////////////////////////////////////// 00062 template <typename PointT> void 00063 pcl::visualization::PointCloudGeometryHandlerXYZ<PointT>::getGeometry (vtkSmartPointer<vtkPoints> &points) const 00064 { 00065 if (!capable_) 00066 return; 00067 00068 if (!points) 00069 points = vtkSmartPointer<vtkPoints>::New (); 00070 points->SetDataTypeToFloat (); 00071 00072 vtkSmartPointer<vtkFloatArray> data = vtkSmartPointer<vtkFloatArray>::New (); 00073 data->SetNumberOfComponents (3); 00074 vtkIdType nr_points = cloud_->points.size (); 00075 00076 // Add all points 00077 vtkIdType j = 0; // true point index 00078 float* pts = static_cast<float*> (malloc (nr_points * 3 * sizeof (float))); 00079 00080 // If the dataset has no invalid values, just copy all of them 00081 if (cloud_->is_dense) 00082 { 00083 for (vtkIdType i = 0; i < nr_points; ++i) 00084 { 00085 pts[i * 3 + 0] = cloud_->points[i].x; 00086 pts[i * 3 + 1] = cloud_->points[i].y; 00087 pts[i * 3 + 2] = cloud_->points[i].z; 00088 } 00089 data->SetArray (&pts[0], nr_points * 3, 0); 00090 points->SetData (data); 00091 } 00092 // Need to check for NaNs, Infs, ec 00093 else 00094 { 00095 for (vtkIdType i = 0; i < nr_points; ++i) 00096 { 00097 // Check if the point is invalid 00098 if (!pcl_isfinite (cloud_->points[i].x) || !pcl_isfinite (cloud_->points[i].y) || !pcl_isfinite (cloud_->points[i].z)) 00099 continue; 00100 00101 pts[j * 3 + 0] = cloud_->points[i].x; 00102 pts[j * 3 + 1] = cloud_->points[i].y; 00103 pts[j * 3 + 2] = cloud_->points[i].z; 00104 // Set j and increment 00105 j++; 00106 } 00107 data->SetArray (&pts[0], j * 3, 0); 00108 points->SetData (data); 00109 } 00110 } 00111 00112 /////////////////////////////////////////////////////////////////////////////////////////// 00113 template <typename PointT> 00114 pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<PointT>::PointCloudGeometryHandlerSurfaceNormal (const PointCloudConstPtr &cloud) 00115 : pcl::visualization::PointCloudGeometryHandler<PointT>::PointCloudGeometryHandler (cloud) 00116 { 00117 field_x_idx_ = pcl::getFieldIndex (*cloud, "normal_x", fields_); 00118 if (field_x_idx_ == -1) 00119 return; 00120 field_y_idx_ = pcl::getFieldIndex (*cloud, "normal_y", fields_); 00121 if (field_y_idx_ == -1) 00122 return; 00123 field_z_idx_ = pcl::getFieldIndex (*cloud, "normal_z", fields_); 00124 if (field_z_idx_ == -1) 00125 return; 00126 capable_ = true; 00127 } 00128 00129 /////////////////////////////////////////////////////////////////////////////////////////// 00130 template <typename PointT> void 00131 pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<PointT>::getGeometry (vtkSmartPointer<vtkPoints> &points) const 00132 { 00133 if (!capable_) 00134 return; 00135 00136 if (!points) 00137 points = vtkSmartPointer<vtkPoints>::New (); 00138 points->SetDataTypeToFloat (); 00139 points->SetNumberOfPoints (cloud_->points.size ()); 00140 00141 // Add all points 00142 double p[3]; 00143 for (vtkIdType i = 0; i < static_cast<vtkIdType> (cloud_->points.size ()); ++i) 00144 { 00145 p[0] = cloud_->points[i].normal[0]; 00146 p[1] = cloud_->points[i].normal[1]; 00147 p[2] = cloud_->points[i].normal[2]; 00148 00149 points->SetPoint (i, p); 00150 } 00151 } 00152 00153 #define PCL_INSTANTIATE_PointCloudGeometryHandlerXYZ(T) template class PCL_EXPORTS pcl::visualization::PointCloudGeometryHandlerXYZ<T>; 00154 #define PCL_INSTANTIATE_PointCloudGeometryHandlerSurfaceNormal(T) template class PCL_EXPORTS pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<T>; 00155 00156 #endif // PCL_POINT_CLOUD_GEOMETRY_HANDLERS_IMPL_HPP_ 00157