39 #ifndef PCL_VISUALIZATION_IMAGE_VISUALIZER_HPP_
40 #define PCL_VISUALIZATION_IMAGE_VISUALIZER_HPP_
42 #include <vtkContextActor.h>
43 #include <vtkContextScene.h>
44 #include <vtkImageData.h>
45 #include <vtkImageFlip.h>
46 #include <vtkPointData.h>
47 #include <vtkImageViewer.h>
49 #include <pcl/visualization/common/common.h>
50 #include <pcl/search/organized.h>
53 template <
typename T>
void
56 boost::shared_array<unsigned char> &data)
59 for (
size_t i = 0; i < cloud.
points.size (); ++i)
61 data[j++] = cloud.
points[i].r;
62 data[j++] = cloud.
points[i].g;
63 data[j++] = cloud.
points[i].b;
68 template <
typename T>
void
70 const std::string &layer_id,
76 data_.reset (
new unsigned char[data_size_]);
79 convertRGBCloudToUChar (cloud, data_);
81 return (addRGBImage (data_.get (), cloud.
width, cloud.
height, layer_id, opacity));
85 template <
typename T>
void
87 const std::string &layer_id,
90 addRGBImage<T> (cloud, layer_id, opacity);
95 template <
typename T>
bool
99 double r,
double g,
double b,
100 const std::string &layer_id,
double opacity)
107 LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
108 if (am_it == layer_map_.end ())
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);
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)
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);
129 xy.push_back (p_projected.
y);
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);
143 template <
typename T>
bool
147 const std::string &layer_id,
double opacity)
149 return (addMask (image, mask, 1.0, 0.0, 0.0, layer_id, opacity));
153 template <
typename T>
bool
157 double r,
double g,
double b,
158 const std::string &layer_id,
double opacity)
165 LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
166 if (am_it == layer_map_.end ())
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);
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)
183 #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 10))
184 xy.push_back (image_height_f - p.
y);
191 xy[xy.size () - 2] = xy[0];
192 xy[xy.size () - 1] = xy[1];
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));
199 am_it->actor->GetScene ()->AddItem (poly);
205 template <
typename T>
bool
209 const std::string &layer_id,
double opacity)
211 return (addPlanarPolygon (image, polygon, 1.0, 0.0, 0.0, layer_id, opacity));
215 template <
typename T>
bool
220 double r,
double g,
double b,
221 const std::string &layer_id,
double opacity)
228 LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
229 if (am_it == layer_map_.end ())
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);
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;
249 std::vector<pcl::PointXY> pp_2d (8);
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 ();
263 for (
size_t i = 0; i < pp_2d.size (); ++i)
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;
270 #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 7))
271 min_pt_2d.
y = float (image->
height) - min_pt_2d.
y;
272 max_pt_2d.
y = float (image->
height) - max_pt_2d.
y;
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, max_pt_2d.
y);
281 am_it->actor->GetScene ()->AddItem (rect);
287 template <
typename T>
bool
292 const std::string &layer_id,
double opacity)
294 return (addRectangle<T> (image, min_pt, max_pt, 0.0, 1.0, 0.0, layer_id, opacity));
298 template <
typename T>
bool
302 double r,
double g,
double b,
303 const std::string &layer_id,
double opacity)
310 LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
311 if (am_it == layer_map_.end ())
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);
320 std::vector<pcl::PointXY> pp_2d (mask.
points.size ());
321 for (
size_t i = 0; i < mask.
points.size (); ++i)
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 ();
328 for (
size_t i = 0; i < pp_2d.size (); ++i)
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;
335 #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 7))
336 min_pt_2d.
y = float (image->
height) - min_pt_2d.
y;
337 max_pt_2d.
y = float (image->
height) - max_pt_2d.
y;
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, max_pt_2d.
y);
346 am_it->actor->GetScene ()->AddItem (rect);
352 template <
typename T>
bool
356 const std::string &layer_id,
double opacity)
358 return (addRectangle (image, mask, 0.0, 1.0, 0.0, layer_id, opacity));
362 template <
typename Po
intT>
bool
368 const std::string &layer_id)
370 if (correspondences.empty ())
372 PCL_DEBUG (
"[pcl::visualization::ImageViewer::addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
377 LayerMap::iterator am_it = std::find_if (layer_map_.begin (), layer_map_.end (), LayerComparator (layer_id));
378 if (am_it == layer_map_.end ())
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);
384 int src_size = source_img.
width * source_img.
height * 3;
385 int tgt_size = target_img.
width * target_img.
height * 3;
391 if (data_size_ < (src_size + tgt_size))
393 data_size_ = src_size + tgt_size;
394 data_.reset (
new unsigned char[data_size_]);
399 for (
size_t i = 0; i < std::max (source_img.
height, target_img.
height); ++i)
402 if (i < source_img.
height)
404 for (
size_t k = 0; k < source_img.
width; ++k)
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;
413 memcpy (&data_[j], 0, source_img.
width * 3);
414 j += source_img.
width * 3;
418 if (i < source_img.
height)
420 for (
size_t k = 0; k < target_img.
width; ++k)
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;
429 memcpy (&data_[j], 0, target_img.
width * 3);
430 j += target_img.
width * 3;
434 void* data =
const_cast<void*
> (
reinterpret_cast<const void*
> (data_.get ()));
437 image->SetDimensions (source_img.
width + target_img.
width, std::max (source_img.
height, target_img.
height), 1);
438 image->AllocateScalars (VTK_UNSIGNED_CHAR, 3);
439 image->GetPointData ()->GetScalars ()->SetVoidArray (data, data_size_, 1);
441 #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 10))
443 algo_->SetInput (image);
445 image_item->set (0, 0, algo_->GetOutput ());
447 image_item->set (0, 0, image);
448 interactor_style_->adjustCamera (image, ren_);
450 am_it->actor->GetScene ()->AddItem (image_item);
451 image_viewer_->SetSize (image->GetDimensions ()[0], image->GetDimensions ()[1]);
454 for (
size_t i = 0; i < correspondences.size (); i += nth)
458 unsigned char u_r =
static_cast<unsigned char> (255.0 * r);
459 unsigned char u_g =
static_cast<unsigned char> (255.0 * g);
460 unsigned char u_b =
static_cast<unsigned char> (255.0 * b);
462 query_circle->setColors (u_r, u_g, u_b);
464 match_circle->setColors (u_r, u_g, u_b);
466 line->setColors (u_r, u_g, u_b);
468 float query_x = correspondences[i].index_query % source_img.
width;
469 float match_x = correspondences[i].index_match % target_img.
width + source_img.
width;
470 #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION > 10))
471 float query_y = correspondences[i].index_query / source_img.
width;
472 float match_y = correspondences[i].index_match / target_img.
width;
474 float query_y = getSize ()[1] - correspondences[i].index_query / source_img.
width;
475 float match_y = getSize ()[1] - correspondences[i].index_match / target_img.
width;
478 query_circle->set (query_x, query_y, 3.0);
479 match_circle->set (match_x, match_y, 3.0);
480 line->set (query_x, query_y, match_x, match_y);
482 am_it->actor->GetScene ()->AddItem (query_circle);
483 am_it->actor->GetScene ()->AddItem (match_circle);
484 am_it->actor->GetScene ()->AddItem (line);
490 #endif // PCL_VISUALIZATION_IMAGE_VISUALIZER_HPP_
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.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
PlanarPolygon represents a planar (2D) polygon, potentially in a 3D space.
A 2D point structure representing Euclidean xy coordinates.
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.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
uint32_t height
The point cloud height (if organized as an image-structure).
bool projectPoint(const PointT &p, pcl::PointXY &q) const
projects a point into the image
pcl::PointCloud< PointT >::VectorType & getContour()
Getter for the contour / boundary.
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
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.
virtual void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input data set, if user has focal length he must set it before calling this...
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 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.
uint32_t width
The point cloud width (if organized as an image-structure).
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void convertRGBCloudToUChar(const pcl::PointCloud< T > &cloud, boost::shared_array< unsigned char > &data)
Convert the RGB information in a PointCloud to an unsigned char array.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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.