Point Cloud Library (PCL)  1.9.0-dev
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 #include <pcl/common/colors.h>
48 
49 ///////////////////////////////////////////////////////////////////////////////////////////
50 template <typename PointT> bool
52 {
53  if (!cloud.isOrganized () || cloud.points.size () != cloud.width * cloud.height)
54  return (false);
55 
56  bool result = this->extractImpl (cloud, img);
57 
58  if (paint_nans_with_black_ && result)
59  {
60  size_t size = img.encoding == "mono16" ? 2 : 3;
61  for (size_t i = 0; i < cloud.points.size (); ++i)
62  if (!pcl::isFinite (cloud[i]))
63  std::memset (&img.data[i * size], 0, size);
64  }
65 
66  return (result);
67 }
68 
69 ///////////////////////////////////////////////////////////////////////////////////////////
70 template <typename PointT> bool
72 {
73  std::vector<pcl::PCLPointField> fields;
74  int field_x_idx = pcl::getFieldIndex (cloud, "normal_x", fields);
75  int field_y_idx = pcl::getFieldIndex (cloud, "normal_y", fields);
76  int field_z_idx = pcl::getFieldIndex (cloud, "normal_z", fields);
77  if (field_x_idx == -1 || field_y_idx == -1 || field_z_idx == -1)
78  return (false);
79  const size_t offset_x = fields[field_x_idx].offset;
80  const size_t offset_y = fields[field_y_idx].offset;
81  const size_t offset_z = fields[field_z_idx].offset;
82 
83  img.encoding = "rgb8";
84  img.width = cloud.width;
85  img.height = cloud.height;
86  img.step = img.width * sizeof (unsigned char) * 3;
87  img.data.resize (img.step * img.height);
88 
89  for (size_t i = 0; i < cloud.points.size (); ++i)
90  {
91  float x;
92  float y;
93  float z;
94  pcl::getFieldValue<PointT, float> (cloud.points[i], offset_x, x);
95  pcl::getFieldValue<PointT, float> (cloud.points[i], offset_y, y);
96  pcl::getFieldValue<PointT, float> (cloud.points[i], offset_z, z);
97  img.data[i * 3 + 0] = static_cast<unsigned char>((x + 1.0) * 127);
98  img.data[i * 3 + 1] = static_cast<unsigned char>((y + 1.0) * 127);
99  img.data[i * 3 + 2] = static_cast<unsigned char>((z + 1.0) * 127);
100  }
101 
102  return (true);
103 }
104 
105 ///////////////////////////////////////////////////////////////////////////////////////////
106 template <typename PointT> bool
108 {
109  std::vector<pcl::PCLPointField> fields;
110  int field_idx = pcl::getFieldIndex (cloud, "rgb", fields);
111  if (field_idx == -1)
112  {
113  field_idx = pcl::getFieldIndex (cloud, "rgba", fields);
114  if (field_idx == -1)
115  return (false);
116  }
117  const size_t offset = fields[field_idx].offset;
118 
119  img.encoding = "rgb8";
120  img.width = cloud.width;
121  img.height = cloud.height;
122  img.step = img.width * sizeof (unsigned char) * 3;
123  img.data.resize (img.step * img.height);
124 
125  for (size_t i = 0; i < cloud.points.size (); ++i)
126  {
127  uint32_t val;
128  pcl::getFieldValue<PointT, uint32_t> (cloud.points[i], offset, val);
129  img.data[i * 3 + 0] = (val >> 16) & 0x0000ff;
130  img.data[i * 3 + 1] = (val >> 8) & 0x0000ff;
131  img.data[i * 3 + 2] = (val) & 0x0000ff;
132  }
133 
134  return (true);
135 }
136 
137 ///////////////////////////////////////////////////////////////////////////////////////////
138 template <typename PointT> bool
140 {
141  std::vector<pcl::PCLPointField> fields;
142  int field_idx = pcl::getFieldIndex (cloud, "label", fields);
143  if (field_idx == -1)
144  return (false);
145  const size_t offset = fields[field_idx].offset;
146 
147  switch (color_mode_)
148  {
149  case COLORS_MONO:
150  {
151  img.encoding = "mono16";
152  img.width = cloud.width;
153  img.height = cloud.height;
154  img.step = img.width * sizeof (unsigned short);
155  img.data.resize (img.step * img.height);
156  unsigned short* data = reinterpret_cast<unsigned short*>(&img.data[0]);
157  for (size_t i = 0; i < cloud.points.size (); ++i)
158  {
159  uint32_t val;
160  pcl::getFieldValue<PointT, uint32_t> (cloud.points[i], offset, val);
161  data[i] = static_cast<unsigned short>(val);
162  }
163  break;
164  }
165  case COLORS_RGB_RANDOM:
166  {
167  img.encoding = "rgb8";
168  img.width = cloud.width;
169  img.height = cloud.height;
170  img.step = img.width * sizeof (unsigned char) * 3;
171  img.data.resize (img.step * img.height);
172 
173  std::srand(std::time(0));
174  std::map<uint32_t, size_t> colormap;
175 
176  for (size_t i = 0; i < cloud.points.size (); ++i)
177  {
178  uint32_t val;
179  pcl::getFieldValue<PointT, uint32_t> (cloud.points[i], offset, val);
180  if (colormap.count (val) == 0)
181  {
182  colormap[val] = i * 3;
183  img.data[i * 3 + 0] = static_cast<uint8_t> ((std::rand () % 256));
184  img.data[i * 3 + 1] = static_cast<uint8_t> ((std::rand () % 256));
185  img.data[i * 3 + 2] = static_cast<uint8_t> ((std::rand () % 256));
186  }
187  else
188  {
189  memcpy (&img.data[i * 3], &img.data[colormap[val]], 3);
190  }
191  }
192  break;
193  }
194  case COLORS_RGB_GLASBEY:
195  {
196  img.encoding = "rgb8";
197  img.width = cloud.width;
198  img.height = cloud.height;
199  img.step = img.width * sizeof (unsigned char) * 3;
200  img.data.resize (img.step * img.height);
201 
202  std::srand(std::time(0));
203  std::set<uint32_t> labels;
204  std::map<uint32_t, size_t> colormap;
205 
206  // First pass: find unique labels
207  for (size_t i = 0; i < cloud.points.size (); ++i)
208  {
209  // If we need to paint NaN points with black do not waste colors on them
210  if (paint_nans_with_black_ && !pcl::isFinite (cloud.points[i]))
211  continue;
212  uint32_t val;
213  pcl::getFieldValue<PointT, uint32_t> (cloud.points[i], offset, val);
214  labels.insert (val);
215  }
216 
217  // Assign Glasbey colors in ascending order of labels
218  // Note: the color LUT has a finite size (256 colors), therefore when
219  // there are more labels the colors will repeat
220  size_t color = 0;
221  for (std::set<uint32_t>::iterator iter = labels.begin (); iter != labels.end (); ++iter)
222  {
223  colormap[*iter] = color % GlasbeyLUT::size ();
224  ++color;
225  }
226 
227  // Second pass: copy colors from the LUT
228  for (size_t i = 0; i < cloud.points.size (); ++i)
229  {
230  uint32_t val;
231  pcl::getFieldValue<PointT, uint32_t> (cloud.points[i], offset, val);
232  memcpy (&img.data[i * 3], GlasbeyLUT::data () + colormap[val] * 3, 3);
233  }
234 
235  break;
236  }
237  }
238 
239  return (true);
240 }
241 
242 ///////////////////////////////////////////////////////////////////////////////////////////
243 template <typename PointT> bool
245 {
246  std::vector<pcl::PCLPointField> fields;
247  int field_idx = pcl::getFieldIndex (cloud, field_name_, fields);
248  if (field_idx == -1)
249  return (false);
250  const size_t offset = fields[field_idx].offset;
251 
252  img.encoding = "mono16";
253  img.width = cloud.width;
254  img.height = cloud.height;
255  img.step = img.width * sizeof (unsigned short);
256  img.data.resize (img.step * img.height);
257  unsigned short* data = reinterpret_cast<unsigned short*>(&img.data[0]);
258 
259  float scaling_factor = scaling_factor_;
260  float data_min = 0.0f;
261  if (scaling_method_ == SCALING_FULL_RANGE)
262  {
263  float min = std::numeric_limits<float>::infinity();
264  float max = -std::numeric_limits<float>::infinity();
265  for (size_t i = 0; i < cloud.points.size (); ++i)
266  {
267  float val;
268  pcl::getFieldValue<PointT, float> (cloud.points[i], offset, val);
269  if (val < min)
270  min = val;
271  if (val > max)
272  max = val;
273  }
274  scaling_factor = min == max ? 0 : std::numeric_limits<unsigned short>::max() / (max - min);
275  data_min = min;
276  }
277 
278  for (size_t i = 0; i < cloud.points.size (); ++i)
279  {
280  float val;
281  pcl::getFieldValue<PointT, float> (cloud.points[i], offset, val);
282  if (scaling_method_ == SCALING_NO)
283  {
284  data[i] = val;
285  }
286  else if (scaling_method_ == SCALING_FULL_RANGE)
287  {
288  data[i] = (val - data_min) * scaling_factor;
289  }
290  else if (scaling_method_ == SCALING_FIXED_FACTOR)
291  {
292  data[i] = val * scaling_factor;
293  }
294  }
295 
296  return (true);
297 }
298 
299 #endif // PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_
300 
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 return true if f...
Definition: point_tests.h:54
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
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
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.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
pcl::uint32_t width
Definition: PCLImage.h:25
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
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.
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
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
Definition: point_cloud.h:331