1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010, 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 #include <thread>
40
41 #include <pcl/common/time.h> // for DO_EVERY
42 #include <pcl/visualization/common/common.h>
43 #include <vtkRenderWindowInteractor.h>
44 #include <pcl/visualization/histogram_visualizer.h>
45
46 #include <vtkVersion.h>
47 #include <vtkDoubleArray.h>
48 #include <vtkTextProperty.h>
49 #include <vtkRenderWindow.h>
50 #include <vtkRenderer.h>
51 #include <vtkDataObject.h>
52 #include <vtkProperty2D.h>
53 #include <vtkFieldData.h>
54
55 using namespace std::chrono_literals;
56
57 //////////////////////////////////////////////////////////////////////////////////////////////
PCLHistogramVisualizer()58 pcl::visualization::PCLHistogramVisualizer::PCLHistogramVisualizer () :
59 exit_main_loop_timer_callback_ (vtkSmartPointer<ExitMainLoopTimerCallback>::New ()),
60 exit_callback_ (vtkSmartPointer<ExitCallback>::New ()),
61 stopped_ ()
62 {
63 }
64
65 //////////////////////////////////////////////////////////////////////////////////////////////
66 /** \brief Spin once method. Calls the interactor and updates the screen once. */
67 void
spinOnce(int time)68 pcl::visualization::PCLHistogramVisualizer::spinOnce (int time)
69 {
70 for (auto &win : wins_)
71 {
72 DO_EVERY(1.0/win.second.interactor_->GetDesiredUpdateRate (),
73 win.second.interactor_->Render ();
74 exit_main_loop_timer_callback_->right_timer_id = win.second.interactor_->CreateRepeatingTimer (time);
75
76 // Set the correct interactor for callbacks
77 exit_main_loop_timer_callback_->interact = win.second.interactor_;
78 win.second.interactor_->Start ();
79 win.second.interactor_->DestroyTimer (exit_main_loop_timer_callback_->right_timer_id);
80 );
81 }
82 }
83
84 //////////////////////////////////////////////////////////////////////////////////////////////
85 void
spin()86 pcl::visualization::PCLHistogramVisualizer::spin ()
87 {
88 stopped_ = false;
89 do
90 {
91 spinOnce ();
92 if (stopped_)
93 break;
94 std::this_thread::sleep_for(1s);
95 }
96 while (true);
97 }
98
99 //////////////////////////////////////////////////////////////////////////////////////////////
100 void
setBackgroundColor(const double & r,const double & g,const double & b)101 pcl::visualization::PCLHistogramVisualizer::setBackgroundColor (const double &r, const double &g, const double &b)
102 {
103 /*
104 rens_->InitTraversal ();
105 vtkRenderer* renderer = NULL;
106 int i = 1;
107 while ((renderer = rens_->GetNextItem ()) != NULL)
108 {
109 // Should we add the actor to all renderers?
110 if (viewport == 0)
111 {
112 renderer->SetBackground (r, g, b);
113 renderer->Render ();
114 }
115 else if (viewport == i) // add the actor only to the specified viewport
116 {
117 renderer->SetBackground (r, g, b);
118 renderer->Render ();
119 }
120 ++i;
121 }
122 */
123 for (auto &win : wins_)
124 {
125 win.second.ren_->SetBackground (r, g, b);
126 win.second.ren_->Render ();
127 }
128 }
129
130 //////////////////////////////////////////////////////////////////////////////////////////////
131 void
setGlobalYRange(float minp,float maxp)132 pcl::visualization::PCLHistogramVisualizer::setGlobalYRange (float minp, float maxp)
133 {
134 for (auto &win : wins_)
135 {
136 win.second.xy_plot_->SetYRange (minp, maxp);
137 win.second.xy_plot_->Modified ();
138 }
139 }
140
141 //////////////////////////////////////////////////////////////////////////////////////////////
142 void
updateWindowPositions()143 pcl::visualization::PCLHistogramVisualizer::updateWindowPositions ()
144 {
145 int posx = 0, posy = 0;
146 for (auto &win : wins_)
147 {
148 // Get the screen size
149 int *scr_size = win.second.win_->GetScreenSize ();
150 int *win_size = win.second.win_->GetActualSize ();
151
152 // Update the position of the current window
153 win.second.win_->SetPosition (posx, posy);
154 win.second.win_->Modified ();
155 // If there is space on Y, go on Y first
156 if ((posy + win_size[1]) <= scr_size[1])
157 posy += win_size[1];
158 // Go on X
159 else
160 {
161 posy = 0;
162 if ((posx + win_size[0]) <= scr_size[0])
163 posx += win_size[0];
164 else
165 posx = 0;
166 }
167 }
168 }
169
170 //////////////////////////////////////////////////////////////////////////////////////////////
171 void
reCreateActor(const vtkSmartPointer<vtkDoubleArray> & xy_array,RenWinInteract * renwinupd,const int hsize)172 pcl::visualization::PCLHistogramVisualizer::reCreateActor (
173 const vtkSmartPointer<vtkDoubleArray> &xy_array, RenWinInteract* renwinupd, const int hsize)
174 {
175 renwinupd->ren_->RemoveActor2D (renwinupd->xy_plot_);
176 renwinupd->xy_plot_->RemoveAllDataSetInputConnections ();
177
178 double min_max[2];
179 xy_array->GetRange (min_max, 1);
180
181 // Create the data structures
182 vtkSmartPointer<vtkFieldData> field_values = vtkSmartPointer<vtkFieldData>::New ();
183 field_values->AddArray (xy_array);
184
185 vtkSmartPointer<vtkDataObject> field_data = vtkSmartPointer<vtkDataObject>::New ();
186 field_data->SetFieldData (field_values);
187
188 renwinupd->xy_plot_->AddDataObjectInput (field_data);
189 renwinupd->ren_->AddActor2D (renwinupd->xy_plot_);
190
191 renwinupd->xy_plot_->SetYTitle (""); renwinupd->xy_plot_->SetXTitle ("");
192 renwinupd->xy_plot_->SetYRange (min_max[0], min_max[1]);
193 renwinupd->xy_plot_->SetXRange (0, hsize - 1);
194 }
195
196
197 ////////////////////////////////////////////////////////////////////////////////////////////
198 void
createActor(const vtkSmartPointer<vtkDoubleArray> & xy_array,pcl::visualization::RenWinInteract & renwinint,const std::string & id,const int win_width,const int win_height)199 pcl::visualization::PCLHistogramVisualizer::createActor (
200 const vtkSmartPointer<vtkDoubleArray> &xy_array,
201 pcl::visualization::RenWinInteract &renwinint,
202 const std::string &id, const int win_width, const int win_height)
203 {
204 // Create the actor
205 renwinint.xy_plot_->SetDataObjectPlotModeToColumns ();
206 renwinint.xy_plot_->SetXValuesToValue ();
207
208 // Create the data structures
209 vtkSmartPointer<vtkFieldData> field_values = vtkSmartPointer<vtkFieldData>::New ();
210 field_values->AddArray (xy_array);
211
212 vtkSmartPointer<vtkDataObject> field_data = vtkSmartPointer<vtkDataObject>::New ();
213 field_data->SetFieldData (field_values);
214
215 renwinint.xy_plot_->AddDataObjectInput (field_data);
216 // Set the plot color
217 renwinint.xy_plot_->SetPlotColor (0, 1.0, 0.0, 0.0);
218
219 renwinint.xy_plot_->SetDataObjectXComponent (0, 0); renwinint.xy_plot_->SetDataObjectYComponent (0, 1);
220 renwinint.xy_plot_->PlotPointsOn ();
221 //renwinint.xy_plot_->PlotCurvePointsOn ();
222 //renwinint.xy_plot_->PlotLinesOn ();
223 renwinint.xy_plot_->PlotCurveLinesOn ();
224
225 // Get min-max range
226 double min_max[2];
227 xy_array->GetRange (min_max, 1);
228
229 renwinint.xy_plot_->SetYTitle (""); renwinint.xy_plot_->SetXTitle ("");
230 renwinint.xy_plot_->SetYRange (min_max[0], min_max[1]);
231 renwinint.xy_plot_->SetXRange (0, static_cast<double> (xy_array->GetNumberOfTuples () - 1));
232
233 //renwinint.xy_plot_->SetTitle (id.c_str ());
234 renwinint.xy_plot_->GetProperty ()->SetColor (0, 0, 0);
235
236 // Adjust text properties
237 vtkSmartPointer<vtkTextProperty> tprop = renwinint.xy_plot_->GetTitleTextProperty ();
238 renwinint.xy_plot_->AdjustTitlePositionOn ();
239 tprop->SetFontSize (8);
240 tprop->ShadowOff (); tprop->ItalicOff ();
241 tprop->SetColor (renwinint.xy_plot_->GetProperty ()->GetColor ());
242
243 renwinint.xy_plot_->SetAxisLabelTextProperty (tprop);
244 renwinint.xy_plot_->SetAxisTitleTextProperty (tprop);
245 renwinint.xy_plot_->SetNumberOfXLabels (8);
246 renwinint.xy_plot_->GetProperty ()->SetPointSize (3);
247 renwinint.xy_plot_->GetProperty ()->SetLineWidth (2);
248
249 renwinint.xy_plot_->SetPosition (0, 0);
250 renwinint.xy_plot_->SetWidth (1); renwinint.xy_plot_->SetHeight (1);
251
252 // Create the new window with its interactor and renderer
253 renwinint.ren_->AddActor2D (renwinint.xy_plot_);
254 renwinint.ren_->SetBackground (1, 1, 1);
255 renwinint.win_->SetWindowName (id.c_str ());
256 renwinint.win_->AddRenderer (renwinint.ren_);
257 renwinint.win_->SetSize (win_width, win_height);
258 renwinint.win_->SetBorders (1);
259
260 // Create the interactor style
261 vtkSmartPointer<pcl::visualization::PCLHistogramVisualizerInteractorStyle> style_ = vtkSmartPointer<pcl::visualization::PCLHistogramVisualizerInteractorStyle>::New ();
262 style_->Initialize ();
263 renwinint.style_ = style_;
264 renwinint.style_->UseTimersOn ();
265 renwinint.interactor_ = vtkSmartPointer<vtkRenderWindowInteractor>::New ();
266 renwinint.interactor_->SetRenderWindow (renwinint.win_);
267 renwinint.interactor_->SetInteractorStyle (renwinint.style_);
268 // Initialize and create timer
269 renwinint.interactor_->Initialize ();
270 renwinint.interactor_->CreateRepeatingTimer (5000L);
271
272 exit_main_loop_timer_callback_->right_timer_id = -1;
273 renwinint.interactor_->AddObserver (vtkCommand::TimerEvent, exit_main_loop_timer_callback_);
274 exit_callback_->his = this;
275 renwinint.interactor_->AddObserver (vtkCommand::ExitEvent, exit_callback_);
276 }
277
278 ////////////////////////////////////////////////////////////////////////////////////////////
279 bool
addFeatureHistogram(const pcl::PCLPointCloud2 & cloud,const std::string & field_name,const std::string & id,int win_width,int win_height)280 pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram (
281 const pcl::PCLPointCloud2 &cloud, const std::string &field_name,
282 const std::string &id, int win_width, int win_height)
283 {
284 // Get the field
285 int field_idx = pcl::getFieldIndex (cloud, field_name);
286 if (field_idx == -1)
287 {
288 PCL_ERROR ("[addFeatureHistogram] Invalid field (%s) given!\n", field_name.c_str ());
289 return (false);
290 }
291
292 RenWinInteractMap::iterator am_it = wins_.find (id);
293 if (am_it != wins_.end ())
294 {
295 PCL_WARN ("[addFeatureHistogram] A window with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
296 return (false);
297 }
298
299 vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New ();
300 xy_array->SetNumberOfComponents (2);
301 xy_array->SetNumberOfTuples (cloud.fields[field_idx].count);
302
303 // Parse the cloud data and store it in the array
304 double xy[2];
305 for (uindex_t d = 0; d < cloud.fields[field_idx].count; ++d)
306 {
307 xy[0] = d;
308 float data;
309 // TODO: replace float with the real data type
310 memcpy (&data, &cloud.data[cloud.fields[field_idx].offset + d * sizeof (float)], sizeof (float));
311 xy[1] = data;
312 xy_array->SetTuple (d, xy);
313 }
314 RenWinInteract renwinint;
315 createActor (xy_array, renwinint, id, win_width, win_height);
316
317 // Save the pointer/ID pair to the global window map
318 wins_[id] = renwinint;
319 return (true);
320 }
321
322 ////////////////////////////////////////////////////////////////////////////////////////////
323 bool
addFeatureHistogram(const pcl::PCLPointCloud2 & cloud,const std::string & field_name,const pcl::index_t index,const std::string & id,int win_width,int win_height)324 pcl::visualization::PCLHistogramVisualizer::addFeatureHistogram (
325 const pcl::PCLPointCloud2 &cloud,
326 const std::string &field_name,
327 const pcl::index_t index,
328 const std::string &id, int win_width, int win_height)
329 {
330 if (index < 0 || index >= static_cast<pcl::index_t> (cloud.width * cloud.height))
331 {
332 PCL_ERROR ("[addFeatureHistogram] Invalid point index (%d) given!\n", index);
333 return (false);
334 }
335
336 // Get the field
337 int field_idx = pcl::getFieldIndex (cloud, field_name);
338 if (field_idx == -1)
339 {
340 PCL_ERROR ("[addFeatureHistogram] The specified field <%s> does not exist!\n", field_name.c_str ());
341 return (false);
342 }
343
344 RenWinInteractMap::iterator am_it = wins_.find (id);
345 if (am_it != wins_.end ())
346 {
347 PCL_ERROR ("[addFeatureHistogram] A window with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
348 return (false);
349 }
350
351 vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New ();
352 xy_array->SetNumberOfComponents (2);
353 xy_array->SetNumberOfTuples (cloud.fields[field_idx].count);
354
355 // Compute the total size of the fields
356 unsigned int fsize = 0;
357 for (const auto &field : cloud.fields)
358 fsize += field.count * pcl::getFieldSize (field.datatype);
359
360 // Parse the cloud data and store it in the array
361 double xy[2];
362 for (uindex_t d = 0; d < cloud.fields[field_idx].count; ++d)
363 {
364 xy[0] = d;
365 float data;
366 // TODO: replace float with the real data type
367 memcpy (&data, &cloud.data[index * fsize + cloud.fields[field_idx].offset + d * sizeof (float)], sizeof (float));
368 xy[1] = data;
369 xy_array->SetTuple (d, xy);
370 }
371 RenWinInteract renwinint;
372 createActor (xy_array, renwinint, id, win_width, win_height);
373
374 // Save the pointer/ID pair to the global window map
375 wins_[id] = renwinint;
376 return (true);
377 }
378
379 //////////////////////////////////////////////////////////////////////////////////////////////
380 bool
updateFeatureHistogram(const pcl::PCLPointCloud2 & cloud,const std::string & field_name,const std::string & id)381 pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram (
382 const pcl::PCLPointCloud2 &cloud, const std::string &field_name,
383 const std::string &id)
384 {
385 RenWinInteractMap::iterator am_it = wins_.find (id);
386 if (am_it == wins_.end ())
387 {
388 PCL_WARN ("[updateFeatureHistogram] A window with id <%s> does not exists!.\n", id.c_str ());
389 return (false);
390 }
391 RenWinInteract* renwinupd = &wins_[id];
392 vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New ();
393 xy_array->SetNumberOfComponents (2);
394
395 // Get the field
396 int field_idx = pcl::getFieldIndex (cloud, field_name);
397 if (field_idx == -1)
398 {
399 pcl::console::print_error ("[updateFeatureHistogram] Invalid field (%s) given!", field_name.c_str ());
400 return (false);
401 }
402 xy_array->SetNumberOfTuples (cloud.fields[field_idx].count);
403
404 // Parse the cloud data and store it in the array
405 double xy[2];
406 for (uindex_t d = 0; d < cloud.fields[field_idx].count; ++d)
407 {
408 xy[0] = d;
409 float data;
410 memcpy (&data, &cloud.data[cloud.fields[field_idx].offset + d * sizeof (float)], sizeof (float));
411 xy[1] = data;
412 xy_array->SetTuple (d, xy);
413 }
414 reCreateActor(xy_array, renwinupd, cloud.fields[field_idx].count - 1);
415 return (true);
416 }
417
418 //////////////////////////////////////////////////////////////////////////////////////////////
419 bool
updateFeatureHistogram(const pcl::PCLPointCloud2 & cloud,const std::string & field_name,const pcl::index_t index,const std::string & id)420 pcl::visualization::PCLHistogramVisualizer::updateFeatureHistogram (
421 const pcl::PCLPointCloud2 &cloud,
422 const std::string &field_name,
423 const pcl::index_t index,
424 const std::string &id)
425 {
426 if (index < 0 || index >= static_cast<pcl::index_t> (cloud.width * cloud.height))
427 {
428 PCL_ERROR ("[updateFeatureHistogram] Invalid point index (%d) given!\n", index);
429 return (false);
430 }
431 RenWinInteractMap::iterator am_it = wins_.find (id);
432 if (am_it == wins_.end ())
433 {
434 PCL_WARN ("[updateFeatureHistogram] A window with id <%s> does not exists!.\n", id.c_str ());
435 return (false);
436 }
437 RenWinInteract* renwinupd = &wins_[id];
438 vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New ();
439 xy_array->SetNumberOfComponents (2);
440
441 // Get the field
442 int field_idx = pcl::getFieldIndex (cloud, field_name);
443 if (field_idx == -1)
444 {
445 pcl::console::print_error ("[updateFeatureHistogram] Invalid field (%s) given!", field_name.c_str ());
446 return (false);
447 }
448 xy_array->SetNumberOfTuples (cloud.fields[field_idx].count);
449
450 // Compute the total size of the fields
451 unsigned int fsize = 0;
452 for (const auto &field : cloud.fields)
453 fsize += field.count * pcl::getFieldSize (field.datatype);
454
455 // Parse the cloud data and store it in the array
456 double xy[2];
457 for (uindex_t d = 0; d < cloud.fields[field_idx].count; ++d)
458 {
459 xy[0] = d;
460 float data;
461 memcpy (&data, &cloud.data[index * fsize + cloud.fields[field_idx].offset + d * sizeof (float)], sizeof (float));
462 xy[1] = data;
463 xy_array->SetTuple (d, xy);
464 }
465 reCreateActor(xy_array, renwinupd, cloud.fields[field_idx].count - 1);
466 return (true);
467 }
468
469 //////////////////////////////////////////////////////////////////////////////
470 void
Execute(vtkObject *,unsigned long event_id,void * call_data)471 pcl::visualization::PCLHistogramVisualizer::ExitMainLoopTimerCallback::Execute (
472 vtkObject*, unsigned long event_id, void* call_data)
473 {
474 if (event_id != vtkCommand::TimerEvent)
475 return;
476 int timer_id = *(reinterpret_cast<int*> (call_data));
477
478 if (timer_id != right_timer_id)
479 return;
480
481 // Stop vtk loop and send notification to app to wake it up
482 interact->TerminateApp ();
483 }
484
485 //////////////////////////////////////////////////////////////////////////////
486 void
Execute(vtkObject *,unsigned long event_id,void *)487 pcl::visualization::PCLHistogramVisualizer::ExitCallback::Execute (
488 vtkObject*, unsigned long event_id, void*)
489 {
490 if (event_id != vtkCommand::ExitEvent)
491 return;
492 his->stopped_ = true;
493 }
494
495