Point Cloud Library (PCL)  1.7.1
image_viewer.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-2012, 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_VISUALIZATION_IMAGE_VISUALIZER_HPP_
40 #define PCL_VISUALIZATION_IMAGE_VISUALIZER_HPP_
41 
42 #include <vtkContextActor.h>
43 #include <vtkContextScene.h>
44 #include <vtkImageData.h>
45 #include <vtkImageFlip.h>
46 #include <vtkPointData.h>
47 #include <vtkImageViewer.h>
48 
49 #include <pcl/visualization/common/common.h>
50 #include <pcl/search/organized.h>
51 
52 ///////////////////////////////////////////////////////////////////////////////////////////
53 template <typename T> void
55  const pcl::PointCloud<T> &cloud,
56  boost::shared_array<unsigned char> &data)
57 {
58  int j = 0;
59  for (size_t i = 0; i < cloud.points.size (); ++i)
60  {
61  data[j++] = cloud.points[i].r;
62  data[j++] = cloud.points[i].g;
63  data[j++] = cloud.points[i].b;
64  }
65 }
66 
67 ///////////////////////////////////////////////////////////////////////////////////////////
68 template <typename T> void
70  const std::string &layer_id,
71  double opacity)
72 {
73  if (data_size_ < cloud.width * cloud.height)
74  {
75  data_size_ = cloud.width * cloud.height * 3;
76  data_.reset (new unsigned char[data_size_]);
77  }
78 
79  convertRGBCloudToUChar (cloud, data_);
80 
81  return (addRGBImage (data_.get (), cloud.width, cloud.height, layer_id, opacity));
82 }
83 
84 ///////////////////////////////////////////////////////////////////////////////////////////
85 template <typename T> void
87  const std::string &layer_id,
88  double opacity)
89 {
90  addRGBImage<T> (cloud, layer_id, opacity);
91  render ();
92 }
93 
94 ///////////////////////////////////////////////////////////////////////////////////////////
95 template <typename T> bool
97  const typename pcl::PointCloud<T>::ConstPtr &image,
98  const pcl::PointCloud<T> &mask,
99  double r, double g, double b,
100  const std::string &layer_id, double opacity)
101 {
102  // We assume that the data passed into image is organized, otherwise this doesn't make sense
103  if (!image->isOrganized ())
104  return (false);
105 
106  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
107  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
108  if (am_it == layer_map_.end ())
109  {
110  PCL_DEBUG ("[pcl::visualization::ImageViewer::addMask] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
111  am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, true);
112  }
113 
114  // Construct a search object to get the camera parameters
116  search.setInputCloud (image);
117  std::vector<float> xy;
118  xy.reserve (mask.size () * 2);
119  const float image_height_f = static_cast<float> (image->height);
120  for (size_t i = 0; i < mask.size (); ++i)
121  {
122  pcl::PointXY p_projected;
123  search.projectPoint (mask[i], p_projected);
124 
125  xy.push_back (p_projected.x);
126  #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 10))
127  xy.push_back (image_height_f - p_projected.y);
128  #else
129  xy.push_back (p_projected.y);
130  #endif
131  }
132 
134  points->setColors (static_cast<unsigned char> (r*255.0),
135  static_cast<unsigned char> (g*255.0),
136  static_cast<unsigned char> (b*255.0));
137  points->setOpacity (opacity);
138  am_it->actor->GetScene ()->AddItem (points);
139  return (true);
140 }
141 
142 ///////////////////////////////////////////////////////////////////////////////////////////
143 template <typename T> bool
145  const typename pcl::PointCloud<T>::ConstPtr &image,
146  const pcl::PointCloud<T> &mask,
147  const std::string &layer_id, double opacity)
148 {
149  return (addMask (image, mask, 1.0, 0.0, 0.0, layer_id, opacity));
150 }
151 
152 ///////////////////////////////////////////////////////////////////////////////////////////
153 template <typename T> bool
155  const typename pcl::PointCloud<T>::ConstPtr &image,
156  const pcl::PlanarPolygon<T> &polygon,
157  double r, double g, double b,
158  const std::string &layer_id, double opacity)
159 {
160  // We assume that the data passed into image is organized, otherwise this doesn't make sense
161  if (!image->isOrganized ())
162  return (false);
163 
164  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
165  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
166  if (am_it == layer_map_.end ())
167  {
168  PCL_DEBUG ("[pcl::visualization::ImageViewer::addPlanarPolygon] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
169  am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, true);
170  }
171 
172  // Construct a search object to get the camera parameters and fill points
174  search.setInputCloud (image);
175  const float image_height_f = static_cast<float> (image->height);
176  std::vector<float> xy;
177  xy.reserve ((polygon.getContour ().size () + 1) * 2);
178  for (size_t i = 0; i < polygon.getContour ().size (); ++i)
179  {
180  pcl::PointXY p;
181  search.projectPoint (polygon.getContour ()[i], p);
182  xy.push_back (p.x);
183  #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 10))
184  xy.push_back (image_height_f - p.y);
185  #else
186  xy.push_back (p.y);
187  #endif
188  }
189 
190  // Close the polygon
191  xy[xy.size () - 2] = xy[0];
192  xy[xy.size () - 1] = xy[1];
193 
195  poly->setColors (static_cast<unsigned char> (r * 255.0),
196  static_cast<unsigned char> (g * 255.0),
197  static_cast<unsigned char> (b * 255.0));
198  poly->set (xy);
199  am_it->actor->GetScene ()->AddItem (poly);
200 
201  return (true);
202 }
203 
204 ///////////////////////////////////////////////////////////////////////////////////////////
205 template <typename T> bool
207  const typename pcl::PointCloud<T>::ConstPtr &image,
208  const pcl::PlanarPolygon<T> &polygon,
209  const std::string &layer_id, double opacity)
210 {
211  return (addPlanarPolygon (image, polygon, 1.0, 0.0, 0.0, layer_id, opacity));
212 }
213 
214 ///////////////////////////////////////////////////////////////////////////////////////////
215 template <typename T> bool
217  const typename pcl::PointCloud<T>::ConstPtr &image,
218  const T &min_pt,
219  const T &max_pt,
220  double r, double g, double b,
221  const std::string &layer_id, double opacity)
222 {
223  // We assume that the data passed into image is organized, otherwise this doesn't make sense
224  if (!image->isOrganized ())
225  return (false);
226 
227  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
228  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
229  if (am_it == layer_map_.end ())
230  {
231  PCL_DEBUG ("[pcl::visualization::ImageViewer::addRectangle] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
232  am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, true);
233  }
234 
235  // Construct a search object to get the camera parameters
237  search.setInputCloud (image);
238  // Project the 8 corners
239  T p1, p2, p3, p4, p5, p6, p7, p8;
240  p1.x = min_pt.x; p1.y = min_pt.y; p1.z = min_pt.z;
241  p2.x = min_pt.x; p2.y = min_pt.y; p2.z = max_pt.z;
242  p3.x = min_pt.x; p3.y = max_pt.y; p3.z = min_pt.z;
243  p4.x = min_pt.x; p4.y = max_pt.y; p4.z = max_pt.z;
244  p5.x = max_pt.x; p5.y = min_pt.y; p5.z = min_pt.z;
245  p6.x = max_pt.x; p6.y = min_pt.y; p6.z = max_pt.z;
246  p7.x = max_pt.x; p7.y = max_pt.y; p7.z = min_pt.z;
247  p8.x = max_pt.x; p8.y = max_pt.y; p8.z = max_pt.z;
248 
249  std::vector<pcl::PointXY> pp_2d (8);
250  search.projectPoint (p1, pp_2d[0]);
251  search.projectPoint (p2, pp_2d[1]);
252  search.projectPoint (p3, pp_2d[2]);
253  search.projectPoint (p4, pp_2d[3]);
254  search.projectPoint (p5, pp_2d[4]);
255  search.projectPoint (p6, pp_2d[5]);
256  search.projectPoint (p7, pp_2d[6]);
257  search.projectPoint (p8, pp_2d[7]);
258 
259  pcl::PointXY min_pt_2d, max_pt_2d;
260  min_pt_2d.x = min_pt_2d.y = std::numeric_limits<float>::max ();
261  max_pt_2d.x = max_pt_2d.y = std::numeric_limits<float>::min ();
262  // Search for the two extrema
263  for (size_t i = 0; i < pp_2d.size (); ++i)
264  {
265  if (pp_2d[i].x < min_pt_2d.x) min_pt_2d.x = pp_2d[i].x;
266  if (pp_2d[i].y < min_pt_2d.y) min_pt_2d.y = pp_2d[i].y;
267  if (pp_2d[i].x > max_pt_2d.x) max_pt_2d.x = pp_2d[i].x;
268  if (pp_2d[i].y > max_pt_2d.y) max_pt_2d.y = pp_2d[i].y;
269  }
270 #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 10))
271  min_pt_2d.y = float (image->height) - min_pt_2d.y;
272  max_pt_2d.y = float (image->height) - max_pt_2d.y;
273 #endif
274 
276  rect->setColors (static_cast<unsigned char> (255.0 * r),
277  static_cast<unsigned char> (255.0 * g),
278  static_cast<unsigned char> (255.0 * b));
279  rect->setOpacity (opacity);
280  rect->set (min_pt_2d.x, min_pt_2d.y, (max_pt_2d.x - min_pt_2d.x), (max_pt_2d.y - min_pt_2d.y));
281  am_it->actor->GetScene ()->AddItem (rect);
282 
283  return (true);
284 }
285 
286 ///////////////////////////////////////////////////////////////////////////////////////////
287 template <typename T> bool
289  const typename pcl::PointCloud<T>::ConstPtr &image,
290  const T &min_pt,
291  const T &max_pt,
292  const std::string &layer_id, double opacity)
293 {
294  return (addRectangle<T> (image, min_pt, max_pt, 0.0, 1.0, 0.0, layer_id, opacity));
295 }
296 
297 ///////////////////////////////////////////////////////////////////////////////////////////
298 template <typename T> bool
300  const typename pcl::PointCloud<T>::ConstPtr &image,
301  const pcl::PointCloud<T> &mask,
302  double r, double g, double b,
303  const std::string &layer_id, double opacity)
304 {
305  // We assume that the data passed into image is organized, otherwise this doesn't make sense
306  if (!image->isOrganized ())
307  return (false);
308 
309  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
310  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
311  if (am_it == layer_map_.end ())
312  {
313  PCL_DEBUG ("[pcl::visualization::ImageViewer::addRectangle] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
314  am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, true);
315  }
316 
317  // Construct a search object to get the camera parameters
319  search.setInputCloud (image);
320  std::vector<pcl::PointXY> pp_2d (mask.points.size ());
321  for (size_t i = 0; i < mask.points.size (); ++i)
322  search.projectPoint (mask.points[i], pp_2d[i]);
323 
324  pcl::PointXY min_pt_2d, max_pt_2d;
325  min_pt_2d.x = min_pt_2d.y = std::numeric_limits<float>::max ();
326  max_pt_2d.x = max_pt_2d.y = std::numeric_limits<float>::min ();
327  // Search for the two extrema
328  for (size_t i = 0; i < pp_2d.size (); ++i)
329  {
330  if (pp_2d[i].x < min_pt_2d.x) min_pt_2d.x = pp_2d[i].x;
331  if (pp_2d[i].y < min_pt_2d.y) min_pt_2d.y = pp_2d[i].y;
332  if (pp_2d[i].x > max_pt_2d.x) max_pt_2d.x = pp_2d[i].x;
333  if (pp_2d[i].y > max_pt_2d.y) max_pt_2d.y = pp_2d[i].y;
334  }
335 #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 10))
336  min_pt_2d.y = float (image->height) - min_pt_2d.y;
337  max_pt_2d.y = float (image->height) - max_pt_2d.y;
338 #endif
339 
341  rect->setColors (static_cast<unsigned char> (255.0 * r),
342  static_cast<unsigned char> (255.0 * g),
343  static_cast<unsigned char> (255.0 * b));
344  rect->setOpacity (opacity);
345  rect->set (min_pt_2d.x, min_pt_2d.y, (max_pt_2d.x - min_pt_2d.x), (max_pt_2d.y - min_pt_2d.y));
346  am_it->actor->GetScene ()->AddItem (rect);
347 
348  return (true);
349 }
350 
351 ///////////////////////////////////////////////////////////////////////////////////////////
352 template <typename T> bool
354  const typename pcl::PointCloud<T>::ConstPtr &image,
355  const pcl::PointCloud<T> &mask,
356  const std::string &layer_id, double opacity)
357 {
358  return (addRectangle (image, mask, 0.0, 1.0, 0.0, layer_id, opacity));
359 }
360 
361 ///////////////////////////////////////////////////////////////////////////////////////////
362 template <typename PointT> bool
364  const pcl::PointCloud<PointT> &source_img,
365  const pcl::PointCloud<PointT> &target_img,
366  const pcl::Correspondences &correspondences,
367  int nth,
368  const std::string &layer_id)
369 {
370  if (correspondences.empty ())
371  {
372  PCL_DEBUG ("[pcl::visualization::ImageViewer::addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
373  return (false);
374  }
375 
376  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
377  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
378  if (am_it == layer_map_.end ())
379  {
380  PCL_DEBUG ("[pcl::visualization::ImageViewer::addCorrespondences] No layer with ID='%s' found. Creating new one...\n", layer_id.c_str ());
381  am_it = createLayer (layer_id, source_img.width + target_img.width, std::max (source_img.height, target_img.height), 1.0, false);
382  }
383 
384  int src_size = source_img.width * source_img.height * 3;
385  int tgt_size = target_img.width * target_img.height * 3;
386 
387  // Set window size
388  setSize (source_img.width + target_img.width , std::max (source_img.height, target_img.height));
389 
390  // Set data size
391  if (data_size_ < (src_size + tgt_size))
392  {
393  data_size_ = src_size + tgt_size;
394  data_.reset (new unsigned char[data_size_]);
395  }
396 
397  // Copy data in VTK format
398  int j = 0;
399  for (size_t i = 0; i < std::max (source_img.height, target_img.height); ++i)
400  {
401  // Still need to copy the source?
402  if (i < source_img.height)
403  {
404  for (size_t k = 0; k < source_img.width; ++k)
405  {
406  data_[j++] = source_img[i * source_img.width + k].r;
407  data_[j++] = source_img[i * source_img.width + k].g;
408  data_[j++] = source_img[i * source_img.width + k].b;
409  }
410  }
411  else
412  {
413  memcpy (&data_[j], 0, source_img.width * 3);
414  j += source_img.width * 3;
415  }
416 
417  // Still need to copy the target?
418  if (i < source_img.height)
419  {
420  for (size_t k = 0; k < target_img.width; ++k)
421  {
422  data_[j++] = target_img[i * source_img.width + k].r;
423  data_[j++] = target_img[i * source_img.width + k].g;
424  data_[j++] = target_img[i * source_img.width + k].b;
425  }
426  }
427  else
428  {
429  memcpy (&data_[j], 0, target_img.width * 3);
430  j += target_img.width * 3;
431  }
432  }
433 
434  void* data = const_cast<void*> (reinterpret_cast<const void*> (data_.get ()));
435 
437  image->SetDimensions (source_img.width + target_img.width, std::max (source_img.height, target_img.height), 1);
438  image->SetScalarTypeToUnsignedChar ();
439  image->SetNumberOfScalarComponents (3);
440  image->AllocateScalars ();
441  image->GetPointData ()->GetScalars ()->SetVoidArray (data, data_size_, 1);
443 #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 10))
444  // Now create filter and set previously created transformation
445  algo_->SetInput (image);
446  algo_->Update ();
447  image_item->set (0, 0, algo_->GetOutput ());
448 #else
449  image_item->set (0, 0, image);
450  interactor_style_->adjustCamera (image, ren_);
451 #endif
452  am_it->actor->GetScene ()->AddItem (image_item);
453  image_viewer_->SetSize (image->GetDimensions ()[0], image->GetDimensions ()[1]);
454 
455  // Draw lines between the best corresponding points
456  for (size_t i = 0; i < correspondences.size (); i += nth)
457  {
458  double r, g, b;
459  getRandomColors (r, g, b);
460  unsigned char u_r = static_cast<unsigned char> (255.0 * r);
461  unsigned char u_g = static_cast<unsigned char> (255.0 * g);
462  unsigned char u_b = static_cast<unsigned char> (255.0 * b);
464  query_circle->setColors (u_r, u_g, u_b);
466  match_circle->setColors (u_r, u_g, u_b);
468  line->setColors (u_r, u_g, u_b);
469 
470  float query_x = correspondences[i].index_query % source_img.width;
471  float match_x = correspondences[i].index_match % target_img.width + source_img.width;
472 #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 10))
473  float query_y = correspondences[i].index_query / source_img.width;
474  float match_y = correspondences[i].index_match / target_img.width;
475 #else
476  float query_y = getSize ()[1] - correspondences[i].index_query / source_img.width;
477  float match_y = getSize ()[1] - correspondences[i].index_match / target_img.width;
478 #endif
479 
480  query_circle->set (query_x, query_y, 3.0);
481  match_circle->set (match_x, match_y, 3.0);
482  line->set (query_x, query_y, match_x, match_y);
483 
484  am_it->actor->GetScene ()->AddItem (query_circle);
485  am_it->actor->GetScene ()->AddItem (match_circle);
486  am_it->actor->GetScene ()->AddItem (line);
487  }
488 
489  return (true);
490 }
491 
492 #endif // PCL_VISUALIZATION_IMAGE_VISUALIZER_HPP_