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 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 */ 00038 00039 #ifndef PCL_PCL_HISTOGRAM_VISUALIZER_IMPL_H_ 00040 #define PCL_PCL_HISTOGRAM_VISUALIZER_IMPL_H_ 00041 00042 #include <vtkDoubleArray.h> 00043 00044 //////////////////////////////////////////////////////////////////////////////////////////// 00045 template <typename PointT> bool 00046 pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram ( 00047 const pcl::PointCloud<PointT> &cloud, int hsize, 00048 const std::string &id, int win_width, int win_height) 00049 { 00050 RenWinInteractMap::iterator am_it = wins_.find (id); 00051 if (am_it != wins_.end ()) 00052 { 00053 PCL_WARN ("[addFeatureHistogram] A window with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ()); 00054 return (false); 00055 } 00056 00057 vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New (); 00058 xy_array->SetNumberOfComponents (2); 00059 xy_array->SetNumberOfTuples (hsize); 00060 00061 // Parse the cloud data and store it in the array 00062 double xy[2]; 00063 for (int d = 0; d < hsize; ++d) 00064 { 00065 xy[0] = d; 00066 xy[1] = cloud.points[0].histogram[d]; 00067 xy_array->SetTuple (d, xy); 00068 } 00069 RenWinInteract renwinint; 00070 createActor (xy_array, renwinint, id, win_width, win_height); 00071 00072 // Save the pointer/ID pair to the global window map 00073 wins_[id] = renwinint; 00074 #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 4)) 00075 resetStoppedFlag (); 00076 #endif 00077 return (true); 00078 } 00079 00080 //////////////////////////////////////////////////////////////////////////////////////////// 00081 template <typename PointT> bool 00082 pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram ( 00083 const pcl::PointCloud<PointT> &cloud, 00084 const std::string &field_name, 00085 const int index, 00086 const std::string &id, int win_width, int win_height) 00087 { 00088 if (index < 0 || index >= cloud.points.size ()) 00089 { 00090 PCL_ERROR ("[addFeatureHistogram] Invalid point index (%d) given!\n", index); 00091 return (false); 00092 } 00093 00094 // Get the fields present in this cloud 00095 std::vector<pcl::PCLPointField> fields; 00096 // Check if our field exists 00097 int field_idx = pcl::getFieldIndex<PointT> (cloud, field_name, fields); 00098 if (field_idx == -1) 00099 { 00100 PCL_ERROR ("[addFeatureHistogram] The specified field <%s> does not exist!\n", field_name.c_str ()); 00101 return (false); 00102 } 00103 00104 RenWinInteractMap::iterator am_it = wins_.find (id); 00105 if (am_it != wins_.end ()) 00106 { 00107 PCL_WARN ("[addFeatureHistogram] A window with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ()); 00108 return (false); 00109 } 00110 00111 vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New (); 00112 xy_array->SetNumberOfComponents (2); 00113 xy_array->SetNumberOfTuples (fields[field_idx].count); 00114 00115 // Parse the cloud data and store it in the array 00116 double xy[2]; 00117 for (int d = 0; d < fields[field_idx].count; ++d) 00118 { 00119 xy[0] = d; 00120 //xy[1] = cloud.points[index].histogram[d]; 00121 float data; 00122 memcpy (&data, reinterpret_cast<const char*> (&cloud.points[index]) + fields[field_idx].offset + d * sizeof (float), sizeof (float)); 00123 xy[1] = data; 00124 xy_array->SetTuple (d, xy); 00125 } 00126 RenWinInteract renwinint; 00127 createActor (xy_array, renwinint, id, win_width, win_height); 00128 00129 // Save the pointer/ID pair to the global window map 00130 wins_[id] = renwinint; 00131 #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 4)) 00132 resetStoppedFlag (); 00133 #endif 00134 return (true); 00135 } 00136 00137 //////////////////////////////////////////////////////////////////////////////////////////// 00138 template <typename PointT> bool 00139 pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram ( 00140 const pcl::PointCloud<PointT> &cloud, int hsize, 00141 const std::string &id) 00142 { 00143 RenWinInteractMap::iterator am_it = wins_.find (id); 00144 if (am_it == wins_.end ()) 00145 { 00146 PCL_WARN ("[updateFeatureHistogram] A window with id <%s> does not exists!.\n", id.c_str ()); 00147 return (false); 00148 } 00149 RenWinInteract* renwinupd = &wins_[id]; 00150 00151 vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New (); 00152 xy_array->SetNumberOfComponents (2); 00153 xy_array->SetNumberOfTuples (hsize); 00154 00155 // Parse the cloud data and store it in the array 00156 double xy[2]; 00157 for (int d = 0; d < hsize; ++d) 00158 { 00159 xy[0] = d; 00160 xy[1] = cloud.points[0].histogram[d]; 00161 xy_array->SetTuple (d, xy); 00162 } 00163 reCreateActor (xy_array, renwinupd, hsize); 00164 return (true); 00165 } 00166 00167 //////////////////////////////////////////////////////////////////////////////////////////// 00168 template <typename PointT> bool 00169 pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram ( 00170 const pcl::PointCloud<PointT> &cloud, const std::string &field_name, const int index, 00171 const std::string &id) 00172 { 00173 if (index < 0 || index >= cloud.points.size ()) 00174 { 00175 PCL_ERROR ("[updateFeatureHistogram] Invalid point index (%d) given!\n", index); 00176 return (false); 00177 } 00178 00179 // Get the fields present in this cloud 00180 std::vector<pcl::PCLPointField> fields; 00181 // Check if our field exists 00182 int field_idx = pcl::getFieldIndex<PointT> (cloud, field_name, fields); 00183 if (field_idx == -1) 00184 { 00185 PCL_ERROR ("[updateFeatureHistogram] The specified field <%s> does not exist!\n", field_name.c_str ()); 00186 return (false); 00187 } 00188 00189 RenWinInteractMap::iterator am_it = wins_.find (id); 00190 if (am_it == wins_.end ()) 00191 { 00192 PCL_WARN ("[updateFeatureHistogram] A window with id <%s> does not exists!.\n", id.c_str ()); 00193 return (false); 00194 } 00195 RenWinInteract* renwinupd = &wins_[id]; 00196 00197 vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New (); 00198 xy_array->SetNumberOfComponents (2); 00199 xy_array->SetNumberOfTuples (fields[field_idx].count); 00200 00201 // Parse the cloud data and store it in the array 00202 double xy[2]; 00203 for (int d = 0; d < fields[field_idx].count; ++d) 00204 { 00205 xy[0] = d; 00206 //xy[1] = cloud.points[index].histogram[d]; 00207 float data; 00208 memcpy (&data, reinterpret_cast<const char*> (&cloud.points[index]) + fields[field_idx].offset + d * sizeof (float), sizeof (float)); 00209 xy[1] = data; 00210 xy_array->SetTuple (d, xy); 00211 } 00212 00213 reCreateActor (xy_array, renwinupd, cloud.fields[field_idx].count - 1); 00214 return (true); 00215 } 00216 00217 #endif 00218