Point Cloud Library (PCL)  1.9.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 template <typename PointT> bool
47  const pcl::PointCloud<PointT> &cloud, int hsize,
48  const std::string &id, int win_width, int win_height)
49 {
50  RenWinInteractMap::iterator am_it = wins_.find (id);
51  if (am_it != wins_.end ())
52  {
53  PCL_WARN ("[addFeatureHistogram] A window with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
54  return (false);
55  }
56 
58  xy_array->SetNumberOfComponents (2);
59  xy_array->SetNumberOfTuples (hsize);
60 
61  // Parse the cloud data and store it in the array
62  double xy[2];
63  for (int d = 0; d < hsize; ++d)
64  {
65  xy[0] = d;
66  xy[1] = cloud.points[0].histogram[d];
67  xy_array->SetTuple (d, xy);
68  }
69  RenWinInteract renwinint;
70  createActor (xy_array, renwinint, id, win_width, win_height);
71 
72  // Save the pointer/ID pair to the global window map
73  wins_[id] = renwinint;
74 
75  return (true);
76 }
77 
78 ////////////////////////////////////////////////////////////////////////////////////////////
79 template <typename PointT> bool
81  const pcl::PointCloud<PointT> &cloud,
82  const std::string &field_name,
83  const int index,
84  const std::string &id, int win_width, int win_height)
85 {
86  if (index < 0 || index >= cloud.points.size ())
87  {
88  PCL_ERROR ("[addFeatureHistogram] Invalid point index (%d) given!\n", index);
89  return (false);
90  }
91 
92  // Get the fields present in this cloud
93  std::vector<pcl::PCLPointField> fields;
94  // Check if our field exists
95  int field_idx = pcl::getFieldIndex<PointT> (cloud, field_name, fields);
96  if (field_idx == -1)
97  {
98  PCL_ERROR ("[addFeatureHistogram] The specified field <%s> does not exist!\n", field_name.c_str ());
99  return (false);
100  }
101 
102  RenWinInteractMap::iterator am_it = wins_.find (id);
103  if (am_it != wins_.end ())
104  {
105  PCL_WARN ("[addFeatureHistogram] A window with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
106  return (false);
107  }
108 
110  xy_array->SetNumberOfComponents (2);
111  xy_array->SetNumberOfTuples (fields[field_idx].count);
112 
113  // Parse the cloud data and store it in the array
114  double xy[2];
115  for (uint32_t d = 0; d < fields[field_idx].count; ++d)
116  {
117  xy[0] = d;
118  //xy[1] = cloud.points[index].histogram[d];
119  float data;
120  memcpy (&data, reinterpret_cast<const char*> (&cloud.points[index]) + fields[field_idx].offset + d * sizeof (float), sizeof (float));
121  xy[1] = data;
122  xy_array->SetTuple (d, xy);
123  }
124  RenWinInteract renwinint;
125  createActor (xy_array, renwinint, id, win_width, win_height);
126 
127  // Save the pointer/ID pair to the global window map
128  wins_[id] = renwinint;
129  return (true);
130 }
131 
132 ////////////////////////////////////////////////////////////////////////////////////////////
133 template <typename PointT> bool
135  const pcl::PointCloud<PointT> &cloud, int hsize,
136  const std::string &id)
137 {
138  RenWinInteractMap::iterator am_it = wins_.find (id);
139  if (am_it == wins_.end ())
140  {
141  PCL_WARN ("[updateFeatureHistogram] A window with id <%s> does not exists!.\n", id.c_str ());
142  return (false);
143  }
144  RenWinInteract* renwinupd = &wins_[id];
145 
147  xy_array->SetNumberOfComponents (2);
148  xy_array->SetNumberOfTuples (hsize);
149 
150  // Parse the cloud data and store it in the array
151  double xy[2];
152  for (int d = 0; d < hsize; ++d)
153  {
154  xy[0] = d;
155  xy[1] = cloud.points[0].histogram[d];
156  xy_array->SetTuple (d, xy);
157  }
158  reCreateActor (xy_array, renwinupd, hsize);
159  return (true);
160 }
161 
162 ////////////////////////////////////////////////////////////////////////////////////////////
163 template <typename PointT> bool
165  const pcl::PointCloud<PointT> &cloud, const std::string &field_name, const int index,
166  const std::string &id)
167 {
168  if (index < 0 || index >= cloud.points.size ())
169  {
170  PCL_ERROR ("[updateFeatureHistogram] Invalid point index (%d) given!\n", index);
171  return (false);
172  }
173 
174  // Get the fields present in this cloud
175  std::vector<pcl::PCLPointField> fields;
176  // Check if our field exists
177  int field_idx = pcl::getFieldIndex<PointT> (cloud, field_name, fields);
178  if (field_idx == -1)
179  {
180  PCL_ERROR ("[updateFeatureHistogram] The specified field <%s> does not exist!\n", field_name.c_str ());
181  return (false);
182  }
183 
184  RenWinInteractMap::iterator am_it = wins_.find (id);
185  if (am_it == wins_.end ())
186  {
187  PCL_WARN ("[updateFeatureHistogram] A window with id <%s> does not exists!.\n", id.c_str ());
188  return (false);
189  }
190  RenWinInteract* renwinupd = &wins_[id];
191 
193  xy_array->SetNumberOfComponents (2);
194  xy_array->SetNumberOfTuples (fields[field_idx].count);
195 
196  // Parse the cloud data and store it in the array
197  double xy[2];
198  for (uint32_t d = 0; d < fields[field_idx].count; ++d)
199  {
200  xy[0] = d;
201  //xy[1] = cloud.points[index].histogram[d];
202  float data;
203  memcpy (&data, reinterpret_cast<const char*> (&cloud.points[index]) + fields[field_idx].offset + d * sizeof (float), sizeof (float));
204  xy[1] = data;
205  xy_array->SetTuple (d, xy);
206  }
207 
208  reCreateActor (xy_array, renwinupd, cloud.fields[field_idx].count - 1);
209  return (true);
210 }
211 
212 #endif
213 
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:423
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 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...
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.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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...