Point Cloud Library (PCL)  1.14.1-dev
histogram_visualizer.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 
39 #ifndef PCL_PCL_HISTOGRAM_VISUALIZER_IMPL_H_
40 #define PCL_PCL_HISTOGRAM_VISUALIZER_IMPL_H_
41 
42 #include <vtkDoubleArray.h>
43 
44 
45 namespace pcl
46 {
47 
48 namespace visualization
49 {
50 
51 template <typename PointT> bool
53  const pcl::PointCloud<PointT> &cloud, int hsize,
54  const std::string &id, int win_width, int win_height)
55 {
56  auto am_it = wins_.find (id);
57  if (am_it != wins_.end ())
58  {
59  PCL_WARN ("[addFeatureHistogram] A window with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
60  return (false);
61  }
62 
64  xy_array->SetNumberOfComponents (2);
65  xy_array->SetNumberOfTuples (hsize);
66 
67  // Parse the cloud data and store it in the array
68  double xy[2];
69  for (int d = 0; d < hsize; ++d)
70  {
71  xy[0] = d;
72  xy[1] = cloud[0].histogram[d];
73  xy_array->SetTuple (d, xy);
74  }
75  RenWinInteract renwinint;
76  createActor (xy_array, renwinint, id, win_width, win_height);
77 
78  // Save the pointer/ID pair to the global window map
79  wins_[id] = renwinint;
80 
81  return (true);
82 }
83 
84 
85 template <typename PointT> bool
87  const pcl::PointCloud<PointT> &cloud,
88  const std::string &field_name,
89  const pcl::index_t index,
90  const std::string &id, int win_width, int win_height)
91 {
92  if (index < 0 || index >= cloud.size ())
93  {
94  PCL_ERROR ("[addFeatureHistogram] Invalid point index (%d) given!\n", index);
95  return (false);
96  }
97 
98  // Get the fields present in this cloud
99  std::vector<pcl::PCLPointField> fields;
100  // Check if our field exists
101  int field_idx = pcl::getFieldIndex<PointT> (cloud, field_name, fields);
102  if (field_idx == -1)
103  {
104  PCL_ERROR ("[addFeatureHistogram] The specified field <%s> does not exist!\n", field_name.c_str ());
105  return (false);
106  }
107 
108  auto am_it = wins_.find (id);
109  if (am_it != wins_.end ())
110  {
111  PCL_WARN ("[addFeatureHistogram] A window with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
112  return (false);
113  }
114 
116  xy_array->SetNumberOfComponents (2);
117  xy_array->SetNumberOfTuples (fields[field_idx].count);
118 
119  // Parse the cloud data and store it in the array
120  double xy[2];
121  for (uindex_t d = 0; d < fields[field_idx].count; ++d)
122  {
123  xy[0] = d;
124  //xy[1] = cloud[index].histogram[d];
125  float data;
126  memcpy (&data, reinterpret_cast<const char*> (&cloud[index]) + fields[field_idx].offset + d * sizeof (float), sizeof (float));
127  xy[1] = data;
128  xy_array->SetTuple (d, xy);
129  }
130  RenWinInteract renwinint;
131  createActor (xy_array, renwinint, id, win_width, win_height);
132 
133  // Save the pointer/ID pair to the global window map
134  wins_[id] = renwinint;
135  return (true);
136 }
137 
138 
139 template <typename PointT> bool
141  const pcl::PointCloud<PointT> &cloud, int hsize,
142  const std::string &id)
143 {
144  auto am_it = wins_.find (id);
145  if (am_it == wins_.end ())
146  {
147  PCL_WARN ("[updateFeatureHistogram] A window with id <%s> does not exists!.\n", id.c_str ());
148  return (false);
149  }
150  RenWinInteract* renwinupd = &wins_[id];
151 
153  xy_array->SetNumberOfComponents (2);
154  xy_array->SetNumberOfTuples (hsize);
155 
156  // Parse the cloud data and store it in the array
157  double xy[2];
158  for (int d = 0; d < hsize; ++d)
159  {
160  xy[0] = d;
161  xy[1] = cloud[0].histogram[d];
162  xy_array->SetTuple (d, xy);
163  }
164  reCreateActor (xy_array, renwinupd, hsize);
165  return (true);
166 }
167 
168 
169 template <typename PointT> bool
171  const pcl::PointCloud<PointT> &cloud, const std::string &field_name, const pcl::index_t index,
172  const std::string &id)
173 {
174  if (index < 0 || index >= cloud.size ())
175  {
176  PCL_ERROR ("[updateFeatureHistogram] Invalid point index (%d) given!\n", index);
177  return (false);
178  }
179 
180  // Get the fields present in this cloud
181  std::vector<pcl::PCLPointField> fields;
182  // Check if our field exists
183  int field_idx = pcl::getFieldIndex<PointT> (cloud, field_name, fields);
184  if (field_idx == -1)
185  {
186  PCL_ERROR ("[updateFeatureHistogram] The specified field <%s> does not exist!\n", field_name.c_str ());
187  return (false);
188  }
189 
190  auto am_it = wins_.find (id);
191  if (am_it == wins_.end ())
192  {
193  PCL_WARN ("[updateFeatureHistogram] A window with id <%s> does not exists!.\n", id.c_str ());
194  return (false);
195  }
196  RenWinInteract* renwinupd = &wins_[id];
197 
199  xy_array->SetNumberOfComponents (2);
200  xy_array->SetNumberOfTuples (fields[field_idx].count);
201 
202  // Parse the cloud data and store it in the array
203  double xy[2];
204  for (std::uint32_t d = 0; d < fields[field_idx].count; ++d)
205  {
206  xy[0] = d;
207  //xy[1] = cloud[index].histogram[d];
208  float data;
209  memcpy (&data, reinterpret_cast<const char*> (&cloud[index]) + fields[field_idx].offset + d * sizeof (float), sizeof (float));
210  xy[1] = data;
211  xy_array->SetTuple (d, xy);
212  }
213 
214  reCreateActor (xy_array, renwinupd, cloud.fields[field_idx].count - 1);
215  return (true);
216 }
217 
218 } // namespace visualization
219 } // namespace pcl
220 
221 #endif
222 
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
std::size_t size() const
Definition: point_cloud.h:443
bool addFeatureHistogram(const pcl::PointCloud< PointT > &cloud, int hsize, const std::string &id="cloud", int win_width=640, int win_height=200)
Add a histogram feature to screen as a separate window, from a cloud containing a single histogram.
bool updateFeatureHistogram(const pcl::PointCloud< PointT > &cloud, int hsize, const std::string &id="cloud")
Update a histogram feature that is already on screen, with a cloud containing a single histogram.
void createActor(const vtkSmartPointer< vtkDoubleArray > &xy_array, RenWinInteract &renwinint, const std::string &id, const int win_width, const int win_height)
Create a 2D actor from the given vtkDoubleArray histogram and add it to the screen.
void reCreateActor(const vtkSmartPointer< vtkDoubleArray > &xy_array, RenWinInteract *renwinupd, const int hsize)
Remove the current 2d actor and create a new 2D actor from the given vtkDoubleArray histogram and add...
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition: types.h:120
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112