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 */ 00037 00038 #ifndef PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_ 00039 #define PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_ 00040 00041 #include <pcl/pcl_macros.h> 00042 00043 /////////////////////////////////////////////////////////////////////////////////////////// 00044 template <typename PointT> bool 00045 pcl::visualization::PointCloudColorHandlerCustom<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const 00046 { 00047 if (!capable_ || !cloud_) 00048 return (false); 00049 00050 if (!scalars) 00051 scalars = vtkSmartPointer<vtkUnsignedCharArray>::New (); 00052 scalars->SetNumberOfComponents (3); 00053 00054 vtkIdType nr_points = cloud_->points.size (); 00055 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points); 00056 00057 // Get a random color 00058 unsigned char* colors = new unsigned char[nr_points * 3]; 00059 00060 // Color every point 00061 for (vtkIdType cp = 0; cp < nr_points; ++cp) 00062 { 00063 colors[cp * 3 + 0] = static_cast<unsigned char> (r_); 00064 colors[cp * 3 + 1] = static_cast<unsigned char> (g_); 00065 colors[cp * 3 + 2] = static_cast<unsigned char> (b_); 00066 } 00067 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0); 00068 return (true); 00069 } 00070 00071 /////////////////////////////////////////////////////////////////////////////////////////// 00072 template <typename PointT> bool 00073 pcl::visualization::PointCloudColorHandlerRandom<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const 00074 { 00075 if (!capable_ || !cloud_) 00076 return (false); 00077 00078 if (!scalars) 00079 scalars = vtkSmartPointer<vtkUnsignedCharArray>::New (); 00080 scalars->SetNumberOfComponents (3); 00081 00082 vtkIdType nr_points = cloud_->points.size (); 00083 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points); 00084 00085 // Get a random color 00086 unsigned char* colors = new unsigned char[nr_points * 3]; 00087 double r, g, b; 00088 pcl::visualization::getRandomColors (r, g, b); 00089 00090 int r_ = static_cast<int> (pcl_lrint (r * 255.0)), 00091 g_ = static_cast<int> (pcl_lrint (g * 255.0)), 00092 b_ = static_cast<int> (pcl_lrint (b * 255.0)); 00093 00094 // Color every point 00095 for (vtkIdType cp = 0; cp < nr_points; ++cp) 00096 { 00097 colors[cp * 3 + 0] = static_cast<unsigned char> (r_); 00098 colors[cp * 3 + 1] = static_cast<unsigned char> (g_); 00099 colors[cp * 3 + 2] = static_cast<unsigned char> (b_); 00100 } 00101 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0); 00102 return (true); 00103 } 00104 00105 /////////////////////////////////////////////////////////////////////////////////////////// 00106 template <typename PointT> void 00107 pcl::visualization::PointCloudColorHandlerRGBField<PointT>::setInputCloud ( 00108 const PointCloudConstPtr &cloud) 00109 { 00110 PointCloudColorHandler<PointT>::setInputCloud (cloud); 00111 // Handle the 24-bit packed RGB values 00112 field_idx_ = pcl::getFieldIndex (*cloud, "rgb", fields_); 00113 if (field_idx_ != -1) 00114 { 00115 capable_ = true; 00116 return; 00117 } 00118 else 00119 { 00120 field_idx_ = pcl::getFieldIndex (*cloud, "rgba", fields_); 00121 if (field_idx_ != -1) 00122 capable_ = true; 00123 else 00124 capable_ = false; 00125 } 00126 } 00127 00128 /////////////////////////////////////////////////////////////////////////////////////////// 00129 template <typename PointT> bool 00130 pcl::visualization::PointCloudColorHandlerRGBField<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const 00131 { 00132 if (!capable_ || !cloud_) 00133 return (false); 00134 00135 if (!scalars) 00136 scalars = vtkSmartPointer<vtkUnsignedCharArray>::New (); 00137 scalars->SetNumberOfComponents (3); 00138 00139 vtkIdType nr_points = cloud_->points.size (); 00140 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points); 00141 unsigned char* colors = reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->GetPointer (0); 00142 00143 int j = 0; 00144 // If XYZ present, check if the points are invalid 00145 int x_idx = -1; 00146 for (size_t d = 0; d < fields_.size (); ++d) 00147 if (fields_[d].name == "x") 00148 x_idx = static_cast<int> (d); 00149 00150 if (x_idx != -1) 00151 { 00152 // Color every point 00153 for (vtkIdType cp = 0; cp < nr_points; ++cp) 00154 { 00155 // Copy the value at the specified field 00156 if (!pcl_isfinite (cloud_->points[cp].x) || 00157 !pcl_isfinite (cloud_->points[cp].y) || 00158 !pcl_isfinite (cloud_->points[cp].z)) 00159 continue; 00160 00161 colors[j ] = cloud_->points[cp].r; 00162 colors[j + 1] = cloud_->points[cp].g; 00163 colors[j + 2] = cloud_->points[cp].b; 00164 j += 3; 00165 } 00166 } 00167 else 00168 { 00169 // Color every point 00170 for (vtkIdType cp = 0; cp < nr_points; ++cp) 00171 { 00172 int idx = static_cast<int> (cp) * 3; 00173 colors[idx ] = cloud_->points[cp].r; 00174 colors[idx + 1] = cloud_->points[cp].g; 00175 colors[idx + 2] = cloud_->points[cp].b; 00176 } 00177 } 00178 return (true); 00179 } 00180 00181 /////////////////////////////////////////////////////////////////////////////////////////// 00182 template <typename PointT> 00183 pcl::visualization::PointCloudColorHandlerHSVField<PointT>::PointCloudColorHandlerHSVField (const PointCloudConstPtr &cloud) : 00184 pcl::visualization::PointCloudColorHandler<PointT>::PointCloudColorHandler (cloud) 00185 { 00186 // Check for the presence of the "H" field 00187 field_idx_ = pcl::getFieldIndex (*cloud, "h", fields_); 00188 if (field_idx_ == -1) 00189 { 00190 capable_ = false; 00191 return; 00192 } 00193 00194 // Check for the presence of the "S" field 00195 s_field_idx_ = pcl::getFieldIndex (*cloud, "s", fields_); 00196 if (s_field_idx_ == -1) 00197 { 00198 capable_ = false; 00199 return; 00200 } 00201 00202 // Check for the presence of the "V" field 00203 v_field_idx_ = pcl::getFieldIndex (*cloud, "v", fields_); 00204 if (v_field_idx_ == -1) 00205 { 00206 capable_ = false; 00207 return; 00208 } 00209 capable_ = true; 00210 } 00211 00212 /////////////////////////////////////////////////////////////////////////////////////////// 00213 template <typename PointT> bool 00214 pcl::visualization::PointCloudColorHandlerHSVField<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const 00215 { 00216 if (!capable_ || !cloud_) 00217 return (false); 00218 00219 if (!scalars) 00220 scalars = vtkSmartPointer<vtkUnsignedCharArray>::New (); 00221 scalars->SetNumberOfComponents (3); 00222 00223 vtkIdType nr_points = cloud_->points.size (); 00224 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points); 00225 unsigned char* colors = reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->GetPointer (0); 00226 00227 int j = 0; 00228 // If XYZ present, check if the points are invalid 00229 int x_idx = -1; 00230 00231 for (size_t d = 0; d < fields_.size (); ++d) 00232 if (fields_[d].name == "x") 00233 x_idx = static_cast<int> (d); 00234 00235 if (x_idx != -1) 00236 { 00237 // Color every point 00238 for (vtkIdType cp = 0; cp < nr_points; ++cp) 00239 { 00240 // Copy the value at the specified field 00241 if (!pcl_isfinite (cloud_->points[cp].x) || 00242 !pcl_isfinite (cloud_->points[cp].y) || 00243 !pcl_isfinite (cloud_->points[cp].z)) 00244 continue; 00245 00246 int idx = j * 3; 00247 00248 ///@todo do this with the point_types_conversion in common, first template it! 00249 00250 // Fill color data with HSV here: 00251 if (cloud_->points[cp].s == 0) 00252 { 00253 colors[idx] = colors[idx+1] = colors[idx+2] = cloud_->points[cp].v; 00254 return; 00255 } 00256 float a = cloud_->points[cp].h / 60; 00257 int i = floor (a); 00258 float f = a - i; 00259 float p = cloud_->points[cp].v * (1 - cloud_->points[cp].s); 00260 float q = cloud_->points[cp].v * (1 - cloud_->points[cp].s * f); 00261 float t = cloud_->points[cp].v * (1 - cloud_->points[cp].s * (1 - f)); 00262 00263 switch (i) 00264 { 00265 case 0: 00266 colors[idx] = cloud_->points[cp].v; colors[idx+1] = t; colors[idx+2] = p; break; 00267 case 1: 00268 colors[idx] = q; colors[idx+1] = cloud_->points[cp].v; colors[idx+2] = p; break; 00269 case 2: 00270 colors[idx] = p; colors[idx+1] = cloud_->points[cp].v; colors[idx+2] = t; break; 00271 case 3: 00272 colors[idx] = p; colors[idx+1] = q; colors[idx+2] = cloud_->points[cp].v; break; 00273 case 4: 00274 colors[idx] = t; colors[idx+1] = p; colors[idx+2] = cloud_->points[cp].v; break; 00275 default: 00276 colors[idx] = cloud_->points[cp].v; colors[idx+1] = p; colors[idx+2] = q; break; 00277 } 00278 j++; 00279 } 00280 } 00281 else 00282 { 00283 // Color every point 00284 for (vtkIdType cp = 0; cp < nr_points; ++cp) 00285 { 00286 int idx = cp * 3; 00287 00288 // Fill color data with HSV here: 00289 if (cloud_->points[cp].s == 0) 00290 { 00291 colors[idx] = colors[idx+1] = colors[idx+2] = cloud_->points[cp].v; 00292 return; 00293 } 00294 float a = cloud_->points[cp].h / 60; 00295 int i = floor (a); 00296 float f = a - i; 00297 float p = cloud_->points[cp].v * (1 - cloud_->points[cp].s); 00298 float q = cloud_->points[cp].v * (1 - cloud_->points[cp].s * f); 00299 float t = cloud_->points[cp].v * (1 - cloud_->points[cp].s * (1 - f)); 00300 00301 switch (i) 00302 { 00303 case 0: 00304 colors[idx] = cloud_->points[cp].v; colors[idx+1] = t; colors[idx+2] = p; break; 00305 case 1: 00306 colors[idx] = q; colors[idx+1] = cloud_->points[cp].v; colors[idx+2] = p; break; 00307 case 2: 00308 colors[idx] = p; colors[idx+1] = cloud_->points[cp].v; colors[idx+2] = t; break; 00309 case 3: 00310 colors[idx] = p; colors[idx+1] = q; colors[idx+2] = cloud_->points[cp].v; break; 00311 case 4: 00312 colors[idx] = t; colors[idx+1] = p; colors[idx+2] = cloud_->points[cp].v; break; 00313 default: 00314 colors[idx] = cloud_->points[cp].v; colors[idx+1] = p; colors[idx+2] = q; break; 00315 } 00316 } 00317 } 00318 return (true); 00319 } 00320 00321 /////////////////////////////////////////////////////////////////////////////////////////// 00322 template <typename PointT> void 00323 pcl::visualization::PointCloudColorHandlerGenericField<PointT>::setInputCloud ( 00324 const PointCloudConstPtr &cloud) 00325 { 00326 PointCloudColorHandler<PointT>::setInputCloud (cloud); 00327 field_idx_ = pcl::getFieldIndex (*cloud, field_name_, fields_); 00328 if (field_idx_ != -1) 00329 capable_ = true; 00330 else 00331 capable_ = false; 00332 } 00333 00334 /////////////////////////////////////////////////////////////////////////////////////////// 00335 template <typename PointT> bool 00336 pcl::visualization::PointCloudColorHandlerGenericField<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const 00337 { 00338 if (!capable_ || !cloud_) 00339 return (false); 00340 00341 if (!scalars) 00342 scalars = vtkSmartPointer<vtkFloatArray>::New (); 00343 scalars->SetNumberOfComponents (1); 00344 00345 vtkIdType nr_points = cloud_->points.size (); 00346 reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetNumberOfTuples (nr_points); 00347 00348 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00349 00350 float* colors = new float[nr_points]; 00351 float field_data; 00352 00353 int j = 0; 00354 // If XYZ present, check if the points are invalid 00355 int x_idx = -1; 00356 for (size_t d = 0; d < fields_.size (); ++d) 00357 if (fields_[d].name == "x") 00358 x_idx = static_cast<int> (d); 00359 00360 if (x_idx != -1) 00361 { 00362 // Color every point 00363 for (vtkIdType cp = 0; cp < nr_points; ++cp) 00364 { 00365 // Copy the value at the specified field 00366 if (!pcl_isfinite (cloud_->points[cp].x) || !pcl_isfinite (cloud_->points[cp].y) || !pcl_isfinite (cloud_->points[cp].z)) 00367 continue; 00368 00369 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud_->points[cp]); 00370 memcpy (&field_data, pt_data + fields_[field_idx_].offset, pcl::getFieldSize (fields_[field_idx_].datatype)); 00371 00372 colors[j] = field_data; 00373 j++; 00374 } 00375 } 00376 else 00377 { 00378 // Color every point 00379 for (vtkIdType cp = 0; cp < nr_points; ++cp) 00380 { 00381 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud_->points[cp]); 00382 memcpy (&field_data, pt_data + fields_[field_idx_].offset, pcl::getFieldSize (fields_[field_idx_].datatype)); 00383 00384 if (!pcl_isfinite (field_data)) 00385 continue; 00386 00387 colors[j] = field_data; 00388 j++; 00389 } 00390 } 00391 reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetArray (colors, j, 0); 00392 return (true); 00393 } 00394 00395 #endif // PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_ 00396