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