Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id: vtk_io.hpp 4968 2012-05-03 06:39:52Z doria $ 00037 * 00038 */ 00039 00040 #ifndef PCL_IO_VTK_IO_IMPL_H_ 00041 #define PCL_IO_VTK_IO_IMPL_H_ 00042 00043 // PCL 00044 #include <pcl/common/io.h> 00045 #include <pcl/io/pcd_io.h> 00046 #include <pcl/point_types.h> 00047 #include <pcl/point_traits.h> 00048 00049 // VTK 00050 // Ignore warnings in the above headers 00051 #ifdef __GNUC__ 00052 #pragma GCC system_header 00053 #endif 00054 #include <vtkFloatArray.h> 00055 #include <vtkPointData.h> 00056 #include <vtkPoints.h> 00057 #include <vtkPolyData.h> 00058 #include <vtkUnsignedCharArray.h> 00059 #include <vtkSmartPointer.h> 00060 #include <vtkStructuredGrid.h> 00061 #include <vtkVertexGlyphFilter.h> 00062 00063 /////////////////////////////////////////////////////////////////////////////////////////// 00064 template <typename PointT> void 00065 pcl::io::vtkPolyDataToPointCloud (vtkPolyData* const polydata, pcl::PointCloud<PointT>& cloud) 00066 { 00067 // This generic template will convert any VTK PolyData 00068 // to a coordinate-only PointXYZ PCL format. 00069 cloud.width = polydata->GetNumberOfPoints (); 00070 cloud.height = 1; // This indicates that the point cloud is unorganized 00071 cloud.is_dense = false; 00072 cloud.points.resize (cloud.width * cloud.height); 00073 00074 // Get a list of all the fields available 00075 std::vector<pcl::PCLPointField> fields; 00076 pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields)); 00077 00078 // Check if XYZ is present 00079 int x_idx = -1, y_idx = -1, z_idx = -1; 00080 for (size_t d = 0; d < fields.size (); ++d) 00081 { 00082 if (fields[d].name == "x") 00083 x_idx = fields[d].offset; 00084 else if (fields[d].name == "y") 00085 y_idx = fields[d].offset; 00086 else if (fields[d].name == "z") 00087 z_idx = fields[d].offset; 00088 } 00089 // Set the coordinates of the pcl::PointCloud (if the pcl::PointCloud supports coordinates) 00090 if (x_idx != -1 && y_idx != -1 && z_idx != -1) 00091 { 00092 for (size_t i = 0; i < cloud.points.size (); ++i) 00093 { 00094 double coordinate[3]; 00095 polydata->GetPoint (i, coordinate); 00096 pcl::setFieldValue<PointT, float> (cloud.points[i], x_idx, coordinate[0]); 00097 pcl::setFieldValue<PointT, float> (cloud.points[i], y_idx, coordinate[1]); 00098 pcl::setFieldValue<PointT, float> (cloud.points[i], z_idx, coordinate[2]); 00099 } 00100 } 00101 00102 // Check if Normals are present 00103 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1; 00104 for (size_t d = 0; d < fields.size (); ++d) 00105 { 00106 if (fields[d].name == "normal_x") 00107 normal_x_idx = fields[d].offset; 00108 else if (fields[d].name == "normal_y") 00109 normal_y_idx = fields[d].offset; 00110 else if (fields[d].name == "normal_z") 00111 normal_z_idx = fields[d].offset; 00112 } 00113 // Set the normals of the pcl::PointCloud (if the pcl::PointCloud supports normals and the input vtkPolyData has normals) 00114 vtkFloatArray* normals = vtkFloatArray::SafeDownCast (polydata->GetPointData ()->GetNormals ()); 00115 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1 && normals) 00116 { 00117 for (size_t i = 0; i < cloud.points.size (); ++i) 00118 { 00119 float normal[3]; 00120 normals->GetTupleValue (i, normal); 00121 pcl::setFieldValue<PointT, float> (cloud.points[i], normal_x_idx, normal[0]); 00122 pcl::setFieldValue<PointT, float> (cloud.points[i], normal_y_idx, normal[1]); 00123 pcl::setFieldValue<PointT, float> (cloud.points[i], normal_z_idx, normal[2]); 00124 } 00125 } 00126 00127 // Set the colors of the pcl::PointCloud (if the pcl::PointCloud supports colors and the input vtkPolyData has colors) 00128 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ()); 00129 int rgb_idx = -1; 00130 for (size_t d = 0; d < fields.size (); ++d) 00131 { 00132 if (fields[d].name == "rgb" || fields[d].name == "rgba") 00133 { 00134 rgb_idx = fields[d].offset; 00135 break; 00136 } 00137 } 00138 00139 if (rgb_idx != -1 && colors) 00140 { 00141 for (size_t i = 0; i < cloud.points.size (); ++i) 00142 { 00143 unsigned char color[3]; 00144 colors->GetTupleValue (i, color); 00145 pcl::RGB rgb; 00146 rgb.r = color[0]; rgb.g = color[1]; rgb.b = color[2]; 00147 pcl::setFieldValue<PointT, uint32_t> (cloud.points[i], rgb_idx, rgb.rgba); 00148 } 00149 } 00150 } 00151 00152 /////////////////////////////////////////////////////////////////////////////////////////// 00153 template <typename PointT> void 00154 pcl::io::vtkStructuredGridToPointCloud (vtkStructuredGrid* const structured_grid, pcl::PointCloud<PointT>& cloud) 00155 { 00156 int dimensions[3]; 00157 structured_grid->GetDimensions (dimensions); 00158 cloud.width = dimensions[0]; 00159 cloud.height = dimensions[1]; // This indicates that the point cloud is organized 00160 cloud.is_dense = true; 00161 cloud.points.resize (cloud.width * cloud.height); 00162 00163 // Get a list of all the fields available 00164 std::vector<pcl::PCLPointField> fields; 00165 pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields)); 00166 00167 // Check if XYZ is present 00168 int x_idx = -1, y_idx = -1, z_idx = -1; 00169 for (size_t d = 0; d < fields.size (); ++d) 00170 { 00171 if (fields[d].name == "x") 00172 x_idx = fields[d].offset; 00173 else if (fields[d].name == "y") 00174 y_idx = fields[d].offset; 00175 else if (fields[d].name == "z") 00176 z_idx = fields[d].offset; 00177 } 00178 00179 if (x_idx != -1 && y_idx != -1 && z_idx != -1) 00180 { 00181 for (size_t i = 0; i < cloud.width; ++i) 00182 { 00183 for (size_t j = 0; j < cloud.height; ++j) 00184 { 00185 int queryPoint[3] = {i, j, 0}; 00186 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint); 00187 double coordinate[3]; 00188 if (structured_grid->IsPointVisible (pointId)) 00189 { 00190 structured_grid->GetPoint (pointId, coordinate); 00191 pcl::setFieldValue<PointT, float> (cloud (i, j), x_idx, coordinate[0]); 00192 pcl::setFieldValue<PointT, float> (cloud (i, j), y_idx, coordinate[1]); 00193 pcl::setFieldValue<PointT, float> (cloud (i, j), z_idx, coordinate[2]); 00194 } 00195 else 00196 { 00197 // Fill the point with an "empty" point? 00198 } 00199 } 00200 } 00201 } 00202 00203 // Check if Normals are present 00204 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1; 00205 for (size_t d = 0; d < fields.size (); ++d) 00206 { 00207 if (fields[d].name == "normal_x") 00208 normal_x_idx = fields[d].offset; 00209 else if (fields[d].name == "normal_y") 00210 normal_y_idx = fields[d].offset; 00211 else if (fields[d].name == "normal_z") 00212 normal_z_idx = fields[d].offset; 00213 } 00214 // Set the normals of the pcl::PointCloud (if the pcl::PointCloud supports normals and the input vtkStructuredGrid has normals) 00215 vtkFloatArray* normals = vtkFloatArray::SafeDownCast (structured_grid->GetPointData ()->GetNormals ()); 00216 00217 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1 && normals) 00218 { 00219 for (size_t i = 0; i < cloud.width; ++i) 00220 { 00221 for (size_t j = 0; j < cloud.height; ++j) 00222 { 00223 int queryPoint[3] = {i, j, 0}; 00224 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint); 00225 float normal[3]; 00226 if (structured_grid->IsPointVisible (pointId)) 00227 { 00228 normals->GetTupleValue (i, normal); 00229 pcl::setFieldValue<PointT, float> (cloud (i, j), normal_x_idx, normal[0]); 00230 pcl::setFieldValue<PointT, float> (cloud (i, j), normal_y_idx, normal[1]); 00231 pcl::setFieldValue<PointT, float> (cloud (i, j), normal_z_idx, normal[2]); 00232 } 00233 else 00234 { 00235 // Fill the point with an "empty" point? 00236 } 00237 } 00238 } 00239 } 00240 00241 // Set the colors of the pcl::PointCloud (if the pcl::PointCloud supports colors and the input vtkStructuredGrid has colors) 00242 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (structured_grid->GetPointData ()->GetArray ("Colors")); 00243 int rgb_idx = -1; 00244 for (size_t d = 0; d < fields.size (); ++d) 00245 { 00246 if (fields[d].name == "rgb" || fields[d].name == "rgba") 00247 { 00248 rgb_idx = fields[d].offset; 00249 break; 00250 } 00251 } 00252 00253 if (rgb_idx != -1 && colors) 00254 { 00255 for (size_t i = 0; i < cloud.width; ++i) 00256 { 00257 for (size_t j = 0; j < cloud.height; ++j) 00258 { 00259 int queryPoint[3] = {i, j, 0}; 00260 vtkIdType pointId = vtkStructuredData::ComputePointId(dimensions, queryPoint); 00261 unsigned char color[3]; 00262 if (structured_grid->IsPointVisible (pointId)) 00263 { 00264 colors->GetTupleValue (i, color); 00265 pcl::RGB rgb; 00266 rgb.r = color[0]; rgb.g = color[1]; rgb.b = color[2]; 00267 pcl::setFieldValue<PointT, uint32_t> (cloud (i, j), rgb_idx, rgb.rgba); 00268 } 00269 else 00270 { 00271 // Fill the point with an "empty" point? 00272 } 00273 } 00274 } 00275 } 00276 } 00277 00278 /////////////////////////////////////////////////////////////////////////////////////////// 00279 template <typename PointT> void 00280 pcl::io::pointCloudTovtkPolyData (const pcl::PointCloud<PointT>& cloud, vtkPolyData* const pdata) 00281 { 00282 // Get a list of all the fields available 00283 std::vector<pcl::PCLPointField> fields; 00284 pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields)); 00285 00286 // Coordinates (always must have coordinates) 00287 vtkIdType nr_points = cloud.points.size (); 00288 vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New (); 00289 points->SetNumberOfPoints (nr_points); 00290 // Get a pointer to the beginning of the data array 00291 float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0); 00292 00293 // Set the points 00294 if (cloud.is_dense) 00295 { 00296 for (vtkIdType i = 0; i < nr_points; ++i) 00297 memcpy (&data[i * 3], &cloud[i].x, 12); // sizeof (float) * 3 00298 } 00299 else 00300 { 00301 vtkIdType j = 0; // true point index 00302 for (vtkIdType i = 0; i < nr_points; ++i) 00303 { 00304 // Check if the point is invalid 00305 if (!pcl_isfinite (cloud[i].x) || 00306 !pcl_isfinite (cloud[i].y) || 00307 !pcl_isfinite (cloud[i].z)) 00308 continue; 00309 00310 memcpy (&data[j * 3], &cloud[i].x, 12); // sizeof (float) * 3 00311 j++; 00312 } 00313 nr_points = j; 00314 points->SetNumberOfPoints (nr_points); 00315 } 00316 00317 // Create a temporary PolyData and add the points to it 00318 vtkSmartPointer<vtkPolyData> temp_polydata = vtkSmartPointer<vtkPolyData>::New (); 00319 temp_polydata->SetPoints (points); 00320 00321 // Check if Normals are present 00322 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1; 00323 for (size_t d = 0; d < fields.size (); ++d) 00324 { 00325 if (fields[d].name == "normal_x") 00326 normal_x_idx = fields[d].offset; 00327 else if (fields[d].name == "normal_y") 00328 normal_y_idx = fields[d].offset; 00329 else if (fields[d].name == "normal_z") 00330 normal_z_idx = fields[d].offset; 00331 } 00332 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1) 00333 { 00334 vtkSmartPointer<vtkFloatArray> normals = vtkSmartPointer<vtkFloatArray>::New (); 00335 normals->SetNumberOfComponents (3); //3d normals (ie x,y,z) 00336 normals->SetNumberOfTuples (cloud.size ()); 00337 normals->SetName ("Normals"); 00338 00339 for (size_t i = 0; i < cloud.size (); ++i) 00340 { 00341 float normal[3]; 00342 pcl::getFieldValue<PointT, float> (cloud[i], normal_x_idx, normal[0]); 00343 pcl::getFieldValue<PointT, float> (cloud[i], normal_y_idx, normal[1]); 00344 pcl::getFieldValue<PointT, float> (cloud[i], normal_z_idx, normal[2]); 00345 normals->SetTupleValue (i, normal); 00346 } 00347 temp_polydata->GetPointData ()->SetNormals (normals); 00348 } 00349 00350 // Colors (optional) 00351 int rgb_idx = -1; 00352 for (size_t d = 0; d < fields.size (); ++d) 00353 { 00354 if (fields[d].name == "rgb" || fields[d].name == "rgba") 00355 { 00356 rgb_idx = fields[d].offset; 00357 break; 00358 } 00359 } 00360 if (rgb_idx != -1) 00361 { 00362 vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New (); 00363 colors->SetNumberOfComponents (3); 00364 colors->SetNumberOfTuples (cloud.size ()); 00365 colors->SetName ("RGB"); 00366 00367 for (size_t i = 0; i < cloud.size (); ++i) 00368 { 00369 unsigned char color[3]; 00370 pcl::RGB rgb; 00371 pcl::getFieldValue<PointT, uint32_t> (cloud[i], rgb_idx, rgb.rgba); 00372 color[0] = rgb.r; color[1] = rgb.g; color[2] = rgb.b; 00373 colors->SetTupleValue (i, color); 00374 } 00375 temp_polydata->GetPointData ()->SetScalars (colors); 00376 } 00377 00378 // Add 0D topology to every point 00379 vtkSmartPointer<vtkVertexGlyphFilter> vertex_glyph_filter = vtkSmartPointer<vtkVertexGlyphFilter>::New (); 00380 #if VTK_MAJOR_VERSION <= 5 00381 vertex_glyph_filter->AddInputConnection (temp_polydata->GetProducerPort ()); 00382 #else 00383 vertex_glyph_filter->SetInputData (temp_polydata); 00384 #endif 00385 vertex_glyph_filter->Update (); 00386 00387 pdata->DeepCopy (vertex_glyph_filter->GetOutput ()); 00388 } 00389 00390 /////////////////////////////////////////////////////////////////////////////////////////// 00391 template <typename PointT> void 00392 pcl::io::pointCloudTovtkStructuredGrid (const pcl::PointCloud<PointT>& cloud, vtkStructuredGrid* const structured_grid) 00393 { 00394 // Get a list of all the fields available 00395 std::vector<pcl::PCLPointField> fields; 00396 pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields)); 00397 00398 int dimensions[3] = {cloud.width, cloud.height, 1}; 00399 structured_grid->SetDimensions (dimensions); 00400 00401 vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New (); 00402 points->SetNumberOfPoints (cloud.width * cloud.height); 00403 00404 for (size_t i = 0; i < cloud.width; ++i) 00405 { 00406 for (size_t j = 0; j < cloud.height; ++j) 00407 { 00408 int queryPoint[3] = {i, j, 0}; 00409 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint); 00410 const PointT &point = cloud (i, j); 00411 00412 if (pcl::isFinite (point)) 00413 { 00414 float p[3] = {point.x, point.y, point.z}; 00415 points->SetPoint (pointId, p); 00416 } 00417 else 00418 { 00419 } 00420 } 00421 } 00422 00423 structured_grid->SetPoints (points); 00424 00425 // Check if Normals are present 00426 int normal_x_idx = -1, normal_y_idx = -1, normal_z_idx = -1; 00427 for (size_t d = 0; d < fields.size (); ++d) 00428 { 00429 if (fields[d].name == "normal_x") 00430 normal_x_idx = fields[d].offset; 00431 else if (fields[d].name == "normal_y") 00432 normal_y_idx = fields[d].offset; 00433 else if (fields[d].name == "normal_z") 00434 normal_z_idx = fields[d].offset; 00435 } 00436 00437 if (normal_x_idx != -1 && normal_y_idx != -1 && normal_z_idx != -1) 00438 { 00439 vtkSmartPointer<vtkFloatArray> normals = vtkSmartPointer<vtkFloatArray>::New (); 00440 normals->SetNumberOfComponents (3); // Note this must come before the SetNumberOfTuples calls 00441 normals->SetNumberOfTuples (cloud.width * cloud.height); 00442 normals->SetName ("Normals"); 00443 for (size_t i = 0; i < cloud.width; ++i) 00444 { 00445 for (size_t j = 0; j < cloud.height; ++j) 00446 { 00447 int queryPoint[3] = {i, j, 0}; 00448 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint); 00449 const PointT &point = cloud (i, j); 00450 00451 float normal[3]; 00452 pcl::getFieldValue<PointT, float> (point, normal_x_idx, normal[0]); 00453 pcl::getFieldValue<PointT, float> (point, normal_y_idx, normal[1]); 00454 pcl::getFieldValue<PointT, float> (point, normal_z_idx, normal[2]); 00455 normals->SetTupleValue (pointId, normal); 00456 } 00457 } 00458 00459 structured_grid->GetPointData ()->SetNormals (normals); 00460 } 00461 00462 // Colors (optional) 00463 int rgb_idx = -1; 00464 for (size_t d = 0; d < fields.size (); ++d) 00465 { 00466 if (fields[d].name == "rgb" || fields[d].name == "rgba") 00467 { 00468 rgb_idx = fields[d].offset; 00469 break; 00470 } 00471 } 00472 00473 if (rgb_idx != -1) 00474 { 00475 vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New(); 00476 colors->SetNumberOfComponents (3); // Note this must come before the SetNumberOfTuples calls 00477 colors->SetNumberOfTuples (cloud.width * cloud.height); 00478 colors->SetName ("Colors"); 00479 for (size_t i = 0; i < cloud.width; ++i) 00480 { 00481 for (size_t j = 0; j < cloud.height; ++j) 00482 { 00483 int queryPoint[3] = {i, j, 0}; 00484 vtkIdType pointId = vtkStructuredData::ComputePointId (dimensions, queryPoint); 00485 const PointT &point = cloud (i, j); 00486 00487 if (pcl::isFinite (point)) 00488 { 00489 00490 unsigned char color[3]; 00491 pcl::RGB rgb; 00492 pcl::getFieldValue<PointT, uint32_t> (cloud[i], rgb_idx, rgb.rgba); 00493 color[0] = rgb.r; color[1] = rgb.g; color[2] = rgb.b; 00494 colors->SetTupleValue (pointId, color); 00495 } 00496 else 00497 { 00498 } 00499 } 00500 } 00501 structured_grid->GetPointData ()->AddArray (colors); 00502 } 00503 } 00504 00505 #endif //#ifndef PCL_IO_VTK_IO_H_ 00506