00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 #include <pcl/surface/vtk_smoothing/vtk_utils.h>
00041
00042 #include <pcl/ros/conversions.h>
00043 #include <pcl/common/common.h>
00044 #include <vtkCellArray.h>
00045 #include <vtkTriangleFilter.h>
00046 #include <vtkPoints.h>
00047 #include <vtkPolyData.h>
00048 #include <vtkPointData.h>
00049 #include <vtkFloatArray.h>
00050
00051
00053 int
00054 pcl::VTKUtils::convertToVTK (const pcl::PolygonMesh &triangles, vtkSmartPointer<vtkPolyData> &triangles_out_vtk)
00055 {
00056 if (triangles.cloud.data.empty ())
00057 {
00058 PCL_ERROR ("[pcl::surface::convertToVTK] Input point cloud has no data!\n");
00059 return (-1);
00060 }
00061
00062 vtkSmartPointer<vtkPolyData> vtk_polygons;
00063 mesh2vtk (triangles, vtk_polygons);
00064
00065 vtkSmartPointer<vtkTriangleFilter> vtk_triangles = vtkTriangleFilter::New ();
00066 vtk_triangles->SetInput (vtk_polygons);
00067 vtk_triangles->Update();
00068
00069 triangles_out_vtk = vtk_triangles->GetOutput ();
00070 return 1;
00071 }
00072
00073
00075 void
00076 pcl::VTKUtils::convertToPCL (vtkSmartPointer<vtkPolyData> &vtk_polygons, pcl::PolygonMesh &triangles)
00077 {
00078 vtk2mesh (vtk_polygons, triangles);
00079 }
00080
00082 int
00083 pcl::VTKUtils::vtk2mesh (const vtkSmartPointer<vtkPolyData>& poly_data, pcl::PolygonMesh& mesh)
00084 {
00085 mesh.polygons.clear ();
00086 mesh.cloud.data.clear ();
00087 mesh.cloud.width = mesh.cloud.height = 0;
00088 mesh.cloud.is_dense = true;
00089
00090 vtkSmartPointer<vtkPoints> mesh_points = poly_data->GetPoints ();
00091 vtkIdType nr_points = mesh_points->GetNumberOfPoints ();
00092 vtkIdType nr_polygons = poly_data->GetNumberOfPolys ();
00093
00094 if (nr_points == 0)
00095 return 0;
00096
00097 vtkUnsignedCharArray* poly_colors = NULL;
00098 if (poly_data->GetPointData() != NULL)
00099 poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("Colors"));
00100
00101
00102 if (!poly_colors)
00103 poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("scalars"));
00104
00105
00106 if (poly_colors && (poly_colors->GetNumberOfComponents () == 3))
00107 {
00108 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZRGB>());
00109 cloud_temp->points.resize (nr_points);
00110 double point_xyz[3];
00111 unsigned char point_color[3];
00112 for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); ++i)
00113 {
00114 mesh_points->GetPoint (i, &point_xyz[0]);
00115 cloud_temp->points[i].x = static_cast<float> (point_xyz[0]);
00116 cloud_temp->points[i].y = static_cast<float> (point_xyz[1]);
00117 cloud_temp->points[i].z = static_cast<float> (point_xyz[2]);
00118
00119 poly_colors->GetTupleValue (i, &point_color[0]);
00120 cloud_temp->points[i].r = point_color[0];
00121 cloud_temp->points[i].g = point_color[1];
00122 cloud_temp->points[i].b = point_color[2];
00123 }
00124 cloud_temp->width = static_cast<uint32_t> (cloud_temp->points.size ());
00125 cloud_temp->height = 1;
00126 cloud_temp->is_dense = true;
00127
00128 pcl::toROSMsg (*cloud_temp, mesh.cloud);
00129 }
00130 else
00131 {
00132 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZ> ());
00133 cloud_temp->points.resize (nr_points);
00134 double point_xyz[3];
00135 for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); ++i)
00136 {
00137 mesh_points->GetPoint (i, &point_xyz[0]);
00138 cloud_temp->points[i].x = static_cast<float> (point_xyz[0]);
00139 cloud_temp->points[i].y = static_cast<float> (point_xyz[1]);
00140 cloud_temp->points[i].z = static_cast<float> (point_xyz[2]);
00141 }
00142 cloud_temp->width = static_cast<uint32_t> (cloud_temp->points.size ());
00143 cloud_temp->height = 1;
00144 cloud_temp->is_dense = true;
00145
00146 pcl::toROSMsg (*cloud_temp, mesh.cloud);
00147 }
00148
00149 mesh.polygons.resize (nr_polygons);
00150 vtkIdType* cell_points;
00151 vtkIdType nr_cell_points;
00152 vtkCellArray * mesh_polygons = poly_data->GetPolys ();
00153 mesh_polygons->InitTraversal ();
00154 int id_poly = 0;
00155 while (mesh_polygons->GetNextCell (nr_cell_points, cell_points))
00156 {
00157 mesh.polygons[id_poly].vertices.resize (nr_cell_points);
00158 for (int i = 0; i < nr_cell_points; ++i)
00159 mesh.polygons[id_poly].vertices[i] = static_cast<unsigned int> (cell_points[i]);
00160 ++id_poly;
00161 }
00162
00163 return (static_cast<int> (nr_points));
00164 }
00165
00167 int
00168 pcl::VTKUtils::mesh2vtk (const pcl::PolygonMesh& mesh, vtkSmartPointer<vtkPolyData> &poly_data)
00169 {
00170 int nr_points = mesh.cloud.width * mesh.cloud.height;
00171 int nr_polygons = static_cast<int> (mesh.polygons.size ());
00172
00173
00174 poly_data = vtkSmartPointer<vtkPolyData>::New ();
00175 vtkSmartPointer<vtkPoints> vtk_mesh_points = vtkSmartPointer<vtkPoints>::New ();
00176 vtkSmartPointer<vtkCellArray> vtk_mesh_polygons = vtkSmartPointer<vtkCellArray>::New ();
00177 poly_data->SetPoints (vtk_mesh_points);
00178
00179
00180 int idx_x = -1, idx_y = -1, idx_z = -1, idx_rgb = -1, idx_rgba = -1, idx_normal_x = -1, idx_normal_y = -1, idx_normal_z = -1;
00181 for (int d = 0; d < static_cast<int> (mesh.cloud.fields.size ()); ++d)
00182 {
00183 if (mesh.cloud.fields[d].name == "x") idx_x = d;
00184 else if (mesh.cloud.fields[d].name == "y") idx_y = d;
00185 else if (mesh.cloud.fields[d].name == "z") idx_z = d;
00186 else if (mesh.cloud.fields[d].name == "rgb") idx_rgb = d;
00187 else if (mesh.cloud.fields[d].name == "rgba") idx_rgba = d;
00188 else if (mesh.cloud.fields[d].name == "normal_x") idx_normal_x = d;
00189 else if (mesh.cloud.fields[d].name == "normal_y") idx_normal_y = d;
00190 else if (mesh.cloud.fields[d].name == "normal_z") idx_normal_z = d;
00191 }
00192 if ( ( idx_x == -1 ) || ( idx_y == -1 ) || ( idx_z == -1 ) )
00193 nr_points = 0;
00194
00195
00196 vtk_mesh_points->SetNumberOfPoints (nr_points);
00197 if (nr_points > 0)
00198 {
00199 Eigen::Vector4f pt = Eigen::Vector4f::Zero ();
00200 Eigen::Array4i xyz_offset (mesh.cloud.fields[idx_x].offset, mesh.cloud.fields[idx_y].offset, mesh.cloud.fields[idx_z].offset, 0);
00201 for (vtkIdType cp = 0; cp < nr_points; ++cp, xyz_offset += mesh.cloud.point_step)
00202 {
00203 memcpy(&pt[0], &mesh.cloud.data[xyz_offset[0]], sizeof(float));
00204 memcpy(&pt[1], &mesh.cloud.data[xyz_offset[1]], sizeof(float));
00205 memcpy(&pt[2], &mesh.cloud.data[xyz_offset[2]], sizeof(float));
00206 vtk_mesh_points->InsertPoint(cp, pt[0], pt[1], pt[2]);
00207 }
00208 }
00209
00210
00211 if (nr_polygons > 0)
00212 {
00213 for (int i = 0; i < nr_polygons; i++)
00214 {
00215 unsigned int nr_points_in_polygon = static_cast<unsigned int> (mesh.polygons[i].vertices.size ());
00216 vtk_mesh_polygons->InsertNextCell (nr_points_in_polygon);
00217 for (unsigned int j = 0; j < nr_points_in_polygon; j++)
00218 vtk_mesh_polygons->InsertCellPoint(mesh.polygons[i].vertices[j]);
00219 }
00220 poly_data->SetPolys (vtk_mesh_polygons);
00221 }
00222
00223
00224 if (idx_rgb != -1 || idx_rgba != -1)
00225 {
00226 vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00227 colors->SetNumberOfComponents (3);
00228 colors->SetName ("Colors");
00229 pcl::RGB rgb;
00230 int offset = (idx_rgb != -1) ? mesh.cloud.fields[idx_rgb].offset : mesh.cloud.fields[idx_rgba].offset;
00231 for (vtkIdType cp = 0; cp < nr_points; ++cp)
00232 {
00233 memcpy (&rgb, &mesh.cloud.data[cp * mesh.cloud.point_step + offset], sizeof (pcl::RGB));
00234 const unsigned char color[3] = {rgb.r, rgb.g, rgb.b};
00235 colors->InsertNextTupleValue (color);
00236 }
00237 poly_data->GetPointData ()->SetScalars (colors);
00238 }
00239
00240
00241 if ( ( idx_normal_x != -1 ) && ( idx_normal_y != -1 ) && ( idx_normal_z != -1 ) )
00242 {
00243 vtkSmartPointer<vtkFloatArray> normals = vtkSmartPointer<vtkFloatArray>::New ();
00244 normals->SetNumberOfComponents (3);
00245 float nx = 0.0f, ny = 0.0f, nz = 0.0f;
00246 for (vtkIdType cp = 0; cp < nr_points; ++cp)
00247 {
00248 memcpy (&nx, &mesh.cloud.data[cp*mesh.cloud.point_step+mesh.cloud.fields[idx_normal_x].offset], sizeof(float));
00249 memcpy (&ny, &mesh.cloud.data[cp*mesh.cloud.point_step+mesh.cloud.fields[idx_normal_y].offset], sizeof(float));
00250 memcpy (&nz, &mesh.cloud.data[cp*mesh.cloud.point_step+mesh.cloud.fields[idx_normal_z].offset], sizeof(float));
00251 const float normal[3] = {nx, ny, nz};
00252 normals->InsertNextTupleValue (normal);
00253 }
00254 poly_data->GetPointData()->SetNormals (normals);
00255 }
00256
00257 if (poly_data->GetPoints() == NULL)
00258 return (0);
00259 return (static_cast<int> (poly_data->GetPoints()->GetNumberOfPoints ()));
00260 }
00261
00262