Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
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///////////////////////////////////////////////////////////////////////////////////////////
54template <typename T> void
56 const pcl::PointCloud<T> &cloud,
57 boost::shared_array<unsigned char> &data)
58{
59 int j = 0;
60 for (const auto& point: cloud)
61 {
62 data[j++] = point.r;
63 data[j++] = point.g;
64 data[j++] = point.b;
65 }
66}
67
68///////////////////////////////////////////////////////////////////////////////////////////
69template <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///////////////////////////////////////////////////////////////////////////////////////////
86template <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///////////////////////////////////////////////////////////////////////////////////////////
96template <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 auto 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 (std::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///////////////////////////////////////////////////////////////////////////////////////////
140template <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///////////////////////////////////////////////////////////////////////////////////////////
150template <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 auto 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 (std::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///////////////////////////////////////////////////////////////////////////////////////////
198template <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///////////////////////////////////////////////////////////////////////////////////////////
208template <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 auto 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 (const auto &point : pp_2d)
257 {
258 if (point.x < min_pt_2d.x) min_pt_2d.x = point.x;
259 if (point.y < min_pt_2d.y) min_pt_2d.y = point.y;
260 if (point.x > max_pt_2d.x) max_pt_2d.x = point.x;
261 if (point.y > max_pt_2d.y) max_pt_2d.y = point.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///////////////////////////////////////////////////////////////////////////////////////////
278template <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///////////////////////////////////////////////////////////////////////////////////////////
289template <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 auto 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.size ());
312 for (std::size_t i = 0; i < mask.size (); ++i)
313 search.projectPoint (mask[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 (const auto &point : pp_2d)
320 {
321 if (point.x < min_pt_2d.x) min_pt_2d.x = point.x;
322 if (point.y < min_pt_2d.y) min_pt_2d.y = point.y;
323 if (point.x > max_pt_2d.x) max_pt_2d.x = point.x;
324 if (point.y > max_pt_2d.y) max_pt_2d.y = point.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///////////////////////////////////////////////////////////////////////////////////////////
341template <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///////////////////////////////////////////////////////////////////////////////////////////
351template <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 auto 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<std::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 (std::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 (std::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 std::fill_n(&data_[j], source_img.width * 3, 0);
403 j += source_img.width * 3;
404 }
405
406 // Still need to copy the target?
407 if (i < source_img.height)
408 {
409 for (std::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 std::fill_n(&data_[j], target_img.width * 3, 0);
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 (std::size_t i = 0; i < correspondences.size (); i += nth)
437 {
438 double r, g, b;
439 getRandomColors (r, g, b);
440 auto u_r = static_cast<unsigned char> (255.0 * r);
441 auto u_g = static_cast<unsigned char> (255.0 * g);
442 auto 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_
PlanarPolygon represents a planar (2D) polygon, potentially in a 3D space.
pcl::PointCloud< PointT >::VectorType & getContour()
Getter for the contour / boundary.
PointCloud represents the base class in PCL for storing collections of 3D points.
std::uint32_t width
The point cloud width (if organized as an image-structure).
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
std::uint32_t height
The point cloud height (if organized as an image-structure).
std::size_t size() const
shared_ptr< const PointCloud< PointT > > ConstPtr
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
Definition organized.h:61
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:125
bool projectPoint(const PointT &p, pcl::PointXY &q) const
projects a point into the image
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.
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.
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.
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.
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.
void addRGBImage(const unsigned char *data, unsigned width, unsigned height, const std::string &layer_id="rgb_image", double opacity=1.0, bool autoresize=true)
Add an RGB 2D image layer, but do not render it (use spin/spinOnce to update).
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.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
A 2D point structure representing Euclidean xy coordinates.