Point Cloud Library (PCL)  1.3.1
point_cloud_handlers.hpp
Go to the documentation of this 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 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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines