pcl_visualization: pcl_visualization: point_cloud

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 36250 2011-02-25 19:57:32Z 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 
00233   // If XYZ present, check if the points are invalid
00234   int x_idx = pcl::getFieldIndex (*cloud_, "x");
00235   if (x_idx != -1)
00236   {
00237     float x_data;
00238     int x_point_offset = cloud_->fields[x_idx].offset;
00239     
00240     // Color every point
00241     for (vtkIdType cp = 0; cp < nr_points; ++cp,
00242                                            point_offset += cloud_->point_step, 
00243                                            x_point_offset += cloud_->point_step)
00244     {
00245       // Copy the value at the specified field
00246       memcpy (&field_data, &cloud_->data[point_offset], sizeof (float));
00247 
00248       if (!pcl_isfinite (field_data))
00249         continue;
00250 
00251       memcpy (&x_data, &cloud_->data[x_point_offset], sizeof (float));
00252       if (!pcl_isfinite (x_data))
00253         continue;
00254 
00255       colors[j] = field_data;
00256       j++;
00257     }
00258   }
00259   // No XYZ data checks
00260   else
00261   {
00262     // Color every point
00263     for (vtkIdType cp = 0; cp < nr_points; ++cp, point_offset += cloud_->point_step)
00264     {
00265       // Copy the value at the specified field
00266       memcpy (&field_data, &cloud_->data[point_offset], sizeof (float));
00267 
00268       if (!pcl_isfinite (field_data))
00269         continue;
00270       colors[j] = field_data;
00271       j++;
00272     }
00273   }
00274   reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetArray (colors, j, 0);
00275 }
00276 
00278 
00279 
00281 
00284 void 
00285 pcl_visualization::PointCloudGeometryHandler<sensor_msgs::PointCloud2>::getGeometry (vtkSmartPointer<vtkPoints> &points) const
00286 {
00287   if (!capable_)
00288     return;
00289 
00290   if (!points)
00291     points = vtkSmartPointer<vtkPoints>::New ();
00292   points->SetDataTypeToFloat ();
00293 
00294   vtkSmartPointer<vtkFloatArray> data = vtkSmartPointer<vtkFloatArray>::New ();
00295   data->SetNumberOfComponents (3);
00296   vtkIdType nr_points = cloud_->width * cloud_->height;
00297 
00298   // Add all points
00299   float dim;
00300   vtkIdType j = 0;    // true point index
00301   float* pts = new float[nr_points * 3];
00302   int point_offset = 0;
00303 
00304   // If the dataset has no invalid values, just copy all of them
00305   if (cloud_->is_dense)
00306   {
00307     for (vtkIdType i = 0; i < nr_points; ++i, point_offset+=cloud_->point_step)
00308     {
00309       // Copy the value at the specified field
00310       memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_x_idx_].offset], sizeof (float));
00311       pts[i * 3 + 0] = dim;
00312 
00313       memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_y_idx_].offset], sizeof (float));
00314       pts[i * 3 + 1] = dim;
00315 
00316       memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_z_idx_].offset], sizeof (float));
00317       pts[i * 3 + 2] = dim;
00318     }
00319     data->SetArray (&pts[0], nr_points * 3, 0);
00320     points->SetData (data);
00321   }
00322   else
00323   {
00324     for (vtkIdType i = 0; i < nr_points; ++i, point_offset+=cloud_->point_step)
00325     {
00326       // Copy the value at the specified field
00327       memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_x_idx_].offset], sizeof (float));
00328       if (!pcl_isfinite (dim))
00329         continue;
00330       pts[j * 3 + 0] = dim;
00331 
00332       memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_y_idx_].offset], sizeof (float));
00333       if (!pcl_isfinite (dim))
00334         continue;
00335       pts[j * 3 + 1] = dim;
00336 
00337       memcpy (&dim, &cloud_->data[point_offset + cloud_->fields[field_z_idx_].offset], sizeof (float));
00338       if (!pcl_isfinite (dim))
00339         continue;
00340       pts[j * 3 + 2] = dim;
00341 
00342       // Set j and increment
00343       j++;
00344     }
00345     data->SetArray (&pts[0], j * 3, 0);
00346     points->SetData (data);
00347   }
00348 }
00349 
00351 
00352 pcl_visualization::PointCloudGeometryHandlerXYZ<sensor_msgs::PointCloud2>::PointCloudGeometryHandlerXYZ (const PointCloud &cloud)
00353 : pcl_visualization::PointCloudGeometryHandler<sensor_msgs::PointCloud2>::PointCloudGeometryHandler (cloud)
00354 {
00355   field_x_idx_ = pcl::getFieldIndex (cloud, "x");
00356   if (field_x_idx_ == -1)
00357     return;
00358   field_y_idx_ = pcl::getFieldIndex (cloud, "y");
00359   if (field_y_idx_ == -1)
00360     return;
00361   field_z_idx_ = pcl::getFieldIndex (cloud, "z");
00362   if (field_z_idx_ == -1)
00363     return;
00364   capable_ = true;
00365 }
00366 
00367 
00368 
00370 
00371 pcl_visualization::PointCloudGeometryHandlerSurfaceNormal<sensor_msgs::PointCloud2>::PointCloudGeometryHandlerSurfaceNormal (const PointCloud &cloud) 
00372 : pcl_visualization::PointCloudGeometryHandler<sensor_msgs::PointCloud2>::PointCloudGeometryHandler (cloud)
00373 {
00374   field_x_idx_ = pcl::getFieldIndex (cloud, "normal_x");
00375   if (field_x_idx_ == -1)
00376     return;
00377   field_y_idx_ = pcl::getFieldIndex (cloud, "normal_y");
00378   if (field_y_idx_ == -1)
00379     return;
00380   field_z_idx_ = pcl::getFieldIndex (cloud, "normal_z");
00381   if (field_z_idx_ == -1)
00382     return;
00383   capable_ = true;
00384 }
00385 
00387 
00388 pcl_visualization::PointCloudGeometryHandlerCustom<sensor_msgs::PointCloud2>::PointCloudGeometryHandlerCustom (
00389     const PointCloud &cloud, const std::string &x_field_name, const std::string &y_field_name, const std::string &z_field_name) 
00390 : pcl_visualization::PointCloudGeometryHandler<sensor_msgs::PointCloud2>::PointCloudGeometryHandler (cloud)
00391 {
00392   field_x_idx_ = pcl::getFieldIndex (cloud, x_field_name);
00393   if (field_x_idx_ == -1)
00394     return;
00395   field_y_idx_ = pcl::getFieldIndex (cloud, y_field_name);
00396   if (field_y_idx_ == -1)
00397     return;
00398   field_z_idx_ = pcl::getFieldIndex (cloud, z_field_name);
00399   if (field_z_idx_ == -1)
00400     return;
00401   field_name_ = x_field_name + y_field_name + z_field_name;
00402   capable_ = true;
00403 }
00404