Point Cloud Library (PCL)  1.7.2
point_cloud_image_extractors.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2013-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_
39 #define PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_
40 
41 #include <set>
42 #include <map>
43 #include <ctime>
44 #include <cstdlib>
45 
46 #include <pcl/common/io.h>
47 
48 ///////////////////////////////////////////////////////////////////////////////////////////
49 template <typename PointT> bool
51 {
52  if (!cloud.isOrganized () || cloud.points.size () != cloud.width * cloud.height)
53  return (false);
54 
55  bool result = this->extractImpl (cloud, img);
56 
57  if (paint_nans_with_black_ && result)
58  {
59  size_t size = img.encoding == "mono16" ? 2 : 3;
60  for (size_t i = 0; i < cloud.points.size (); ++i)
61  if (!pcl::isFinite (cloud[i]))
62  std::memset (&img.data[i * size], 0, size);
63  }
64 
65  return (result);
66 }
67 
68 ///////////////////////////////////////////////////////////////////////////////////////////
69 template <typename PointT> bool
71 {
72  std::vector<pcl::PCLPointField> fields;
73  int field_x_idx = pcl::getFieldIndex (cloud, "normal_x", fields);
74  int field_y_idx = pcl::getFieldIndex (cloud, "normal_y", fields);
75  int field_z_idx = pcl::getFieldIndex (cloud, "normal_z", fields);
76  if (field_x_idx == -1 || field_y_idx == -1 || field_z_idx == -1)
77  return (false);
78  const size_t offset_x = fields[field_x_idx].offset;
79  const size_t offset_y = fields[field_y_idx].offset;
80  const size_t offset_z = fields[field_z_idx].offset;
81 
82  img.encoding = "rgb8";
83  img.width = cloud.width;
84  img.height = cloud.height;
85  img.step = img.width * sizeof (unsigned char) * 3;
86  img.data.resize (img.step * img.height);
87 
88  for (size_t i = 0; i < cloud.points.size (); ++i)
89  {
90  float x;
91  float y;
92  float z;
93  pcl::getFieldValue<PointT, float> (cloud.points[i], offset_x, x);
94  pcl::getFieldValue<PointT, float> (cloud.points[i], offset_y, y);
95  pcl::getFieldValue<PointT, float> (cloud.points[i], offset_z, z);
96  img.data[i * 3 + 0] = static_cast<unsigned char>((x + 1.0) * 127);
97  img.data[i * 3 + 1] = static_cast<unsigned char>((y + 1.0) * 127);
98  img.data[i * 3 + 2] = static_cast<unsigned char>((z + 1.0) * 127);
99  }
100 
101  return (true);
102 }
103 
104 ///////////////////////////////////////////////////////////////////////////////////////////
105 template <typename PointT> bool
107 {
108  std::vector<pcl::PCLPointField> fields;
109  int field_idx = pcl::getFieldIndex (cloud, "rgb", fields);
110  if (field_idx == -1)
111  {
112  field_idx = pcl::getFieldIndex (cloud, "rgba", fields);
113  if (field_idx == -1)
114  return (false);
115  }
116  const size_t offset = fields[field_idx].offset;
117 
118  img.encoding = "rgb8";
119  img.width = cloud.width;
120  img.height = cloud.height;
121  img.step = img.width * sizeof (unsigned char) * 3;
122  img.data.resize (img.step * img.height);
123 
124  for (size_t i = 0; i < cloud.points.size (); ++i)
125  {
126  uint32_t val;
127  pcl::getFieldValue<PointT, uint32_t> (cloud.points[i], offset, val);
128  img.data[i * 3 + 0] = (val >> 16) & 0x0000ff;
129  img.data[i * 3 + 1] = (val >> 8) & 0x0000ff;
130  img.data[i * 3 + 2] = (val) & 0x0000ff;
131  }
132 
133  return (true);
134 }
135 
136 // Lookup table is copied from (excluding the first entry):
137 // https://github.com/fiji/fiji/blob/master/luts/glasbey.lut
138 const uint8_t GLASBEY_LUT[] =
139 {
140  0, 0, 255,
141  255, 0, 0,
142  0, 255, 0,
143  0, 0, 51,
144  255, 0, 182,
145  0, 83, 0,
146  255, 211, 0,
147  0, 159, 255,
148  154, 77, 66,
149  0, 255, 190,
150  120, 63, 193,
151  31, 150, 152,
152  255, 172, 253,
153  177, 204, 113,
154  241, 8, 92,
155  254, 143, 66,
156  221, 0, 255,
157  32, 26, 1,
158  114, 0, 85,
159  118, 108, 149,
160  2, 173, 36,
161  200, 255, 0,
162  136, 108, 0,
163  255, 183, 159,
164  133, 133, 103,
165  161, 3, 0,
166  20, 249, 255,
167  0, 71, 158,
168  220, 94, 147,
169  147, 212, 255,
170  0, 76, 255,
171  0, 66, 80,
172  57, 167, 106,
173  238, 112, 254,
174  0, 0, 100,
175  171, 245, 204,
176  161, 146, 255,
177  164, 255, 115,
178  255, 206, 113,
179  71, 0, 21,
180  212, 173, 197,
181  251, 118, 111,
182  171, 188, 0,
183  117, 0, 215,
184  166, 0, 154,
185  0, 115, 254,
186  165, 93, 174,
187  98, 132, 2,
188  0, 121, 168,
189  0, 255, 131,
190  86, 53, 0,
191  159, 0, 63,
192  66, 45, 66,
193  255, 242, 187,
194  0, 93, 67,
195  252, 255, 124,
196  159, 191, 186,
197  167, 84, 19,
198  74, 39, 108,
199  0, 16, 166,
200  145, 78, 109,
201  207, 149, 0,
202  195, 187, 255,
203  253, 68, 64,
204  66, 78, 32,
205  106, 1, 0,
206  181, 131, 84,
207  132, 233, 147,
208  96, 217, 0,
209  255, 111, 211,
210  102, 75, 63,
211  254, 100, 0,
212  228, 3, 127,
213  17, 199, 174,
214  210, 129, 139,
215  91, 118, 124,
216  32, 59, 106,
217  180, 84, 255,
218  226, 8, 210,
219  0, 1, 20,
220  93, 132, 68,
221  166, 250, 255,
222  97, 123, 201,
223  98, 0, 122,
224  126, 190, 58,
225  0, 60, 183,
226  255, 253, 0,
227  7, 197, 226,
228  180, 167, 57,
229  148, 186, 138,
230  204, 187, 160,
231  55, 0, 49,
232  0, 40, 1,
233  150, 122, 129,
234  39, 136, 38,
235  206, 130, 180,
236  150, 164, 196,
237  180, 32, 128,
238  110, 86, 180,
239  147, 0, 185,
240  199, 48, 61,
241  115, 102, 255,
242  15, 187, 253,
243  172, 164, 100,
244  182, 117, 250,
245  216, 220, 254,
246  87, 141, 113,
247  216, 85, 34,
248  0, 196, 103,
249  243, 165, 105,
250  216, 255, 182,
251  1, 24, 219,
252  52, 66, 54,
253  255, 154, 0,
254  87, 95, 1,
255  198, 241, 79,
256  255, 95, 133,
257  123, 172, 240,
258  120, 100, 49,
259  162, 133, 204,
260  105, 255, 220,
261  198, 82, 100,
262  121, 26, 64,
263  0, 238, 70,
264  231, 207, 69,
265  217, 128, 233,
266  255, 211, 209,
267  209, 255, 141,
268  36, 0, 3,
269  87, 163, 193,
270  211, 231, 201,
271  203, 111, 79 ,
272  62, 24, 0,
273  0, 117, 223,
274  112, 176, 88 ,
275  209, 24, 0,
276  0, 30, 107,
277  105, 200, 197,
278  255, 203, 255,
279  233, 194, 137,
280  191, 129, 46,
281  69, 42, 145,
282  171, 76, 194,
283  14, 117, 61,
284  0, 30, 25,
285  118, 73, 127,
286  255, 169, 200,
287  94, 55, 217,
288  238, 230, 138,
289  159, 54, 33,
290  80, 0, 148,
291  189, 144, 128,
292  0, 109, 126,
293  88, 223, 96,
294  71, 80, 103,
295  1, 93, 159,
296  99, 48, 60,
297  2, 206, 148,
298  139, 83, 37,
299  171, 0, 255,
300  141, 42, 135,
301  85, 83, 148,
302  150, 255, 0,
303  0, 152, 123,
304  255, 138, 203,
305  222, 69, 200,
306  107, 109, 230,
307  30, 0, 68,
308  173, 76, 138,
309  255, 134, 161,
310  0, 35, 60,
311  138, 205, 0,
312  111, 202, 157,
313  225, 75, 253,
314  255, 176, 77,
315  229, 232, 57,
316  114, 16, 255,
317  111, 82, 101,
318  134, 137, 48,
319  99, 38, 80,
320  105, 38, 32,
321  200, 110, 0,
322  209, 164, 255,
323  198, 210, 86,
324  79, 103, 77,
325  174, 165, 166,
326  170, 45, 101,
327  199, 81, 175,
328  255, 89, 172,
329  146, 102, 78,
330  102, 134, 184,
331  111, 152, 255,
332  92, 255, 159,
333  172, 137, 178,
334  210, 34, 98,
335  199, 207, 147,
336  255, 185, 30,
337  250, 148, 141,
338  49, 34, 78,
339  254, 81, 97,
340  254, 141, 100,
341  68, 54, 23,
342  201, 162, 84,
343  199, 232, 240,
344  68, 152, 0,
345  147, 172, 58,
346  22, 75, 28,
347  8, 84, 121,
348  116, 45, 0,
349  104, 60, 255,
350  64, 41, 38,
351  164, 113, 215,
352  207, 0, 155,
353  118, 1, 35,
354  83, 0, 88,
355  0, 82, 232,
356  43, 92, 87,
357  160, 217, 146,
358  176, 26, 229,
359  29, 3, 36,
360  122, 58, 159,
361  214, 209, 207,
362  160, 100, 105,
363  106, 157, 160,
364  153, 219, 113,
365  192, 56, 207,
366  125, 255, 89,
367  149, 0, 34,
368  213, 162, 223,
369  22, 131, 204,
370  166, 249, 69,
371  109, 105, 97,
372  86, 188, 78,
373  255, 109, 81,
374  255, 3, 248,
375  255, 0, 73,
376  202, 0, 35,
377  67, 109, 18,
378  234, 170, 173,
379  191, 165, 0,
380  38, 44, 51,
381  85, 185, 2,
382  121, 182, 158,
383  254, 236, 212,
384  139, 165, 89,
385  141, 254, 193,
386  0, 60, 43,
387  63, 17, 40,
388  255, 221, 246,
389  17, 26, 146,
390  154, 66, 84,
391  149, 157, 238,
392  126, 130, 72,
393  58, 6, 101,
394  189, 117, 101,
395 };
396 
397 const size_t GLASBEY_LUT_SIZE = sizeof (GLASBEY_LUT) / sizeof (GLASBEY_LUT[0]);
398 
399 ///////////////////////////////////////////////////////////////////////////////////////////
400 template <typename PointT> bool
402 {
403  std::vector<pcl::PCLPointField> fields;
404  int field_idx = pcl::getFieldIndex (cloud, "label", fields);
405  if (field_idx == -1)
406  return (false);
407  const size_t offset = fields[field_idx].offset;
408 
409  switch (color_mode_)
410  {
411  case COLORS_MONO:
412  {
413  img.encoding = "mono16";
414  img.width = cloud.width;
415  img.height = cloud.height;
416  img.step = img.width * sizeof (unsigned short);
417  img.data.resize (img.step * img.height);
418  unsigned short* data = reinterpret_cast<unsigned short*>(&img.data[0]);
419  for (size_t i = 0; i < cloud.points.size (); ++i)
420  {
421  uint32_t val;
422  pcl::getFieldValue<PointT, uint32_t> (cloud.points[i], offset, val);
423  data[i] = static_cast<unsigned short>(val);
424  }
425  break;
426  }
427  case COLORS_RGB_RANDOM:
428  {
429  img.encoding = "rgb8";
430  img.width = cloud.width;
431  img.height = cloud.height;
432  img.step = img.width * sizeof (unsigned char) * 3;
433  img.data.resize (img.step * img.height);
434 
435  std::srand(std::time(0));
436  std::map<uint32_t, size_t> colormap;
437 
438  for (size_t i = 0; i < cloud.points.size (); ++i)
439  {
440  uint32_t val;
441  pcl::getFieldValue<PointT, uint32_t> (cloud.points[i], offset, val);
442  if (colormap.count (val) == 0)
443  {
444  colormap[val] = i * 3;
445  img.data[i * 3 + 0] = static_cast<uint8_t> ((std::rand () % 256));
446  img.data[i * 3 + 1] = static_cast<uint8_t> ((std::rand () % 256));
447  img.data[i * 3 + 2] = static_cast<uint8_t> ((std::rand () % 256));
448  }
449  else
450  {
451  memcpy (&img.data[i * 3], &img.data[colormap[val]], 3);
452  }
453  }
454  break;
455  }
456  case COLORS_RGB_GLASBEY:
457  {
458  img.encoding = "rgb8";
459  img.width = cloud.width;
460  img.height = cloud.height;
461  img.step = img.width * sizeof (unsigned char) * 3;
462  img.data.resize (img.step * img.height);
463 
464  std::srand(std::time(0));
465  std::set<uint32_t> labels;
466  std::map<uint32_t, size_t> colormap;
467 
468  // First pass: find unique labels
469  for (size_t i = 0; i < cloud.points.size (); ++i)
470  {
471  uint32_t val;
472  pcl::getFieldValue<PointT, uint32_t> (cloud.points[i], offset, val);
473  labels.insert (val);
474  }
475 
476  // Assign Glasbey colors in ascending order of labels
477  size_t color = 0;
478  for (std::set<uint32_t>::iterator iter = labels.begin (); iter != labels.end (); ++iter)
479  {
480  colormap[*iter] = color % GLASBEY_LUT_SIZE;
481  ++color;
482  }
483 
484  // Second pass: copy colors from the LUT
485  for (size_t i = 0; i < cloud.points.size (); ++i)
486  {
487  uint32_t val;
488  pcl::getFieldValue<PointT, uint32_t> (cloud.points[i], offset, val);
489  memcpy (&img.data[i * 3], &GLASBEY_LUT[colormap[val] * 3], 3);
490  }
491 
492  break;
493  }
494  }
495 
496  return (true);
497 }
498 
499 ///////////////////////////////////////////////////////////////////////////////////////////
500 template <typename PointT> bool
502 {
503  std::vector<pcl::PCLPointField> fields;
504  int field_idx = pcl::getFieldIndex (cloud, field_name_, fields);
505  if (field_idx == -1)
506  return (false);
507  const size_t offset = fields[field_idx].offset;
508 
509  img.encoding = "mono16";
510  img.width = cloud.width;
511  img.height = cloud.height;
512  img.step = img.width * sizeof (unsigned short);
513  img.data.resize (img.step * img.height);
514  unsigned short* data = reinterpret_cast<unsigned short*>(&img.data[0]);
515 
516  float scaling_factor = scaling_factor_;
517  float data_min = 0.0f;
518  if (scaling_method_ == SCALING_FULL_RANGE)
519  {
520  float min = std::numeric_limits<float>::infinity();
521  float max = -std::numeric_limits<float>::infinity();
522  for (size_t i = 0; i < cloud.points.size (); ++i)
523  {
524  float val;
525  pcl::getFieldValue<PointT, float> (cloud.points[i], offset, val);
526  if (val < min)
527  min = val;
528  if (val > max)
529  max = val;
530  }
531  scaling_factor = min == max ? 0 : std::numeric_limits<unsigned short>::max() / (max - min);
532  data_min = min;
533  }
534 
535  for (size_t i = 0; i < cloud.points.size (); ++i)
536  {
537  float val;
538  pcl::getFieldValue<PointT, float> (cloud.points[i], offset, val);
539  if (scaling_method_ == SCALING_NO)
540  {
541  data[i] = val;
542  }
543  else if (scaling_method_ == SCALING_FULL_RANGE)
544  {
545  data[i] = (val - data_min) * scaling_factor;
546  }
547  else if (scaling_method_ == SCALING_FIXED_FACTOR)
548  {
549  data[i] = val * scaling_factor;
550  }
551  }
552 
553  return (true);
554 }
555 
556 #endif // PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_
557 
pcl::uint32_t height
Definition: PCLImage.h:24
virtual bool extractImpl(const PointCloud &cloud, pcl::PCLImage &img) const
Implementation of the extract() function, has to be implemented in deriving classes.
virtual bool extractImpl(const PointCloud &cloud, pcl::PCLImage &img) const
Implementation of the extract() function, has to be implemented in deriving classes.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
Definition: point_tests.h:53
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
Definition: io.h:59
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
virtual bool extractImpl(const PointCloud &cloud, pcl::PCLImage &image) const
Implementation of the extract() function, has to be implemented in deriving classes.
bool extract(const PointCloud &cloud, pcl::PCLImage &image) const
Obtain the image from the given cloud.
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
Definition: point_cloud.h:331
pcl::uint32_t width
Definition: PCLImage.h:25
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
pcl::uint32_t step
Definition: PCLImage.h:29
virtual bool extractImpl(const PointCloud &cloud, pcl::PCLImage &img) const
Implementation of the extract() function, has to be implemented in deriving classes.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
PointCloud represents the base class in PCL for storing collections of 3D points. ...
std::vector< pcl::uint8_t > data
Definition: PCLImage.h:31
std::string encoding
Definition: PCLImage.h:26