$search
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