pcl_visualization: point_cloud_handlers.hpp Source File

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 36093 2011-02-19 21:54:55Z rusu $
00035  *
00036  */
00037 
00038 #include <terminal_tools/time.h>
00039 #include <pcl/win32_macros.h>
00040 
00042 
00045 template <typename PointT> void
00046 pcl_visualization::PointCloudColorHandlerCustom<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
00047 {
00048   if (!capable_)
00049     return;
00050 
00051   if (!scalars)
00052     scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00053   scalars->SetNumberOfComponents (3);
00054   
00055   vtkIdType nr_points = cloud_->points.size ();
00056   reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
00057 
00058   // Get a random color
00059   unsigned char* colors = new unsigned char[nr_points * 3];
00060 
00061   // Color every point
00062   for (vtkIdType cp = 0; cp < nr_points; ++cp)
00063   {
00064     colors[cp * 3 + 0] = r_;
00065     colors[cp * 3 + 1] = g_;
00066     colors[cp * 3 + 2] = b_;
00067   }
00068   reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0);
00069 }
00070 
00072 
00075 template <typename PointT> void
00076 pcl_visualization::PointCloudColorHandlerRandom<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
00077 {
00078   if (!capable_)
00079     return;
00080 
00081   if (!scalars)
00082     scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00083   scalars->SetNumberOfComponents (3);
00084   
00085   vtkIdType nr_points = cloud_->points.size ();
00086   reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
00087 
00088   // Get a random color
00089   unsigned char* colors = new unsigned char[nr_points * 3];
00090   double r, g, b;
00091   pcl_visualization::getRandomColors (r, g, b);
00092 
00093   int r_ = lrint (r * 255.0), g_ = lrint (g * 255.0), b_ = lrint (b * 255.0);
00094 
00095   // Color every point
00096   for (vtkIdType cp = 0; cp < nr_points; ++cp)
00097   {
00098     colors[cp * 3 + 0] = r_;
00099     colors[cp * 3 + 1] = g_;
00100     colors[cp * 3 + 2] = b_;
00101   }
00102   reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0);
00103 }
00104 
00106 
00107 template <typename PointT>
00108 pcl_visualization::PointCloudColorHandlerRGBField<PointT>::PointCloudColorHandlerRGBField (const typename pcl_visualization::PointCloudColorHandler<PointT>::PointCloud &cloud) : 
00109   pcl_visualization::PointCloudColorHandler<PointT>::PointCloudColorHandler (cloud)
00110 {
00111   // Handle the 24-bit packed RGB values
00112   field_idx_ = pcl::getFieldIndex (cloud, "rgb", fields_);
00113   if (field_idx_ != -1)
00114     capable_ = true;
00115   else
00116     capable_ = false;
00117 }
00118 
00120 
00123 template <typename PointT> void 
00124 pcl_visualization::PointCloudColorHandlerRGBField<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
00125 {
00126   if (!capable_)
00127     return;
00128 
00129   if (!scalars)
00130     scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00131   scalars->SetNumberOfComponents (3);
00132 
00133   vtkIdType nr_points = cloud_->points.size ();
00134   reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
00135 
00136   // Allocate enough memory to hold all colors
00137   unsigned char* colors = new unsigned char[nr_points * 3];
00138 
00139   int j = 0;
00140   // If XYZ present, check if the points are invalid
00141   int x_idx = -1;
00142   for (size_t d = 0; d < fields_.size (); ++d)
00143     if (fields_[d].name == "x")
00144       x_idx = d;
00145 
00146   if (x_idx != -1)
00147   {
00148     // Color every point
00149     for (vtkIdType cp = 0; cp < nr_points; ++cp)
00150     {
00151       // Copy the value at the specified field
00152       if (!pcl_isfinite (cloud_->points[cp].x) || !pcl_isfinite (cloud_->points[cp].rgb))
00153         continue;
00154 
00155       int rgb = *reinterpret_cast<int*>(&cloud_->points[cp].rgb);
00156       colors[j * 3 + 0] = ((rgb >> 16) & 0xff);
00157       colors[j * 3 + 1] = ((rgb >> 8) & 0xff);
00158       colors[j * 3 + 2] = (rgb & 0xff);
00159       j++;
00160     }
00161   }
00162   else
00163   {
00164     // Color every point
00165     for (vtkIdType cp = 0; cp < nr_points; ++cp)
00166     {
00167       // Copy the value at the specified field
00168       if (!pcl_isfinite (cloud_->points[cp].rgb))
00169         continue;
00170 
00171       int rgb = *reinterpret_cast<int*>(&cloud_->points[cp].rgb);
00172       colors[j * 3 + 0] = ((rgb >> 16) & 0xff);
00173       colors[j * 3 + 1] = ((rgb >> 8) & 0xff);
00174       colors[j * 3 + 2] = (rgb & 0xff);
00175       j++;
00176     }
00177   }
00178   reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * j, 0);
00179   //delete [] colors;
00180 }
00181 
00183 
00184 template <typename PointT>
00185 pcl_visualization::PointCloudColorHandlerGenericField<PointT>::PointCloudColorHandlerGenericField (const typename pcl_visualization::PointCloudColorHandler<PointT>::PointCloud &cloud, 
00186     const std::string &field_name) : pcl_visualization::PointCloudColorHandler<PointT>::PointCloudColorHandler (cloud)
00187 {
00188   field_name_ = field_name;
00189   field_idx_  = pcl::getFieldIndex (cloud, field_name, fields_);
00190   if (field_idx_ != -1)
00191     capable_ = true;
00192   else
00193     capable_ = false;
00194 }
00195 
00197 
00200 template <typename PointT> void 
00201 pcl_visualization::PointCloudColorHandlerGenericField<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
00202 {
00203   if (!capable_)
00204     return;
00205 
00206   if (!scalars)
00207     scalars = vtkSmartPointer<vtkFloatArray>::New ();
00208   scalars->SetNumberOfComponents (1);
00209 
00210   vtkIdType nr_points = cloud_->points.size ();
00211   reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
00212 
00213   typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00214 
00215   float* colors = new float[nr_points];
00216   float field_data;
00217   int j = 0;
00218   // Color every point
00219   for (vtkIdType cp = 0; cp < nr_points; ++cp)
00220   {
00221     uint8_t* pt_data = (uint8_t*)&cloud_->points[cp];
00222     memcpy (&field_data, pt_data + fields_[field_idx_].offset, sizeof (float));
00223 
00224     if (!pcl_isfinite (field_data))
00225       continue;
00226 
00227     colors[j] = field_data;
00228     j++;
00229   }
00230   reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetArray (colors, j, 0);
00231 }
00232 
00234 
00235 template <typename PointT>
00236 pcl_visualization::PointCloudGeometryHandlerXYZ<PointT>::PointCloudGeometryHandlerXYZ (const typename pcl_visualization::PointCloudGeometryHandler<PointT>::PointCloud &cloud) 
00237   : pcl_visualization::PointCloudGeometryHandler<PointT>::PointCloudGeometryHandler (cloud)
00238 {
00239   field_x_idx_ = pcl::getFieldIndex (cloud, "x", fields_);
00240   if (field_x_idx_ == -1)
00241     return;
00242   field_y_idx_ = pcl::getFieldIndex (cloud, "y", fields_);
00243   if (field_y_idx_ == -1)
00244     return;
00245   field_z_idx_ = pcl::getFieldIndex (cloud, "z", fields_);
00246   if (field_z_idx_ == -1)
00247     return;
00248   capable_ = true;
00249 }
00250 
00252 
00255 template <typename PointT> void 
00256 pcl_visualization::PointCloudGeometryHandlerXYZ<PointT>::getGeometry (vtkSmartPointer<vtkPoints> &points) const
00257 {
00258   if (!capable_)
00259     return;
00260 
00261   if (!points)
00262     points = vtkSmartPointer<vtkPoints>::New ();
00263   points->SetDataTypeToFloat ();
00264 
00265   vtkSmartPointer<vtkFloatArray> data = vtkSmartPointer<vtkFloatArray>::New ();
00266   data->SetNumberOfComponents (3);
00267   vtkIdType nr_points = cloud_->points.size ();
00268 
00269   // Add all points
00270   vtkIdType j = 0;    // true point index
00271   float* pts = new float[nr_points * 3];
00272 
00273   // If the dataset has no invalid values, just copy all of them
00274   if (cloud_->is_dense)
00275   {
00276     for (vtkIdType i = 0; i < nr_points; ++i)
00277     {
00278       pts[i * 3 + 0] = cloud_->points[i].x;
00279       pts[i * 3 + 1] = cloud_->points[i].y;
00280       pts[i * 3 + 2] = cloud_->points[i].z;
00281     }
00282     data->SetArray (&pts[0], nr_points * 3, 0);
00283     points->SetData (data);
00284   }
00285   // Need to check for NaNs, Infs, ec
00286   else
00287   {
00288     for (vtkIdType i = 0; i < nr_points; ++i)
00289     {
00290       // Check if the point is invalid
00291       if (!pcl_isfinite (cloud_->points[i].x) || !pcl_isfinite (cloud_->points[i].y) || !pcl_isfinite (cloud_->points[i].z))
00292         continue;
00293 
00294       pts[j * 3 + 0] = cloud_->points[i].x;
00295       pts[j * 3 + 1] = cloud_->points[i].y;
00296       pts[j * 3 + 2] = cloud_->points[i].z;
00297       // Set j and increment
00298       j++;
00299     }
00300     data->SetArray (&pts[0], j * 3, 0);
00301     points->SetData (data);
00302   }
00303 }
00304 
00306 
00307 template <typename PointT>
00308 pcl_visualization::PointCloudGeometryHandlerSurfaceNormal<PointT>::PointCloudGeometryHandlerSurfaceNormal (const typename pcl_visualization::PointCloudGeometryHandler<PointT>::PointCloud &cloud) 
00309   : pcl_visualization::PointCloudGeometryHandler<PointT>::PointCloudGeometryHandler (cloud)
00310 {
00311   field_x_idx_ = pcl::getFieldIndex (cloud, "normal_x", fields_);
00312   if (field_x_idx_ == -1)
00313     return;
00314   field_y_idx_ = pcl::getFieldIndex (cloud, "normal_y", fields_);
00315   if (field_y_idx_ == -1)
00316     return;
00317   field_z_idx_ = pcl::getFieldIndex (cloud, "normal_z", fields_);
00318   if (field_z_idx_ == -1)
00319     return;
00320   capable_ = true;
00321 }
00322 
00324 
00327 template <typename PointT> void 
00328 pcl_visualization::PointCloudGeometryHandlerSurfaceNormal<PointT>::getGeometry (vtkSmartPointer<vtkPoints> &points) const
00329 {
00330   if (!capable_)
00331     return;
00332 
00333   if (!points)
00334     points = vtkSmartPointer<vtkPoints>::New ();
00335   points->SetDataTypeToFloat ();
00336   points->SetNumberOfPoints (cloud_->points.size ());
00337 
00338   // Add all points
00339   double p[3];
00340   for (vtkIdType i = 0; i < (int)cloud_->points.size (); ++i)
00341   {
00342     p[0] = cloud_->points[i].normal[0];
00343     p[1] = cloud_->points[i].normal[1];
00344     p[2] = cloud_->points[i].normal[2];
00345 
00346     points->SetPoint (i, p);
00347   }
00348 }
00349 
00351 
00352 template <typename PointT>
00353 pcl_visualization::PointCloudGeometryHandlerCustom<PointT>::PointCloudGeometryHandlerCustom (const typename pcl_visualization::PointCloudGeometryHandler<PointT>::PointCloud &cloud,
00354     const std::string &x_field_name, const std::string &y_field_name, const std::string &z_field_name) : pcl_visualization::PointCloudGeometryHandler<PointT>::PointCloudGeometryHandler (cloud)
00355 {
00356   field_x_idx_ = pcl::getFieldIndex (cloud, x_field_name, fields_);
00357   if (field_x_idx_ == -1)
00358     return;
00359   field_y_idx_ = pcl::getFieldIndex (cloud, y_field_name, fields_);
00360   if (field_y_idx_ == -1)
00361     return;
00362   field_z_idx_ = pcl::getFieldIndex (cloud, z_field_name, fields_);
00363   if (field_z_idx_ == -1)
00364     return;
00365   field_name_ = x_field_name + y_field_name + z_field_name;
00366   capable_ = true;
00367 }
00368 
00370 
00373 template <typename PointT> void 
00374 pcl_visualization::PointCloudGeometryHandlerCustom<PointT>::getGeometry (vtkSmartPointer<vtkPoints> &points) const
00375 {
00376   if (!capable_)
00377     return;
00378 
00379   if (!points)
00380     points = vtkSmartPointer<vtkPoints>::New ();
00381   points->SetDataTypeToFloat ();
00382   points->SetNumberOfPoints (cloud_->points.size ());
00383 
00384   float data;
00385   // Add all points
00386   double p[3];
00387   for (vtkIdType i = 0; i < (int)cloud_->points.size (); ++i)
00388   {
00389     // Copy the value at the specified field
00390     uint8_t* pt_data = (uint8_t*)&cloud_->points[i];
00391     memcpy (&data, pt_data + fields_[field_x_idx_].offset, sizeof (float));
00392     p[0] = data;
00393 
00394     memcpy (&data, pt_data + fields_[field_y_idx_].offset, sizeof (float));
00395     p[1] = data;
00396 
00397     memcpy (&data, pt_data + fields_[field_z_idx_].offset, sizeof (float));
00398     p[2] = data;
00399 
00400     points->SetPoint (i, p);
00401   }
00402 }
00403