pcl_visualization: point_cloud_handlers.cpp 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.cpp 36093 2011-02-19 21:54:55Z rusu $
00035  *
00036  */
00037 
00038 #include <pcl_visualization/point_cloud_handlers.h>
00039 #include <terminal_tools/time.h>
00040 #include <pcl/win32_macros.h>
00041 
00043 
00046 void
00047 pcl_visualization::PointCloudColorHandlerCustom<sensor_msgs::PointCloud2>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
00048 {
00049   if (!capable_)
00050     return;
00051 
00052   if (!scalars)
00053     scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00054   scalars->SetNumberOfComponents (3);
00055   
00056   vtkIdType nr_points = cloud_->width * cloud_->height;
00057   reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
00058 
00059   // Get a random color
00060   unsigned char* colors = new unsigned char[nr_points * 3];
00061 
00062   // Color every point
00063   for (vtkIdType cp = 0; cp < nr_points; ++cp)
00064   {
00065     colors[cp * 3 + 0] = r_;
00066     colors[cp * 3 + 1] = g_;
00067     colors[cp * 3 + 2] = b_;
00068   }
00069   reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0);
00070 }
00071 
00073 
00076 void
00077 pcl_visualization::PointCloudColorHandlerRandom<sensor_msgs::PointCloud2>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
00078 {
00079   if (!capable_)
00080     return;
00081 
00082   if (!scalars)
00083     scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00084   scalars->SetNumberOfComponents (3);
00085   
00086   vtkIdType nr_points = cloud_->width * cloud_->height;
00087   reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
00088 
00089   // Get a random color
00090   unsigned char* colors = new unsigned char[nr_points * 3];
00091   double r, g, b;
00092   pcl_visualization::getRandomColors (r, g, b);
00093 
00094   int r_ = lrint (r * 255.0), g_ = lrint (g * 255.0), b_ = lrint (b * 255.0);
00095 
00096   // Color every point
00097   for (vtkIdType cp = 0; cp < nr_points; ++cp)
00098   {
00099     colors[cp * 3 + 0] = r_;
00100     colors[cp * 3 + 1] = g_;
00101     colors[cp * 3 + 2] = b_;
00102   }
00103   reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0);
00104 }
00105 
00107 
00108 pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2>::PointCloudColorHandlerRGBField (
00109     const pcl_visualization::PointCloudColorHandler<sensor_msgs::PointCloud2>::PointCloud &cloud) : 
00110   pcl_visualization::PointCloudColorHandler<sensor_msgs::PointCloud2>::PointCloudColorHandler (cloud)
00111 {
00112   // Handle the 24-bit packed RGB values
00113   field_idx_ = pcl::getFieldIndex (cloud, "rgb");
00114   if (field_idx_ != -1)
00115     capable_ = true;
00116   else
00117     capable_ = false;
00118 }
00119 
00121 
00124 void 
00125 pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
00126 {
00127   if (!capable_)
00128     return;
00129 
00130   if (!scalars)
00131     scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00132   scalars->SetNumberOfComponents (3);
00133 
00134   vtkIdType nr_points = cloud_->width * cloud_->height;
00135   reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
00136   
00137   
00138   // Allocate enough memory to hold all colors
00139   unsigned char* colors = new unsigned char[nr_points * 3];
00140 
00141   float rgb_data;
00142   int point_offset = cloud_->fields[field_idx_].offset;
00143   int j = 0;
00144   
00145   // If XYZ present, check if the points are invalid
00146   int x_idx = pcl::getFieldIndex (*cloud_, "x");
00147   if (x_idx != -1)
00148   {
00149     float x_data;
00150     int x_point_offset = cloud_->fields[x_idx].offset;
00151     
00152     // Color every point
00153     for (vtkIdType cp = 0; cp < nr_points; ++cp, 
00154                                            point_offset += cloud_->point_step, 
00155                                            x_point_offset += cloud_->point_step)
00156     {
00157       // Copy the value at the specified field
00158       memcpy (&rgb_data, &cloud_->data[point_offset], sizeof (float));
00159 
00160       if (!pcl_isfinite (rgb_data))
00161         continue;
00162 
00163       memcpy (&x_data, &cloud_->data[x_point_offset], sizeof (float));
00164       if (!pcl_isfinite (x_data))
00165         continue;
00166 
00167       int rgb = *reinterpret_cast<int*>(&rgb_data);
00168       colors[j * 3 + 0] = ((rgb >> 16) & 0xff);
00169       colors[j * 3 + 1] = ((rgb >> 8) & 0xff);
00170       colors[j * 3 + 2] = (rgb & 0xff);
00171       j++;
00172     }
00173   }
00174   // No XYZ data checks
00175   else
00176   {
00177     // Color every point
00178     for (vtkIdType cp = 0; cp < nr_points; ++cp, point_offset += cloud_->point_step)
00179     {
00180       // Copy the value at the specified field
00181       memcpy (&rgb_data, &cloud_->data[point_offset], sizeof (float));
00182 
00183       if (!pcl_isfinite (rgb_data))
00184         continue;
00185 
00186       int rgb = *reinterpret_cast<int*>(&rgb_data);
00187       colors[j * 3 + 0] = ((rgb >> 16) & 0xff);
00188       colors[j * 3 + 1] = ((rgb >> 8) & 0xff);
00189       colors[j * 3 + 2] = (rgb & 0xff);
00190       j++;
00191     }
00192   }
00193   reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * j, 0);
00194   //delete [] colors;
00195 }
00196 
00198 
00199 pcl_visualization::PointCloudColorHandlerGenericField<sensor_msgs::PointCloud2>::PointCloudColorHandlerGenericField (
00200     const pcl_visualization::PointCloudColorHandler<sensor_msgs::PointCloud2>::PointCloud &cloud, 
00201     const std::string &field_name) : pcl_visualization::PointCloudColorHandler<sensor_msgs::PointCloud2>::PointCloudColorHandler (cloud)
00202 {
00203   field_name_ = field_name;
00204   field_idx_  = pcl::getFieldIndex (cloud, field_name);
00205   if (field_idx_ != -1)
00206     capable_ = true;
00207   else
00208     capable_ = false;
00209 }
00210 
00212 
00215 void 
00216 pcl_visualization::PointCloudColorHandlerGenericField<sensor_msgs::PointCloud2>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
00217 {
00218   if (!capable_)
00219     return;
00220 
00221   if (!scalars)
00222     scalars = vtkSmartPointer<vtkFloatArray>::New ();
00223   scalars->SetNumberOfComponents (1);
00224 
00225   vtkIdType nr_points = cloud_->width * cloud_->height;
00226   reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
00227 
00228   float* colors = new float[nr_points];
00229   float field_data;
00230   int j = 0;
00231   int point_offset = cloud_->fields[field_idx_].offset;
00232   // Color every point
00233   for (vtkIdType cp = 0; cp < nr_points; ++cp, point_offset += cloud_->point_step)
00234   {
00235     // Copy the value at the specified field
00236     memcpy (&field_data, &cloud_->data[point_offset], sizeof (float));
00237 
00238     if (!pcl_isfinite (field_data))
00239       continue;
00240     colors[j] = field_data;
00241     j++;
00242   }
00243   reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetArray (colors, j, 0);
00244 }
00245 
00247 
00248 
00250 
00253 void 
00254 pcl_visualization::PointCloudGeometryHandler<sensor_msgs::PointCloud2>::getGeometry (vtkSmartPointer<vtkPoints> &points) const
00255 {
00256   if (!capable_)
00257     return;
00258 
00259   if (!points)
00260     points = vtkSmartPointer<vtkPoints>::New ();
00261   points->SetDataTypeToFloat ();
00262 
00263   vtkSmartPointer<vtkFloatArray> data = vtkSmartPointer<vtkFloatArray>::New ();
00264   data->SetNumberOfComponents (3);
00265   vtkIdType nr_points = cloud_->width * cloud_->height;
00266 
00267   // Add all points
00268   float dim;
00269   vtkIdType j = 0;    // true point index
00270   float* pts = new float[nr_points * 3];
00271   int point_offset = 0;
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, point_offset+=cloud_->point_step)
00277     {
00278       // Copy the value at the specified field
00279       memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_x_idx_].offset], sizeof (float));
00280       pts[i * 3 + 0] = dim;
00281 
00282       memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_y_idx_].offset], sizeof (float));
00283       pts[i * 3 + 1] = dim;
00284 
00285       memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_z_idx_].offset], sizeof (float));
00286       pts[i * 3 + 2] = dim;
00287     }
00288     data->SetArray (&pts[0], nr_points * 3, 0);
00289     points->SetData (data);
00290   }
00291   else
00292   {
00293     for (vtkIdType i = 0; i < nr_points; ++i, point_offset+=cloud_->point_step)
00294     {
00295       // Copy the value at the specified field
00296       memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_x_idx_].offset], sizeof (float));
00297       if (!pcl_isfinite (dim))
00298         continue;
00299       pts[j * 3 + 0] = dim;
00300 
00301       memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_y_idx_].offset], sizeof (float));
00302       if (!pcl_isfinite (dim))
00303         continue;
00304       pts[j * 3 + 1] = dim;
00305 
00306       memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_z_idx_].offset], sizeof (float));
00307       if (!pcl_isfinite (dim))
00308         continue;
00309       pts[j * 3 + 2] = dim;
00310 
00311       // Set j and increment
00312       j++;
00313     }
00314     data->SetArray (&pts[0], j * 3, 0);
00315     points->SetData (data);
00316   }
00317 }
00318 
00320 
00321 pcl_visualization::PointCloudGeometryHandlerXYZ<sensor_msgs::PointCloud2>::PointCloudGeometryHandlerXYZ (const PointCloud &cloud)
00322 : pcl_visualization::PointCloudGeometryHandler<sensor_msgs::PointCloud2>::PointCloudGeometryHandler (cloud)
00323 {
00324   field_x_idx_ = pcl::getFieldIndex (cloud, "x");
00325   if (field_x_idx_ == -1)
00326     return;
00327   field_y_idx_ = pcl::getFieldIndex (cloud, "y");
00328   if (field_y_idx_ == -1)
00329     return;
00330   field_z_idx_ = pcl::getFieldIndex (cloud, "z");
00331   if (field_z_idx_ == -1)
00332     return;
00333   capable_ = true;
00334 }
00335 
00336 
00337 
00339 
00340 pcl_visualization::PointCloudGeometryHandlerSurfaceNormal<sensor_msgs::PointCloud2>::PointCloudGeometryHandlerSurfaceNormal (const PointCloud &cloud) 
00341 : pcl_visualization::PointCloudGeometryHandler<sensor_msgs::PointCloud2>::PointCloudGeometryHandler (cloud)
00342 {
00343   field_x_idx_ = pcl::getFieldIndex (cloud, "normal_x");
00344   if (field_x_idx_ == -1)
00345     return;
00346   field_y_idx_ = pcl::getFieldIndex (cloud, "normal_y");
00347   if (field_y_idx_ == -1)
00348     return;
00349   field_z_idx_ = pcl::getFieldIndex (cloud, "normal_z");
00350   if (field_z_idx_ == -1)
00351     return;
00352   capable_ = true;
00353 }
00354 
00356 
00357 pcl_visualization::PointCloudGeometryHandlerCustom<sensor_msgs::PointCloud2>::PointCloudGeometryHandlerCustom (
00358     const PointCloud &cloud, const std::string &x_field_name, const std::string &y_field_name, const std::string &z_field_name) 
00359 : pcl_visualization::PointCloudGeometryHandler<sensor_msgs::PointCloud2>::PointCloudGeometryHandler (cloud)
00360 {
00361   field_x_idx_ = pcl::getFieldIndex (cloud, x_field_name);
00362   if (field_x_idx_ == -1)
00363     return;
00364   field_y_idx_ = pcl::getFieldIndex (cloud, y_field_name);
00365   if (field_y_idx_ == -1)
00366     return;
00367   field_z_idx_ = pcl::getFieldIndex (cloud, z_field_name);
00368   if (field_z_idx_ == -1)
00369     return;
00370   field_name_ = x_field_name + y_field_name + z_field_name;
00371   capable_ = true;
00372 }
00373