$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: shapes.hpp 31665 2010-08-09 19:37:27Z rusu $ 00035 * 00036 */ 00037 00039 00042 template <typename PointT> vtkSmartPointer<vtkDataSet> 00043 pcl_visualization::createPolygon (const pcl::PointCloud<PointT> &cloud) 00044 { 00045 vtkSmartPointer<vtkPoints> poly_points = vtkSmartPointer<vtkPoints>::New (); 00046 vtkSmartPointer<vtkPolygon> polygon = vtkSmartPointer<vtkPolygon>::New (); 00047 00048 poly_points->SetNumberOfPoints (cloud.points.size ()); 00049 polygon->GetPointIds ()->SetNumberOfIds (cloud.points.size ()); 00050 00051 size_t i; 00052 for (i = 0; i < cloud.points.size (); ++i) 00053 { 00054 poly_points->InsertPoint (i, cloud.points[i].x, cloud.points[i].y, cloud.points[i].z); 00055 polygon->GetPointIds ()->SetId (i, i); 00056 } 00057 00058 vtkSmartPointer<vtkUnstructuredGrid> poly_grid = vtkSmartPointer<vtkUnstructuredGrid>::New (); 00059 poly_grid->Allocate (1, 1); 00060 poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ()); 00061 poly_grid->SetPoints (poly_points); 00062 poly_grid->Update (); 00063 00064 return (poly_grid); 00065 } 00066 00068 00072 template <typename P1, typename P2> vtkSmartPointer<vtkDataSet> 00073 pcl_visualization::createLine (const P1 &pt1, const P2 &pt2) 00074 { 00075 vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New (); 00076 line->SetPoint1 (pt1.x, pt1.y, pt1.z); 00077 line->SetPoint2 (pt2.x, pt2.y, pt2.z); 00078 line->Update (); 00079 00080 return (line->GetOutput ()); 00081 } 00082 00084 00089 template <typename PointT> vtkSmartPointer<vtkDataSet> 00090 pcl_visualization::createSphere (const PointT ¢er, double radius, int res) 00091 { 00092 // Set the sphere origin 00093 vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New (); 00094 t->Identity (); t->Translate (center.x, center.y, center.z); 00095 00096 vtkSmartPointer<vtkSphereSource> s_sphere = vtkSmartPointer<vtkSphereSource>::New (); 00097 s_sphere->SetRadius (radius); 00098 s_sphere->SetPhiResolution (res); 00099 s_sphere->SetThetaResolution (res); 00100 s_sphere->LatLongTessellationOff (); 00101 00102 vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New (); 00103 tf->SetTransform (t); 00104 tf->SetInputConnection (s_sphere->GetOutputPort ()); 00105 tf->Update (); 00106 00107 return (tf->GetOutput ()); 00108 } 00109