Point Cloud Library (PCL)  1.7.1
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 <map>
42 #include <ctime>
43 #include <cstdlib>
44 
45 #include <pcl/common/io.h>
46 
47 ///////////////////////////////////////////////////////////////////////////////////////////
48 template <typename PointT> bool
50 {
51  if (!cloud.isOrganized () || cloud.points.size () != cloud.width * cloud.height)
52  return (false);
53 
54  std::vector<pcl::PCLPointField> fields;
55  int field_x_idx = pcl::getFieldIndex (cloud, "normal_x", fields);
56  int field_y_idx = pcl::getFieldIndex (cloud, "normal_y", fields);
57  int field_z_idx = pcl::getFieldIndex (cloud, "normal_z", fields);
58  if (field_x_idx == -1 || field_y_idx == -1 || field_z_idx == -1)
59  return (false);
60  const size_t offset_x = fields[field_x_idx].offset;
61  const size_t offset_y = fields[field_y_idx].offset;
62  const size_t offset_z = fields[field_z_idx].offset;
63 
64  img.encoding = "rgb8";
65  img.width = cloud.width;
66  img.height = cloud.height;
67  img.step = img.width * sizeof (unsigned char) * 3;
68  img.data.resize (img.step * img.height);
69 
70  for (size_t i = 0; i < cloud.points.size (); ++i)
71  {
72  float x;
73  float y;
74  float z;
75  pcl::getFieldValue<PointT, float> (cloud.points[i], offset_x, x);
76  pcl::getFieldValue<PointT, float> (cloud.points[i], offset_y, y);
77  pcl::getFieldValue<PointT, float> (cloud.points[i], offset_z, z);
78  img.data[i * 3 + 0] = static_cast<unsigned char>((x + 1.0) * 127);
79  img.data[i * 3 + 1] = static_cast<unsigned char>((y + 1.0) * 127);
80  img.data[i * 3 + 2] = static_cast<unsigned char>((z + 1.0) * 127);
81  }
82 
83  return (true);
84 }
85 
86 ///////////////////////////////////////////////////////////////////////////////////////////
87 template <typename PointT> bool
89 {
90  if (!cloud.isOrganized () || cloud.points.size () != cloud.width * cloud.height)
91  return (false);
92 
93  std::vector<pcl::PCLPointField> fields;
94  int field_idx = pcl::getFieldIndex (cloud, "rgb", fields);
95  if (field_idx == -1)
96  {
97  field_idx = pcl::getFieldIndex (cloud, "rgba", fields);
98  if (field_idx == -1)
99  return (false);
100  }
101  const size_t offset = fields[field_idx].offset;
102 
103  img.encoding = "rgb8";
104  img.width = cloud.width;
105  img.height = cloud.height;
106  img.step = img.width * sizeof (unsigned char) * 3;
107  img.data.resize (img.step * img.height);
108 
109  for (size_t i = 0; i < cloud.points.size (); ++i)
110  {
111  uint32_t val;
112  pcl::getFieldValue<PointT, uint32_t> (cloud.points[i], offset, val);
113  img.data[i * 3 + 0] = (val >> 16) & 0x0000ff;
114  img.data[i * 3 + 1] = (val >> 8) & 0x0000ff;
115  img.data[i * 3 + 2] = (val) & 0x0000ff;
116  }
117 
118  return (true);
119 }
120 
121 ///////////////////////////////////////////////////////////////////////////////////////////
122 template <typename PointT> bool
124 {
125  if (!cloud.isOrganized () || cloud.points.size () != cloud.width * cloud.height)
126  return (false);
127 
128  std::vector<pcl::PCLPointField> fields;
129  int field_idx = pcl::getFieldIndex (cloud, "label", fields);
130  if (field_idx == -1)
131  return (false);
132  const size_t offset = fields[field_idx].offset;
133 
134  switch (color_mode_)
135  {
136  case COLORS_MONO:
137  {
138  img.encoding = "mono16";
139  img.width = cloud.width;
140  img.height = cloud.height;
141  img.step = img.width * sizeof (unsigned short);
142  img.data.resize (img.step * img.height);
143  unsigned short* data = reinterpret_cast<unsigned short*>(&img.data[0]);
144  for (size_t i = 0; i < cloud.points.size (); ++i)
145  {
146  uint32_t val;
147  pcl::getFieldValue<PointT, uint32_t> (cloud.points[i], offset, val);
148  data[i] = static_cast<unsigned short>(val);
149  }
150  break;
151  }
152  case COLORS_RGB_RANDOM:
153  {
154  img.encoding = "rgb8";
155  img.width = cloud.width;
156  img.height = cloud.height;
157  img.step = img.width * sizeof (unsigned char) * 3;
158  img.data.resize (img.step * img.height);
159 
160  std::srand(std::time(0));
161  std::map<uint32_t, size_t> colormap;
162 
163  for (size_t i = 0; i < cloud.points.size (); ++i)
164  {
165  uint32_t val;
166  pcl::getFieldValue<PointT, uint32_t> (cloud.points[i], offset, val);
167  if (colormap.count (val) == 0)
168  {
169  colormap[val] = i * 3;
170  img.data[i * 3 + 0] = static_cast<uint8_t> ((std::rand () % 256));
171  img.data[i * 3 + 1] = static_cast<uint8_t> ((std::rand () % 256));
172  img.data[i * 3 + 2] = static_cast<uint8_t> ((std::rand () % 256));
173  }
174  else
175  {
176  memcpy (&img.data[i * 3], &img.data[colormap[val]], 3);
177  }
178  }
179  break;
180  }
181  }
182 
183  return (true);
184 }
185 
186 ///////////////////////////////////////////////////////////////////////////////////////////
187 template <typename PointT> bool
189 {
190  if (!cloud.isOrganized () || cloud.points.size () != cloud.width * cloud.height)
191  return (false);
192 
193  std::vector<pcl::PCLPointField> fields;
194  int field_idx = pcl::getFieldIndex (cloud, field_name_, fields);
195  if (field_idx == -1)
196  return (false);
197  const size_t offset = fields[field_idx].offset;
198 
199  img.encoding = "mono16";
200  img.width = cloud.width;
201  img.height = cloud.height;
202  img.step = img.width * sizeof (unsigned short);
203  img.data.resize (img.step * img.height);
204  unsigned short* data = reinterpret_cast<unsigned short*>(&img.data[0]);
205 
206  float scaling_factor = scaling_factor_;
207  float data_min = 0.0f;
208  if (scaling_method_ == SCALING_FULL_RANGE)
209  {
210  float min = std::numeric_limits<float>::infinity();
211  float max = -std::numeric_limits<float>::infinity();
212  for (size_t i = 0; i < cloud.points.size (); ++i)
213  {
214  float val;
215  pcl::getFieldValue<PointT, float> (cloud.points[i], offset, val);
216  if (val < min)
217  min = val;
218  if (val > max)
219  max = val;
220  }
221  scaling_factor = min == max ? 0 : std::numeric_limits<unsigned short>::max() / (max - min);
222  data_min = min;
223  }
224 
225  for (size_t i = 0; i < cloud.points.size (); ++i)
226  {
227  float val;
228  pcl::getFieldValue<PointT, float> (cloud.points[i], offset, val);
229  if (scaling_method_ == SCALING_NO)
230  {
231  data[i] = val;
232  }
233  else if (scaling_method_ == SCALING_FULL_RANGE)
234  {
235  data[i] = (val - data_min) * scaling_factor;
236  }
237  else if (scaling_method_ == SCALING_FIXED_FACTOR)
238  {
239  data[i] = val * scaling_factor;
240  }
241  }
242 
243  return (true);
244 }
245 
246 #endif // PCL_POINT_CLOUD_IMAGE_EXTRACTORS_IMPL_HPP_
247 
virtual bool extract(const PointCloud &cloud, pcl::PCLImage &img) const
Obtain the label image from the given cloud.
std::string encoding
Definition: PCLImage.h:26
pcl::uint32_t width
Definition: PCLImage.h:25
virtual bool extract(const PointCloud &cloud, pcl::PCLImage &img) const
Obtain the color image from the given cloud.
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:58
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
pcl::uint32_t step
Definition: PCLImage.h:29
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
Definition: point_cloud.h:331
std::vector< pcl::uint8_t > data
Definition: PCLImage.h:31
virtual bool extract(const PointCloud &cloud, pcl::PCLImage &img) const
Obtain the color image from the given cloud.
virtual bool extract(const PointCloud &cloud, pcl::PCLImage &image) const
Obtain the image from the given cloud.
pcl::uint32_t height
Definition: PCLImage.h:24