PointCloudConversions.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package VisionX::Tools
19  * @author Markus Grotz ( markus dot grotz at kit dot edu )
20  * @date 2015
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "PointCloudConversions.h"
26 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
27 #include <pcl/io/pcd_io.h>
28 #include <pcl/common/colors.h>
29 
30 namespace visionx::tools
31 {
32 
33  template <> visionx::PointContentType getPointContentType<pcl::PointXYZ>()
34  {
35  return visionx::ePoints;
36  }
37  template <> visionx::PointContentType getPointContentType<pcl::PointXYZL>()
38  {
39  return visionx::eLabeledPoints;
40  }
41  template <> visionx::PointContentType getPointContentType<pcl::PointXYZI>()
42  {
43  return visionx::eIntensity;
44  }
45  template<>
46  PointContentType getPointContentType<pcl::PointNormal>()
47  {
48  return visionx::eOrientedPoints;
49  }
50  template <> visionx::PointContentType getPointContentType<pcl::PointXYZRGBA>()
51  {
52  return visionx::eColoredPoints;
53  }
54  template <> visionx::PointContentType getPointContentType<pcl::PointXYZRGBL>()
55  {
56  return visionx::eColoredLabeledPoints;
57  }
58  template <> visionx::PointContentType getPointContentType<pcl::PointXYZRGBNormal>()
59  {
60  return visionx::eColoredOrientedPoints;
61  }
62 
63 
64  size_t getBytesPerPoint(visionx::PointContentType pointContent)
65  {
66  switch (pointContent)
67  {
68  case visionx::ePoints:
69  return sizeof(visionx::Point3D);
70 
71  case visionx::eColoredPoints:
72  return sizeof(visionx::ColoredPoint3D);
73 
74  case visionx::eOrientedPoints:
75  return sizeof(visionx::OrientedPoint3D);
76 
77  case visionx::eColoredOrientedPoints:
78  return sizeof(visionx::ColoredOrientedPoint3D);
79 
80  case visionx::eLabeledPoints:
81  return sizeof(visionx::LabeledPoint3D);
82 
83  case visionx::eColoredLabeledPoints:
84  return sizeof(visionx::ColoredLabeledPoint3D);
85 
86  case visionx::eIntensity:
87  return sizeof(visionx::IntensityPoint3D);
88 
89  default:
90  return 0;
91  //ARMARX_FATAL_S << "invalid point cloud content " << pointContent;
92  }
93  }
94 
95  bool isColored(PointContentType type)
96  {
97  switch (type)
98  {
99  case visionx::eColoredPoints:
100  case visionx::eColoredOrientedPoints:
101  case visionx::eColoredLabeledPoints:
102  return true;
103  default:
104  return false;
105  }
106  }
107 
108  bool isLabeled(PointContentType type)
109  {
110  switch (type)
111  {
112  case visionx::eLabeledPoints:
113  case visionx::eColoredLabeledPoints:
114  return true;
115  default:
116  return false;
117  }
118  }
119 
120 
121  void convertFromPCL(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr sourcePtr, void** target)
122  {
123  const unsigned int Area = sourcePtr->width * sourcePtr->height;
124  visionx::ColoredPoint3D* pBuffer = new visionx::ColoredPoint3D[Area];
125 
126  size_t size = sourcePtr->points.size();
127  auto& points = sourcePtr->points;
128  for (size_t i = 0; i < size; i++)
129  {
130  auto& p = points[i];
131  auto& pB = pBuffer[i];
132  pB.point.x = p.x;
133  pB.point.y = p.y;
134  pB.point.z = p.z;
135  pB.color.r = p.r;
136  pB.color.g = p.g;
137  pB.color.b = p.b;
138  pB.color.a = p.a;
139  }
140 
141  memcpy(target[0], pBuffer, Area * sizeof(visionx::ColoredPoint3D));
142  delete[] pBuffer;
143  }
144 
145 
146  void convertToPCL(void** source, visionx::MetaPointCloudFormatPtr pointCloudFormat, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr targetPtr)
147  {
148  if (pointCloudFormat->type == visionx::ePoints)
149  {
150  targetPtr->width = static_cast<uint32_t>(pointCloudFormat->width);
151  targetPtr->height = static_cast<uint32_t>(pointCloudFormat->height);
152  targetPtr->points.resize(pointCloudFormat->size / getBytesPerPoint(pointCloudFormat->type));
153  visionx::Point3D* pBuffer = reinterpret_cast<visionx::Point3D*>(*source);
154  size_t size = targetPtr->points.size();
155  auto& points = targetPtr->points;
156  for (size_t i = 0; i < size; i++)
157  {
158  auto& p = points[i];
159  auto& pB = pBuffer[i];
160  p.x = pB.x;
161  p.y = pB.y;
162  p.z = pB.z;
163  }
164  }
165  else if (pointCloudFormat->type == visionx::eColoredPoints)
166  {
167  targetPtr->width = static_cast<uint32_t>(pointCloudFormat->width);
168  targetPtr->height = static_cast<uint32_t>(pointCloudFormat->height);
169  targetPtr->points.resize(pointCloudFormat->size / getBytesPerPoint(pointCloudFormat->type));
170  visionx::ColoredPoint3D* pBuffer = reinterpret_cast<visionx::ColoredPoint3D*>(*source);
171  size_t size = targetPtr->points.size();
172  auto& points = targetPtr->points;
173  for (size_t i = 0; i < size; i++)
174  {
175  auto& p = points[i];
176  auto& pB = pBuffer[i];
177  p.x = pB.point.x;
178  p.y = pB.point.y;
179  p.z = pB.point.z;
180  p.r = pB.color.r;
181  p.g = pB.color.g;
182  p.b = pB.color.b;
183  p.a = pB.color.a;
184  }
185 
186  }
187  else if (pointCloudFormat->type == visionx::eColoredLabeledPoints)
188  {
189 
190  pcl::PointCloud<pcl::PointXYZRGBL>::Ptr temp(new pcl::PointCloud<pcl::PointXYZRGBL>());
191  convertToPCL(source, pointCloudFormat, temp);
192  pcl::copyPointCloud(*temp, *targetPtr);
193  }
194  }
195 
196  void convertFromPCL(pcl::PointCloud<pcl::PointXYZL>::Ptr sourcePtr, void** target)
197  {
198  const unsigned int Area = sourcePtr->width * sourcePtr->height;
199  visionx::LabeledPoint3D* pBuffer = new visionx::LabeledPoint3D[Area];
200 
201  for (size_t i = 0; i < sourcePtr->points.size(); i++)
202  {
203  pBuffer[i].point.x = sourcePtr->points[i].x;
204  pBuffer[i].point.y = sourcePtr->points[i].y;
205  pBuffer[i].point.z = sourcePtr->points[i].z;
206  pBuffer[i].label = static_cast<int>(sourcePtr->points[i].label);
207  }
208 
209  memcpy(target[0], pBuffer, Area * sizeof(visionx::LabeledPoint3D));
210  delete[] pBuffer;
211  }
212 
213 
214  void convertToPCL(void** source, visionx::MetaPointCloudFormatPtr pointCloudFormat, pcl::PointCloud<pcl::PointXYZL>::Ptr targetPtr)
215  {
216  targetPtr->width = static_cast<uint32_t>(pointCloudFormat->width);
217  targetPtr->height = static_cast<uint32_t>(pointCloudFormat->height);
218  targetPtr->points.resize(pointCloudFormat->size / getBytesPerPoint(pointCloudFormat->type));
219  visionx::LabeledPoint3D* pBuffer = reinterpret_cast<visionx::LabeledPoint3D*>(*source);
220 
221  for (size_t i = 0; i < targetPtr->points.size(); i++)
222  {
223  targetPtr->points[i].x = pBuffer[i].point.x;
224  targetPtr->points[i].y = pBuffer[i].point.y;
225  targetPtr->points[i].z = pBuffer[i].point.z;
226  targetPtr->points[i].label = static_cast<uint32_t>(pBuffer[i].label);
227  }
228  }
229 
230 
231 
232  void convertFromPCL(pcl::PointCloud<pcl::PointXYZI>::Ptr sourcePtr, void** target)
233  {
234  const unsigned int Area = sourcePtr->width * sourcePtr->height;
235  visionx::IntensityPoint3D* pBuffer = new visionx::IntensityPoint3D[Area];
236 
237  for (size_t i = 0; i < sourcePtr->points.size(); i++)
238  {
239  pBuffer[i].point.x = sourcePtr->points[i].x;
240  pBuffer[i].point.y = sourcePtr->points[i].y;
241  pBuffer[i].point.z = sourcePtr->points[i].z;
242  pBuffer[i].intensity = sourcePtr->points[i].intensity;
243  }
244 
245  memcpy(target[0], pBuffer, Area * sizeof(visionx::IntensityPoint3D));
246  delete[] pBuffer;
247  }
248 
249 
250  void convertToPCL(void** source, visionx::MetaPointCloudFormatPtr pointCloudFormat, pcl::PointCloud<pcl::PointXYZI>::Ptr targetPtr)
251  {
252  targetPtr->width = static_cast<uint32_t>(pointCloudFormat->width);
253  targetPtr->height = static_cast<uint32_t>(pointCloudFormat->height);
254  targetPtr->points.resize(pointCloudFormat->size / getBytesPerPoint(pointCloudFormat->type));
255  visionx::IntensityPoint3D* pBuffer = reinterpret_cast<visionx::IntensityPoint3D*>(*source);
256 
257  for (size_t i = 0; i < targetPtr->points.size(); i++)
258  {
259  targetPtr->points[i].x = pBuffer[i].point.x;
260  targetPtr->points[i].y = pBuffer[i].point.y;
261  targetPtr->points[i].z = pBuffer[i].point.z;
262  targetPtr->points[i].intensity = pBuffer[i].intensity;
263  }
264  }
265 
266 
267 
268  void convertFromPCL(pcl::PointCloud<pcl::PointXYZ>::Ptr sourcePtr, void** target)
269  {
270  const unsigned int Area = sourcePtr->width * sourcePtr->height;
271  visionx::Point3D* pBuffer = new visionx::Point3D[Area];
272 
273  for (size_t i = 0; i < sourcePtr->points.size(); i++)
274  {
275  pBuffer[i].x = sourcePtr->points[i].x;
276  pBuffer[i].y = sourcePtr->points[i].y;
277  pBuffer[i].z = sourcePtr->points[i].z;
278  }
279 
280  memcpy(target[0], pBuffer, Area * sizeof(visionx::Point3D));
281  delete[] pBuffer;
282 
283  }
284 
285  void convertToPCL(void** source, visionx::MetaPointCloudFormatPtr pointCloudFormat, pcl::PointCloud<pcl::PointXYZ>::Ptr targetPtr)
286  {
287  targetPtr->width = static_cast<uint32_t>(pointCloudFormat->width);
288  targetPtr->height = static_cast<uint32_t>(pointCloudFormat->height);
289  targetPtr->points.resize(pointCloudFormat->size / getBytesPerPoint(pointCloudFormat->type));
290  visionx::Point3D* pBuffer = reinterpret_cast<visionx::Point3D*>(*source);
291 
292  for (size_t i = 0; i < targetPtr->points.size(); i++)
293  {
294  targetPtr->points[i].x = pBuffer[i].x;
295  targetPtr->points[i].y = pBuffer[i].y;
296  targetPtr->points[i].z = pBuffer[i].z;
297  }
298  }
299 
300  void convertToPCL(const ColoredPointCloud& source, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr targetPtr)
301  {
302  targetPtr->points.resize(targetPtr->size());
303 
304  for (size_t i = 0; i < targetPtr->points.size(); i++)
305  {
306  targetPtr->points[i].x = source.at(i).point.x;
307  targetPtr->points[i].y = source.at(i).point.y;
308  targetPtr->points[i].z = source.at(i).point.z;
309  targetPtr->points[i].r = source.at(i).color.r;
310  targetPtr->points[i].g = source.at(i).color.g;
311  targetPtr->points[i].b = source.at(i).color.b;
312  targetPtr->points[i].a = source.at(i).color.a;
313  }
314  }
315 
316  void convertFromPCL(const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr sourcePtr, ColoredPointCloud& target)
317  {
318  target.resize(sourcePtr->size());
319 
320  for (size_t i = 0; i < target.size(); i++)
321  {
322  target.at(i).point.x = sourcePtr->points[i].x;
323  target.at(i).point.y = sourcePtr->points[i].y;
324  target.at(i).point.z = sourcePtr->points[i].z;
325  target.at(i).color.r = sourcePtr->points[i].r;
326  target.at(i).color.g = sourcePtr->points[i].g;
327  target.at(i).color.b = sourcePtr->points[i].b;
328  target.at(i).color.a = sourcePtr->points[i].a;
329  }
330  }
331 
332 
333  void convertFromPCL(pcl::PointCloud<pcl::PointXYZRGBL>::Ptr sourcePtr, void** target)
334  {
335  const unsigned int Area = sourcePtr->width * sourcePtr->height;
336  visionx::ColoredLabeledPoint3D* pBuffer = new visionx::ColoredLabeledPoint3D[Area];
337 
338  for (size_t i = 0; i < sourcePtr->points.size(); i++)
339  {
340  pBuffer[i].point.x = sourcePtr->points[i].x;
341  pBuffer[i].point.y = sourcePtr->points[i].y;
342  pBuffer[i].point.z = sourcePtr->points[i].z;
343  pBuffer[i].color.r = sourcePtr->points[i].r;
344  pBuffer[i].color.g = sourcePtr->points[i].g;
345  pBuffer[i].color.b = sourcePtr->points[i].b;
346  pBuffer[i].color.a = sourcePtr->points[i].a;
347  pBuffer[i].label = static_cast<int>(sourcePtr->points[i].label);
348  }
349 
350  memcpy(target[0], pBuffer, Area * sizeof(visionx::ColoredLabeledPoint3D));
351  delete[] pBuffer;
352  }
353 
354  void convertFromPCL(const pcl::PointCloud<pcl::PointXYZRGBL>& source,
355  ColoredLabeledPointCloud& target)
356  {
357  target.resize(source.size());
358 
359  for (std::size_t i = 0; i < source.size(); ++i)
360  {
361  const pcl::PointXYZRGBL& p = source.points[i];
362  target[i].point = visionx::Point3D { p.x, p.y, p.z };
363  target[i].color = visionx::RGBA { p.r, p.g, p.b, p.a };
364  target[i].label = static_cast<int>(p.label);
365  }
366  }
367 
368  void convertToPCL(const ColoredLabeledPointCloud& source,
369  pcl::PointCloud<pcl::PointXYZRGBL>& target)
370  {
371  target.resize(source.size());
372 
373  for (std::size_t i = 0; i < source.size(); ++i)
374  {
375  const visionx::ColoredLabeledPoint3D& p = source[i];
376 
377  target[i].x = p.point.x;
378  target[i].y = p.point.y;
379  target[i].z = p.point.z;
380  target[i].r = p.color.r;
381  target[i].g = p.color.g;
382  target[i].b = p.color.b;
383  target[i].a = p.color.a;
384  target[i].label = static_cast<uint32_t>(p.label);
385  }
386  }
387 
388 
389  void convertToPCL(void** source, visionx::MetaPointCloudFormatPtr pointCloudFormat, pcl::PointCloud<pcl::PointXYZRGBL>::Ptr targetPtr)
390  {
391 
392  if (pointCloudFormat->type == visionx::eColoredLabeledPoints)
393  {
394  targetPtr->width = static_cast<uint32_t>(pointCloudFormat->width);
395  targetPtr->height = static_cast<uint32_t>(pointCloudFormat->height);
396  targetPtr->points.resize(pointCloudFormat->size / getBytesPerPoint(pointCloudFormat->type));
397 
398  visionx::ColoredLabeledPoint3D* pBuffer = reinterpret_cast<visionx::ColoredLabeledPoint3D*>(*source);
399 
400  for (size_t i = 0; i < targetPtr->points.size(); i++)
401  {
402  targetPtr->points[i].x = pBuffer[i].point.x;
403  targetPtr->points[i].y = pBuffer[i].point.y;
404  targetPtr->points[i].z = pBuffer[i].point.z;
405  targetPtr->points[i].r = pBuffer[i].color.r;
406  targetPtr->points[i].g = pBuffer[i].color.g;
407  targetPtr->points[i].b = pBuffer[i].color.b;
408  targetPtr->points[i].a = pBuffer[i].color.a;
409  targetPtr->points[i].label = static_cast<uint32_t>(pBuffer[i].label);
410  }
411  }
412  else if (pointCloudFormat->type == visionx::eColoredPoints)
413  {
414 
415  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr temp(new pcl::PointCloud<pcl::PointXYZRGBA>());
416  convertToPCL(source, pointCloudFormat, temp);
417  pcl::copyPointCloud(*temp, *targetPtr);
418 
419  }
420  else if (pointCloudFormat->type == visionx::eLabeledPoints)
421  {
422 
423  pcl::PointCloud<pcl::PointXYZL>::Ptr temp(new pcl::PointCloud<pcl::PointXYZL>());
424  convertToPCL(source, pointCloudFormat, temp);
425  pcl::copyPointCloud(*temp, *targetPtr);
426  }
427  }
428 
429  void convertFromPCL(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& sourcePtr, void** target)
430  {
431  const unsigned int area = sourcePtr->width * sourcePtr->height;
432  std::vector<visionx::ColoredOrientedPoint3D> buffer(area);
433 
434  for (size_t i = 0; i < sourcePtr->points.size(); i++)
435  {
436  buffer[i].point.x = sourcePtr->points[i].x;
437  buffer[i].point.y = sourcePtr->points[i].y;
438  buffer[i].point.z = sourcePtr->points[i].z;
439  buffer[i].color.r = sourcePtr->points[i].r;
440  buffer[i].color.g = sourcePtr->points[i].g;
441  buffer[i].color.b = sourcePtr->points[i].b;
442  buffer[i].normal.nx = sourcePtr->points[i].normal_x;
443  buffer[i].normal.ny = sourcePtr->points[i].normal_y;
444  buffer[i].normal.nz = sourcePtr->points[i].normal_z;
445  }
446 
447  copy(buffer.begin(), buffer.end(), static_cast<visionx::ColoredOrientedPoint3D*>(target[0]));
448  }
449 
450  void convertToPCL(void** source, visionx::MetaPointCloudFormatPtr pointCloudFormat, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr targetPtr)
451  {
452  targetPtr->width = static_cast<uint32_t>(pointCloudFormat->width);
453  targetPtr->height = static_cast<uint32_t>(pointCloudFormat->height);
454  targetPtr->points.resize(pointCloudFormat->size / getBytesPerPoint(pointCloudFormat->type));
455  visionx::ColoredOrientedPoint3D* pBuffer = static_cast<visionx::ColoredOrientedPoint3D*>(*source);
456 
457  for (size_t i = 0; i < targetPtr->points.size(); i++)
458  {
459  targetPtr->points[i].x = pBuffer[i].point.x;
460  targetPtr->points[i].y = pBuffer[i].point.y;
461  targetPtr->points[i].z = pBuffer[i].point.z;
462  targetPtr->points[i].r = pBuffer[i].color.r;
463  targetPtr->points[i].g = pBuffer[i].color.g;
464  targetPtr->points[i].b = pBuffer[i].color.b;
465  targetPtr->points[i].normal_x = pBuffer[i].normal.nx;
466  targetPtr->points[i].normal_y = pBuffer[i].normal.ny;
467  targetPtr->points[i].normal_z = pBuffer[i].normal.nz;
468  }
469  }
470 
471  void convertFromPCL(pcl::PointCloud<pcl::PointXYZRGB>::Ptr sourcePtr, void** target)
472  {
473  ARMARX_CHECK_EXPRESSION(target);
474  ARMARX_CHECK_EXPRESSION(sourcePtr);
475  visionx::ColoredPoint3D* pBuffer = reinterpret_cast<ColoredPoint3D*>(target);
476 
477  size_t size = sourcePtr->points.size();
478  auto& points = sourcePtr->points;
479  for (size_t i = 0; i < size; i++)
480  {
481  auto& p = points[i];
482  auto& pB = pBuffer[i];
483  pB.point.x = p.x;
484  pB.point.y = p.y;
485  pB.point.z = p.z;
486  pB.color.r = p.r;
487  pB.color.g = p.g;
488  pB.color.b = p.b;
489  pB.color.a = 255;
490  }
491 
492 
493  }
494 
495 
496  void convertToPCL(void** source, visionx::MetaPointCloudFormatPtr pointCloudFormat, pcl::PointCloud<pcl::PointXYZRGB>::Ptr targetPtr)
497  {
498  if (pointCloudFormat->type == visionx::eColoredPoints)
499  {
500 
501 
502  targetPtr->width = pointCloudFormat->width;
503  targetPtr->height = pointCloudFormat->height;
504  targetPtr->points.resize(pointCloudFormat->size / getBytesPerPoint(pointCloudFormat->type));
505  visionx::ColoredPoint3D* pBuffer = reinterpret_cast<visionx::ColoredPoint3D*>(*source);
506  size_t size = targetPtr->points.size();
507  auto& points = targetPtr->points;
508  for (size_t i = 0; i < size; i++)
509  {
510  auto& p = points[i];
511  auto& pB = pBuffer[i];
512  p.x = pB.point.x;
513  p.y = pB.point.y;
514  p.z = pB.point.z;
515  p.r = pB.color.r;
516  p.g = pB.color.g;
517  p.b = pB.color.b;
518  }
519 
520  }
521  else if (pointCloudFormat->type == visionx::eColoredLabeledPoints)
522  {
523 
524  pcl::PointCloud<pcl::PointXYZRGBL>::Ptr temp(new pcl::PointCloud<pcl::PointXYZRGBL>());
525  convertToPCL(source, pointCloudFormat, temp);
526  pcl::copyPointCloud(*temp, *targetPtr);
527  }
528  }
529 
530  void convertFromPCLToDebugDrawer(const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& sourcePtr, armarx::DebugDrawer24BitColoredPointCloud& target, PCLToDebugDrawerConversionMode mode, float pointSize)
531  {
532  target.pointSize = pointSize;
533 
534  std::size_t numPoints = sourcePtr->width * sourcePtr->height;
535  target.points.reserve(numPoints);
536 
537  armarx::DebugDrawer24BitColoredPointCloudElement e;
538  for (const auto& p : sourcePtr->points)
539  {
540  if (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z))
541  {
542  e.x = p.x;
543  e.y = p.y;
544  e.z = p.z;
545 
546  if (mode == eConvertAsColors)
547  {
548  e.color.r = p.r;
549  e.color.g = p.g;
550  e.color.b = p.b;
551  }
552  else
553  {
554  // Simple rainbow coloring
555  float value = (static_cast<int>(e.z) / 10) % 256;
556 
557  e.color.r = static_cast<Ice::Byte>((value > 128) ? (value - 128) * 2 : 0);
558  e.color.g = static_cast<Ice::Byte>((value < 128) ? 2 * value : 255 - ((value - 128) * 2));
559  e.color.b = static_cast<Ice::Byte>((value < 128) ? 255 - (2 * value) : 0);
560  }
561 
562  target.points.push_back(e);
563  }
564  }
565  }
566 
567  void convertFromPCLToDebugDrawer(const pcl::PointCloud<pcl::PointXYZL>::Ptr& sourcePtr, armarx::DebugDrawer24BitColoredPointCloud& target, PCLToDebugDrawerConversionMode mode, float pointSize)
568  {
569  target.pointSize = pointSize;
570 
571  std::size_t numPoints = sourcePtr->width * sourcePtr->height;
572  target.points.reserve(numPoints);
573  std::map<uint32_t, pcl::RGB> colorMap;
574 
575  armarx::DebugDrawer24BitColoredPointCloudElement e;
576  for (const auto& p : sourcePtr->points)
577  {
578  if (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z))
579  {
580  e.x = p.x;
581  e.y = p.y;
582  e.z = p.z;
583 
584  if (mode == eConvertAsLabels)
585  {
586  if (!colorMap.count(p.label))
587  {
588  colorMap[p.label] = pcl::GlasbeyLUT::at(p.label % pcl::GlasbeyLUT::size());
589  }
590  const auto c = colorMap[p.label];
591 
592  e.color.r = c.r;
593  e.color.g = c.g;
594  e.color.b = c.b;
595  }
596  else
597  {
598  // Simple rainbow coloring
599  float value = (static_cast<int>(e.z) / 10) % 256;
600 
601  e.color.r = static_cast<Ice::Byte>((value > 128) ? (value - 128) * 2 : 0);
602  e.color.g = static_cast<Ice::Byte>((value < 128) ? 2 * value : 255 - ((value - 128) * 2));
603  e.color.b = static_cast<Ice::Byte>((value < 128) ? 255 - (2 * value) : 0);
604  }
605 
606  target.points.push_back(e);
607  }
608  }
609  }
610 
611  void convertFromPCLToDebugDrawer(
612  const pcl::PointCloud<pcl::PointXYZ>::Ptr& sourcePtr,
613  armarx::DebugDrawer24BitColoredPointCloud& target,
614  PCLToDebugDrawerConversionMode mode, float pointSize)
615  {
616  (void) mode; // unused
617 
618  target.pointSize = pointSize;
619 
620  std::size_t numPoints = sourcePtr->width * sourcePtr->height;
621  target.points.reserve(numPoints);
622 
623  armarx::DebugDrawer24BitColoredPointCloudElement e;
624  for (const auto& p : sourcePtr->points)
625  {
626  if (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z))
627  {
628  e.x = p.x;
629  e.y = p.y;
630  e.z = p.z;
631 
632  // Simple rainbow coloring
633  float value = (static_cast<int>(e.z) / 10) % 256;
634 
635  e.color.r = static_cast<Ice::Byte>((value > 128) ? (value - 128) * 2 : 0);
636  e.color.g = static_cast<Ice::Byte>((value < 128) ? 2 * value : 255 - ((value - 128) * 2));
637  e.color.b = static_cast<Ice::Byte>((value < 128) ? 255 - (2 * value) : 0);
638 
639  target.points.push_back(e);
640  }
641  }
642  }
643 
644  void convertFromPCLToDebugDrawer(const pcl::PointCloud<pcl::PointXYZRGBL>::Ptr& sourcePtr, armarx::DebugDrawer24BitColoredPointCloud& target, PCLToDebugDrawerConversionMode mode, float pointSize)
645  {
646  target.pointSize = pointSize;
647 
648  std::size_t numPoints = sourcePtr->width * sourcePtr->height;
649  target.points.reserve(numPoints);
650  std::map<uint32_t, pcl::RGB> colorMap;
651 
652  armarx::DebugDrawer24BitColoredPointCloudElement e;
653  for (const auto& p : sourcePtr->points)
654  {
655  if (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z))
656  {
657  e.x = p.x;
658  e.y = p.y;
659  e.z = p.z;
660 
661  if (mode == eConvertAsColors)
662  {
663  e.color.r = p.r;
664  e.color.g = p.g;
665  e.color.b = p.b;
666  }
667  else if (mode == eConvertAsLabels)
668  {
669  if (!colorMap.count(p.label))
670  {
671  colorMap[p.label] = pcl::GlasbeyLUT::at(p.label % pcl::GlasbeyLUT::size());
672  }
673  const auto c = colorMap[p.label];
674 
675  e.color.r = c.r;
676  e.color.g = c.g;
677  e.color.b = c.b;
678  }
679  else
680  {
681  // Simple rainbow coloring
682  float value = (static_cast<int>(e.z) / 10) % 256;
683 
684  e.color.r = static_cast<Ice::Byte>((value > 128) ? (value - 128) * 2 : 0);
685  e.color.g = static_cast<Ice::Byte>((value < 128) ? 2 * value : 255 - ((value - 128) * 2));
686  e.color.b = static_cast<Ice::Byte>((value < 128) ? 255 - (2 * value) : 0);
687  }
688 
689  target.points.push_back(e);
690  }
691  }
692  }
693 
694 
695  // pcl::PointXYZ <-> visionx::Point3D
696  template <>
697  void convertPointFromPCL(const pcl::PointXYZ& source, visionx::Point3D& target)
698  {
699  detail::convertXYZ(source, target);
700  }
701  template <>
702  void convertPointToPCL(const visionx::Point3D& source, pcl::PointXYZ& target)
703  {
704  detail::convertXYZ(source, target);
705  }
706 
707  // pcl::PointXYZRGBA <-> visionx::ColoredPoint3D
708  template <>
709  void convertPointFromPCL(const pcl::PointXYZRGBA& source, visionx::ColoredPoint3D& target)
710  {
711  detail::convertXYZ(source, target.point);
712  detail::convertRGBA(source, target.color);
713  }
714  template <>
715  void convertPointToPCL(const visionx::ColoredPoint3D& source, pcl::PointXYZRGBA& target)
716  {
717  detail::convertXYZ(source.point, target);
718  detail::convertRGBA(source.color, target);
719  }
720 
721  // pcl::PointXYZL <-> visionx::LabeledPoint3D
722  template <>
723  void convertPointFromPCL(const pcl::PointXYZL& source, visionx::LabeledPoint3D& target)
724  {
725  detail::convertXYZ(source, target.point);
726  detail::convertLabel(source, target);
727  }
728  template <>
729  void convertPointToPCL(const visionx::LabeledPoint3D& source, pcl::PointXYZL& target)
730  {
731  detail::convertXYZ(source.point, target);
732  detail::convertLabel(source, target);
733  }
734 
735  // pcl::PointXYZRGBL <-> visionx::ColoredLabeledPoint3D
736  template <>
737  void convertPointFromPCL(const pcl::PointXYZRGBL& source, visionx::ColoredLabeledPoint3D& target)
738  {
739  detail::convertXYZ(source, target.point);
740  detail::convertRGBA(source, target.color);
741  detail::convertLabel(source, target);
742  }
743 
744  template <>
745  void convertPointToPCL(const visionx::ColoredLabeledPoint3D& source, pcl::PointXYZRGBL& target)
746  {
747  detail::convertXYZ(source.point, target);
748  detail::convertRGBA(source.color, target);
749  detail::convertLabel(source, target);
750  }
751 
752  static std::vector<std::string> makePointContentTypeNames()
753  {
754  std::vector<std::string> names(7);
755  names.at(ePoints) = "ePoints";
756  names.at(eColoredPoints) = "eColoredPoints";
757  names.at(eOrientedPoints) = "eOrientedPoints";
758  names.at(eColoredOrientedPoints) = "eColoredOrientedPoints";
759  names.at(eLabeledPoints) = "eLabeledPoints";
760  names.at(eColoredLabeledPoints) = "eColoredLabeledPoints";
761  names.at(eIntensity) = "eIntensity";
762  return names;
763  }
764 
765  std::string toString(PointContentType pointContent)
766  {
767  static const std::vector<std::string> names = makePointContentTypeNames();
768  return names.at(pointContent);
769  }
770 
771 }
772 
Area
ldouble Area(const point2d &a, const point2d &b, const point2d &c)
Definition: gdiam.cpp:1564
visionx::tools::PCLToDebugDrawerConversionMode
PCLToDebugDrawerConversionMode
Definition: PointCloudConversions.h:102
visionx::tools
Definition: PCLUtilities.cpp:4
visionx::tools::detail::convertXYZ
void convertXYZ(const SourceT &source, TargetT &target)
Convert x, y, z attributes. Use with visionx::Point3D and PCL type with XYZ.
Definition: PointCloudConversions.h:181
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:688
visionx::tools::eConvertAsColors
@ eConvertAsColors
Definition: PointCloudConversions.h:105
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
visionx::tools::isLabeled
bool isLabeled(PointContentType type)
Definition: PointCloudConversions.cpp:108
visionx::tools::detail::convertRGBA
void convertRGBA(const SourceT &source, TargetT &target)
Convert r, g, b attributes. Use with visionx::RGBA and PCL type with RGBA.
Definition: PointCloudConversions.h:190
visionx::tools::detail::convertLabel
void convertLabel(const SourceT &source, TargetT &target)
Convert label attribute. Use with visionx point type and PCL type with labels.
Definition: PointCloudConversions.h:200
visionx::tools::convertFromPCL
void convertFromPCL(pcl::PointCloud< pcl::PointXYZRGBA >::Ptr sourcePtr, void **target)
Definition: PointCloudConversions.cpp:121
std::isfinite
bool isfinite(const std::vector< T, Ts... > &v)
Definition: algorithm.h:327
visionx::tools::convertPointFromPCL
void convertPointFromPCL(const pcl::PointXYZ &source, visionx::Point3D &target)
Definition: PointCloudConversions.cpp:697
visionx::tools::convertPointToPCL
void convertPointToPCL(const visionx::Point3D &source, pcl::PointXYZ &target)
Definition: PointCloudConversions.cpp:702
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
visionx::tools::isColored
bool isColored(PointContentType type)
Definition: PointCloudConversions.cpp:95
copy
Use of this software is granted under one of the following two to be chosen freely by the user Boost Software License Version Marcin Kalicinski Permission is hereby free of to any person or organization obtaining a copy of the software and accompanying documentation covered by this and transmit the and to prepare derivative works of the and to permit third parties to whom the Software is furnished to do all subject to the including the above license this restriction and the following must be included in all copies of the in whole or in and all derivative works of the unless such copies or derivative works are solely in the form of machine executable object code generated by a source language processor THE SOFTWARE IS PROVIDED AS WITHOUT WARRANTY OF ANY EXPRESS OR INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF FITNESS FOR A PARTICULAR TITLE AND NON INFRINGEMENT IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE FOR ANY DAMAGES OR OTHER WHETHER IN TORT OR ARISING OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE The MIT Marcin Kalicinski Permission is hereby free of to any person obtaining a copy of this software and associated documentation to deal in the Software without including without limitation the rights to copy
Definition: license.txt:39
visionx::tools::convertFromPCLToDebugDrawer
void convertFromPCLToDebugDrawer(const pcl::PointCloud< pcl::PointXYZRGBA >::Ptr &sourcePtr, armarx::DebugDrawer24BitColoredPointCloud &target, PCLToDebugDrawerConversionMode mode, float pointSize)
Definition: PointCloudConversions.cpp:530
visionx::tools::toString
std::string toString(PointContentType pointContent)
Definition: PointCloudConversions.cpp:765
visionx::tools::eConvertAsLabels
@ eConvertAsLabels
Definition: PointCloudConversions.h:106
boost::source
Vertex source(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:681
visionx::tools::convertToPCL
void convertToPCL(void **source, visionx::MetaPointCloudFormatPtr pointCloudFormat, pcl::PointCloud< pcl::PointXYZRGBA >::Ptr targetPtr)
Definition: PointCloudConversions.cpp:146
ExpressionException.h
PointCloudConversions.h
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
armarx::viz::data::ElementFlags::names
const simox::meta::IntEnumNames names
Definition: json_elements.cpp:14
visionx::tools::getBytesPerPoint
size_t getBytesPerPoint(visionx::PointContentType pointContent)
Definition: PointCloudConversions.cpp:64