Point Cloud Library (PCL)  1.9.1-dev
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 <vtkVersion.h>
43 #include <vtkContextActor.h>
44 #include <vtkContextScene.h>
45 #include <vtkImageData.h>
46 #include <vtkImageFlip.h>
47 #include <vtkPointData.h>
48 #include <vtkImageViewer.h>
49 
50 #include <pcl/visualization/common/common.h>
51 #include <pcl/search/organized.h>
52 
53 ///////////////////////////////////////////////////////////////////////////////////////////
54 template <typename T> void
56  const pcl::PointCloud<T> &cloud,
57  boost::shared_array<unsigned char> &data)
58 {
59  int j = 0;
60  for (size_t i = 0; i < cloud.points.size (); ++i)
61  {
62  data[j++] = cloud.points[i].r;
63  data[j++] = cloud.points[i].g;
64  data[j++] = cloud.points[i].b;
65  }
66 }
67 
68 ///////////////////////////////////////////////////////////////////////////////////////////
69 template <typename T> void
71  const std::string &layer_id,
72  double opacity)
73 {
74  if (data_size_ < cloud.width * cloud.height)
75  {
76  data_size_ = cloud.width * cloud.height * 3;
77  data_.reset (new unsigned char[data_size_]);
78  }
79 
80  convertRGBCloudToUChar (cloud, data_);
81 
82  return (addRGBImage (data_.get (), cloud.width, cloud.height, layer_id, opacity));
83 }
84 
85 ///////////////////////////////////////////////////////////////////////////////////////////
86 template <typename T> void
88  const std::string &layer_id,
89  double opacity)
90 {
91  addRGBImage<T> (cloud, layer_id, opacity);
92  render ();
93 }
94 
95 ///////////////////////////////////////////////////////////////////////////////////////////
96 template <typename T> bool
98  const typename pcl::PointCloud<T>::ConstPtr &image,
99  const pcl::PointCloud<T> &mask,
100  double r, double g, double b,
101  const std::string &layer_id, double opacity)
102 {
103  // We assume that the data passed into image is organized, otherwise this doesn't make sense
104  if (!image->isOrganized ())
105  return (false);
106 
107  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
108  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
109  if (am_it == layer_map_.end ())
110  {
111  PCL_DEBUG ("[pcl::visualization::ImageViewer::addMask] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
112  am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, false);
113  }
114 
115  // Construct a search object to get the camera parameters
117  search.setInputCloud (image);
118  std::vector<float> xy;
119  xy.reserve (mask.size () * 2);
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  xy.push_back (static_cast<float> (image->height) - p_projected.y);
127  }
128 
130  points->setColors (static_cast<unsigned char> (r*255.0),
131  static_cast<unsigned char> (g*255.0),
132  static_cast<unsigned char> (b*255.0));
133  points->setOpacity (opacity);
134  points->set (xy);
135  am_it->actor->GetScene ()->AddItem (points);
136  return (true);
137 }
138 
139 ///////////////////////////////////////////////////////////////////////////////////////////
140 template <typename T> bool
142  const typename pcl::PointCloud<T>::ConstPtr &image,
143  const pcl::PointCloud<T> &mask,
144  const std::string &layer_id, double opacity)
145 {
146  return (addMask (image, mask, 1.0, 0.0, 0.0, layer_id, opacity));
147 }
148 
149 ///////////////////////////////////////////////////////////////////////////////////////////
150 template <typename T> bool
152  const typename pcl::PointCloud<T>::ConstPtr &image,
153  const pcl::PlanarPolygon<T> &polygon,
154  double r, double g, double b,
155  const std::string &layer_id, double opacity)
156 {
157  // We assume that the data passed into image is organized, otherwise this doesn't make sense
158  if (!image->isOrganized ())
159  return (false);
160 
161  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
162  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
163  if (am_it == layer_map_.end ())
164  {
165  PCL_DEBUG ("[pcl::visualization::ImageViewer::addPlanarPolygon] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
166  am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, false);
167  }
168 
169  // Construct a search object to get the camera parameters and fill points
171  search.setInputCloud (image);
172  std::vector<float> xy;
173  xy.reserve ((polygon.getContour ().size () + 1) * 2);
174  for (size_t i = 0; i < polygon.getContour ().size (); ++i)
175  {
176  pcl::PointXY p;
177  search.projectPoint (polygon.getContour ()[i], p);
178  xy.push_back (p.x);
179  xy.push_back (p.y);
180  }
181 
182  // Close the polygon
183  xy[xy.size () - 2] = xy[0];
184  xy[xy.size () - 1] = xy[1];
185 
187  poly->setColors (static_cast<unsigned char> (r * 255.0),
188  static_cast<unsigned char> (g * 255.0),
189  static_cast<unsigned char> (b * 255.0));
190  poly->setOpacity (opacity);
191  poly->set (xy);
192  am_it->actor->GetScene ()->AddItem (poly);
193 
194  return (true);
195 }
196 
197 ///////////////////////////////////////////////////////////////////////////////////////////
198 template <typename T> bool
200  const typename pcl::PointCloud<T>::ConstPtr &image,
201  const pcl::PlanarPolygon<T> &polygon,
202  const std::string &layer_id, double opacity)
203 {
204  return (addPlanarPolygon (image, polygon, 1.0, 0.0, 0.0, layer_id, opacity));
205 }
206 
207 ///////////////////////////////////////////////////////////////////////////////////////////
208 template <typename T> bool
210  const typename pcl::PointCloud<T>::ConstPtr &image,
211  const T &min_pt,
212  const T &max_pt,
213  double r, double g, double b,
214  const std::string &layer_id, double opacity)
215 {
216  // We assume that the data passed into image is organized, otherwise this doesn't make sense
217  if (!image->isOrganized ())
218  return (false);
219 
220  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
221  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
222  if (am_it == layer_map_.end ())
223  {
224  PCL_DEBUG ("[pcl::visualization::ImageViewer::addRectangle] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
225  am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, false);
226  }
227 
228  // Construct a search object to get the camera parameters
230  search.setInputCloud (image);
231  // Project the 8 corners
232  T p1, p2, p3, p4, p5, p6, p7, p8;
233  p1.x = min_pt.x; p1.y = min_pt.y; p1.z = min_pt.z;
234  p2.x = min_pt.x; p2.y = min_pt.y; p2.z = max_pt.z;
235  p3.x = min_pt.x; p3.y = max_pt.y; p3.z = min_pt.z;
236  p4.x = min_pt.x; p4.y = max_pt.y; p4.z = max_pt.z;
237  p5.x = max_pt.x; p5.y = min_pt.y; p5.z = min_pt.z;
238  p6.x = max_pt.x; p6.y = min_pt.y; p6.z = max_pt.z;
239  p7.x = max_pt.x; p7.y = max_pt.y; p7.z = min_pt.z;
240  p8.x = max_pt.x; p8.y = max_pt.y; p8.z = max_pt.z;
241 
242  std::vector<pcl::PointXY> pp_2d (8);
243  search.projectPoint (p1, pp_2d[0]);
244  search.projectPoint (p2, pp_2d[1]);
245  search.projectPoint (p3, pp_2d[2]);
246  search.projectPoint (p4, pp_2d[3]);
247  search.projectPoint (p5, pp_2d[4]);
248  search.projectPoint (p6, pp_2d[5]);
249  search.projectPoint (p7, pp_2d[6]);
250  search.projectPoint (p8, pp_2d[7]);
251 
252  pcl::PointXY min_pt_2d, max_pt_2d;
253  min_pt_2d.x = min_pt_2d.y = std::numeric_limits<float>::max ();
254  max_pt_2d.x = max_pt_2d.y = -std::numeric_limits<float>::max ();
255  // Search for the two extrema
256  for (size_t i = 0; i < pp_2d.size (); ++i)
257  {
258  if (pp_2d[i].x < min_pt_2d.x) min_pt_2d.x = pp_2d[i].x;
259  if (pp_2d[i].y < min_pt_2d.y) min_pt_2d.y = pp_2d[i].y;
260  if (pp_2d[i].x > max_pt_2d.x) max_pt_2d.x = pp_2d[i].x;
261  if (pp_2d[i].y > max_pt_2d.y) max_pt_2d.y = pp_2d[i].y;
262  }
263  min_pt_2d.y = float (image->height) - min_pt_2d.y;
264  max_pt_2d.y = float (image->height) - max_pt_2d.y;
265 
267  rect->setColors (static_cast<unsigned char> (255.0 * r),
268  static_cast<unsigned char> (255.0 * g),
269  static_cast<unsigned char> (255.0 * b));
270  rect->setOpacity (opacity);
271  rect->set (min_pt_2d.x, min_pt_2d.y, max_pt_2d.x, max_pt_2d.y);
272  am_it->actor->GetScene ()->AddItem (rect);
273 
274  return (true);
275 }
276 
277 ///////////////////////////////////////////////////////////////////////////////////////////
278 template <typename T> bool
280  const typename pcl::PointCloud<T>::ConstPtr &image,
281  const T &min_pt,
282  const T &max_pt,
283  const std::string &layer_id, double opacity)
284 {
285  return (addRectangle<T> (image, min_pt, max_pt, 0.0, 1.0, 0.0, layer_id, opacity));
286 }
287 
288 ///////////////////////////////////////////////////////////////////////////////////////////
289 template <typename T> bool
291  const typename pcl::PointCloud<T>::ConstPtr &image,
292  const pcl::PointCloud<T> &mask,
293  double r, double g, double b,
294  const std::string &layer_id, double opacity)
295 {
296  // We assume that the data passed into image is organized, otherwise this doesn't make sense
297  if (!image->isOrganized ())
298  return (false);
299 
300  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
301  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
302  if (am_it == layer_map_.end ())
303  {
304  PCL_DEBUG ("[pcl::visualization::ImageViewer::addRectangle] No layer with ID'=%s' found. Creating new one...\n", layer_id.c_str ());
305  am_it = createLayer (layer_id, getSize ()[0] - 1, getSize ()[1] - 1, opacity, false);
306  }
307 
308  // Construct a search object to get the camera parameters
310  search.setInputCloud (image);
311  std::vector<pcl::PointXY> pp_2d (mask.points.size ());
312  for (size_t i = 0; i < mask.points.size (); ++i)
313  search.projectPoint (mask.points[i], pp_2d[i]);
314 
315  pcl::PointXY min_pt_2d, max_pt_2d;
316  min_pt_2d.x = min_pt_2d.y = std::numeric_limits<float>::max ();
317  max_pt_2d.x = max_pt_2d.y = -std::numeric_limits<float>::max ();
318  // Search for the two extrema
319  for (size_t i = 0; i < pp_2d.size (); ++i)
320  {
321  if (pp_2d[i].x < min_pt_2d.x) min_pt_2d.x = pp_2d[i].x;
322  if (pp_2d[i].y < min_pt_2d.y) min_pt_2d.y = pp_2d[i].y;
323  if (pp_2d[i].x > max_pt_2d.x) max_pt_2d.x = pp_2d[i].x;
324  if (pp_2d[i].y > max_pt_2d.y) max_pt_2d.y = pp_2d[i].y;
325  }
326  min_pt_2d.y = float (image->height) - min_pt_2d.y;
327  max_pt_2d.y = float (image->height) - max_pt_2d.y;
328 
330  rect->setColors (static_cast<unsigned char> (255.0 * r),
331  static_cast<unsigned char> (255.0 * g),
332  static_cast<unsigned char> (255.0 * b));
333  rect->setOpacity (opacity);
334  rect->set (min_pt_2d.x, min_pt_2d.y, max_pt_2d.x, max_pt_2d.y);
335  am_it->actor->GetScene ()->AddItem (rect);
336 
337  return (true);
338 }
339 
340 ///////////////////////////////////////////////////////////////////////////////////////////
341 template <typename T> bool
343  const typename pcl::PointCloud<T>::ConstPtr &image,
344  const pcl::PointCloud<T> &mask,
345  const std::string &layer_id, double opacity)
346 {
347  return (addRectangle (image, mask, 0.0, 1.0, 0.0, layer_id, opacity));
348 }
349 
350 ///////////////////////////////////////////////////////////////////////////////////////////
351 template <typename PointT> bool
353  const pcl::PointCloud<PointT> &source_img,
354  const pcl::PointCloud<PointT> &target_img,
355  const pcl::Correspondences &correspondences,
356  int nth,
357  const std::string &layer_id)
358 {
359  if (correspondences.empty ())
360  {
361  PCL_DEBUG ("[pcl::visualization::ImageViewer::addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
362  return (false);
363  }
364 
365  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
366  LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
367  if (am_it == layer_map_.end ())
368  {
369  PCL_DEBUG ("[pcl::visualization::ImageViewer::addCorrespondences] No layer with ID='%s' found. Creating new one...\n", layer_id.c_str ());
370  am_it = createLayer (layer_id, source_img.width + target_img.width, std::max (source_img.height, target_img.height), 1.0, false);
371  }
372 
373  int src_size = source_img.width * source_img.height * 3;
374  int tgt_size = target_img.width * target_img.height * 3;
375 
376  // Set window size
377  setSize (source_img.width + target_img.width , std::max (source_img.height, target_img.height));
378 
379  // Set data size
380  if (data_size_ < static_cast<size_t> (src_size + tgt_size))
381  {
382  data_size_ = src_size + tgt_size;
383  data_.reset (new unsigned char[data_size_]);
384  }
385 
386  // Copy data in VTK format
387  int j = 0;
388  for (size_t i = 0; i < std::max (source_img.height, target_img.height); ++i)
389  {
390  // Still need to copy the source?
391  if (i < source_img.height)
392  {
393  for (size_t k = 0; k < source_img.width; ++k)
394  {
395  data_[j++] = source_img[i * source_img.width + k].r;
396  data_[j++] = source_img[i * source_img.width + k].g;
397  data_[j++] = source_img[i * source_img.width + k].b;
398  }
399  }
400  else
401  {
402  memcpy (&data_[j], 0, source_img.width * 3);
403  j += source_img.width * 3;
404  }
405 
406  // Still need to copy the target?
407  if (i < source_img.height)
408  {
409  for (size_t k = 0; k < target_img.width; ++k)
410  {
411  data_[j++] = target_img[i * source_img.width + k].r;
412  data_[j++] = target_img[i * source_img.width + k].g;
413  data_[j++] = target_img[i * source_img.width + k].b;
414  }
415  }
416  else
417  {
418  memcpy (&data_[j], 0, target_img.width * 3);
419  j += target_img.width * 3;
420  }
421  }
422 
423  void* data = const_cast<void*> (reinterpret_cast<const void*> (data_.get ()));
424 
426  image->SetDimensions (source_img.width + target_img.width, std::max (source_img.height, target_img.height), 1);
427  image->AllocateScalars (VTK_UNSIGNED_CHAR, 3);
428  image->GetPointData ()->GetScalars ()->SetVoidArray (data, data_size_, 1);
430  image_item->set (0, 0, image);
431  interactor_style_->adjustCamera (image, ren_);
432  am_it->actor->GetScene ()->AddItem (image_item);
433  image_viewer_->SetSize (image->GetDimensions ()[0], image->GetDimensions ()[1]);
434 
435  // Draw lines between the best corresponding points
436  for (size_t i = 0; i < correspondences.size (); i += nth)
437  {
438  double r, g, b;
439  getRandomColors (r, g, b);
440  unsigned char u_r = static_cast<unsigned char> (255.0 * r);
441  unsigned char u_g = static_cast<unsigned char> (255.0 * g);
442  unsigned char u_b = static_cast<unsigned char> (255.0 * b);
444  query_circle->setColors (u_r, u_g, u_b);
446  match_circle->setColors (u_r, u_g, u_b);
448  line->setColors (u_r, u_g, u_b);
449 
450  float query_x = correspondences[i].index_query % source_img.width;
451  float match_x = correspondences[i].index_match % target_img.width + source_img.width;
452  float query_y = getSize ()[1] - correspondences[i].index_query / source_img.width;
453  float match_y = getSize ()[1] - correspondences[i].index_match / target_img.width;
454 
455  query_circle->set (query_x, query_y, 3.0);
456  match_circle->set (match_x, match_y, 3.0);
457  line->set (query_x, query_y, match_x, match_y);
458 
459  am_it->actor->GetScene ()->AddItem (query_circle);
460  am_it->actor->GetScene ()->AddItem (match_circle);
461  am_it->actor->GetScene ()->AddItem (line);
462  }
463 
464  return (true);
465 }
466 
467 #endif // PCL_VISUALIZATION_IMAGE_VISUALIZER_HPP_
size_t size() const
Definition: point_cloud.h:447
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:409
bool projectPoint(const PointT &p, pcl::PointXY &q) const
projects a point into the image
Definition: organized.hpp:382
pcl::PointCloud< PointT >::VectorType & getContour()
Getter for the contour / boundary.
bool addMask(const typename pcl::PointCloud< T >::ConstPtr &image, const pcl::PointCloud< T > &mask, double r, double g, double b, const std::string &layer_id="image_mask", double opacity=0.5)
Add a generic 2D mask to an image.
void setSize(int xw, int yw)
Set the window size in screen coordinates.
void render()
Trigger a render call.
void showRGBImage(const unsigned char *data, unsigned width, unsigned height, const std::string &layer_id="rgb_image", double opacity=1.0)
Show a 2D RGB image on screen.
A 2D point structure representing Euclidean xy coordinates.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:414
PCL_EXPORTS void getRandomColors(double &r, double &g, double &b, double min=0.2, double max=2.8)
Get (good) random values for R/G/B.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:412
bool addRectangle(const pcl::PointXY &min_pt, const pcl::PointXY &max_pt, const std::string &layer_id="rectangles", double opacity=1.0)
Add a 2D box and color its edges with a given color.
PlanarPolygon represents a planar (2D) polygon, potentially in a 3D space.
void convertRGBCloudToUChar(const pcl::PointCloud< T > &cloud, boost::shared_array< unsigned char > &data)
Convert the RGB information in a PointCloud<T> to an unsigned char array.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:428
bool addPlanarPolygon(const typename pcl::PointCloud< T >::ConstPtr &image, const pcl::PlanarPolygon< T > &polygon, double r, double g, double b, const std::string &layer_id="planar_polygon", double opacity=1.0)
Add a generic 2D planar polygon to an image.
bool showCorrespondences(const pcl::PointCloud< PointT > &source_img, const pcl::PointCloud< PointT > &target_img, const pcl::Correspondences &correspondences, int nth=1, const std::string &layer_id="correspondences")
Add the specified correspondences to the display.
int * getSize()
Return the window size in pixels.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input data set, if user has focal length he must set it before calling this...
Definition: organized.h:128
void addRGBImage(const unsigned char *data, unsigned width, unsigned height, const std::string &layer_id="rgb_image", double opacity=1.0)
Add an RGB 2D image layer, but do not render it (use spin/spinOnce to update).
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
Definition: point_cloud.h:330
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
Definition: organized.h:61