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 
27 #include <pcl/common/colors.h>
28 #include <pcl/io/pcd_io.h>
29 
30 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
31 
32 namespace visionx::tools
33 {
34 
35  template <>
36  visionx::PointContentType
37  getPointContentType<pcl::PointXYZ>()
38  {
39  return visionx::ePoints;
40  }
41 
42  template <>
43  visionx::PointContentType
44  getPointContentType<pcl::PointXYZL>()
45  {
46  return visionx::eLabeledPoints;
47  }
48 
49  template <>
50  visionx::PointContentType
51  getPointContentType<pcl::PointXYZI>()
52  {
53  return visionx::eIntensity;
54  }
55 
56  template <>
57  PointContentType
58  getPointContentType<pcl::PointNormal>()
59  {
60  return visionx::eOrientedPoints;
61  }
62 
63  template <>
64  visionx::PointContentType
65  getPointContentType<pcl::PointXYZRGBA>()
66  {
67  return visionx::eColoredPoints;
68  }
69 
70  template <>
71  visionx::PointContentType
72  getPointContentType<pcl::PointXYZRGBL>()
73  {
74  return visionx::eColoredLabeledPoints;
75  }
76 
77  template <>
78  visionx::PointContentType
79  getPointContentType<pcl::PointXYZRGBNormal>()
80  {
81  return visionx::eColoredOrientedPoints;
82  }
83 
84  size_t
85  getBytesPerPoint(visionx::PointContentType pointContent)
86  {
87  switch (pointContent)
88  {
89  case visionx::ePoints:
90  return sizeof(visionx::Point3D);
91 
92  case visionx::eColoredPoints:
93  return sizeof(visionx::ColoredPoint3D);
94 
95  case visionx::eOrientedPoints:
96  return sizeof(visionx::OrientedPoint3D);
97 
98  case visionx::eColoredOrientedPoints:
99  return sizeof(visionx::ColoredOrientedPoint3D);
100 
101  case visionx::eLabeledPoints:
102  return sizeof(visionx::LabeledPoint3D);
103 
104  case visionx::eColoredLabeledPoints:
105  return sizeof(visionx::ColoredLabeledPoint3D);
106 
107  case visionx::eIntensity:
108  return sizeof(visionx::IntensityPoint3D);
109 
110  default:
111  return 0;
112  //ARMARX_FATAL_S << "invalid point cloud content " << pointContent;
113  }
114  }
115 
116  bool
117  isColored(PointContentType type)
118  {
119  switch (type)
120  {
121  case visionx::eColoredPoints:
122  case visionx::eColoredOrientedPoints:
123  case visionx::eColoredLabeledPoints:
124  return true;
125  default:
126  return false;
127  }
128  }
129 
130  bool
131  isLabeled(PointContentType type)
132  {
133  switch (type)
134  {
135  case visionx::eLabeledPoints:
136  case visionx::eColoredLabeledPoints:
137  return true;
138  default:
139  return false;
140  }
141  }
142 
143  void
144  convertFromPCL(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr sourcePtr, void** target)
145  {
146  const unsigned int Area = sourcePtr->width * sourcePtr->height;
147  visionx::ColoredPoint3D* pBuffer = new visionx::ColoredPoint3D[Area];
148 
149  size_t size = sourcePtr->points.size();
150  auto& points = sourcePtr->points;
151  for (size_t i = 0; i < size; i++)
152  {
153  auto& p = points[i];
154  auto& pB = pBuffer[i];
155  pB.point.x = p.x;
156  pB.point.y = p.y;
157  pB.point.z = p.z;
158  pB.color.r = p.r;
159  pB.color.g = p.g;
160  pB.color.b = p.b;
161  pB.color.a = p.a;
162  }
163 
164  memcpy(target[0], pBuffer, Area * sizeof(visionx::ColoredPoint3D));
165  delete[] pBuffer;
166  }
167 
168  void
169  convertToPCL(void** source,
170  visionx::MetaPointCloudFormatPtr pointCloudFormat,
171  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr targetPtr)
172  {
173  if (pointCloudFormat->type == visionx::ePoints)
174  {
175  targetPtr->width = static_cast<uint32_t>(pointCloudFormat->width);
176  targetPtr->height = static_cast<uint32_t>(pointCloudFormat->height);
177  targetPtr->points.resize(pointCloudFormat->size /
178  getBytesPerPoint(pointCloudFormat->type));
179  visionx::Point3D* pBuffer = reinterpret_cast<visionx::Point3D*>(*source);
180  size_t size = targetPtr->points.size();
181  auto& points = targetPtr->points;
182  for (size_t i = 0; i < size; i++)
183  {
184  auto& p = points[i];
185  auto& pB = pBuffer[i];
186  p.x = pB.x;
187  p.y = pB.y;
188  p.z = pB.z;
189  }
190  }
191  else if (pointCloudFormat->type == visionx::eColoredPoints)
192  {
193  targetPtr->width = static_cast<uint32_t>(pointCloudFormat->width);
194  targetPtr->height = static_cast<uint32_t>(pointCloudFormat->height);
195  targetPtr->points.resize(pointCloudFormat->size /
196  getBytesPerPoint(pointCloudFormat->type));
197  visionx::ColoredPoint3D* pBuffer = reinterpret_cast<visionx::ColoredPoint3D*>(*source);
198  size_t size = targetPtr->points.size();
199  auto& points = targetPtr->points;
200  for (size_t i = 0; i < size; i++)
201  {
202  auto& p = points[i];
203  auto& pB = pBuffer[i];
204  p.x = pB.point.x;
205  p.y = pB.point.y;
206  p.z = pB.point.z;
207  p.r = pB.color.r;
208  p.g = pB.color.g;
209  p.b = pB.color.b;
210  p.a = pB.color.a;
211  }
212  }
213  else if (pointCloudFormat->type == visionx::eColoredLabeledPoints)
214  {
215 
216  pcl::PointCloud<pcl::PointXYZRGBL>::Ptr temp(new pcl::PointCloud<pcl::PointXYZRGBL>());
217  convertToPCL(source, pointCloudFormat, temp);
218  pcl::copyPointCloud(*temp, *targetPtr);
219  }
220  }
221 
222  void
223  convertFromPCL(pcl::PointCloud<pcl::PointXYZL>::Ptr sourcePtr, void** target)
224  {
225  const unsigned int Area = sourcePtr->width * sourcePtr->height;
226  visionx::LabeledPoint3D* pBuffer = new visionx::LabeledPoint3D[Area];
227 
228  for (size_t i = 0; i < sourcePtr->points.size(); i++)
229  {
230  pBuffer[i].point.x = sourcePtr->points[i].x;
231  pBuffer[i].point.y = sourcePtr->points[i].y;
232  pBuffer[i].point.z = sourcePtr->points[i].z;
233  pBuffer[i].label = static_cast<int>(sourcePtr->points[i].label);
234  }
235 
236  memcpy(target[0], pBuffer, Area * sizeof(visionx::LabeledPoint3D));
237  delete[] pBuffer;
238  }
239 
240  void
241  convertToPCL(void** source,
242  visionx::MetaPointCloudFormatPtr pointCloudFormat,
243  pcl::PointCloud<pcl::PointXYZL>::Ptr targetPtr)
244  {
245  targetPtr->width = static_cast<uint32_t>(pointCloudFormat->width);
246  targetPtr->height = static_cast<uint32_t>(pointCloudFormat->height);
247  targetPtr->points.resize(pointCloudFormat->size / getBytesPerPoint(pointCloudFormat->type));
248  visionx::LabeledPoint3D* pBuffer = reinterpret_cast<visionx::LabeledPoint3D*>(*source);
249 
250  for (size_t i = 0; i < targetPtr->points.size(); i++)
251  {
252  targetPtr->points[i].x = pBuffer[i].point.x;
253  targetPtr->points[i].y = pBuffer[i].point.y;
254  targetPtr->points[i].z = pBuffer[i].point.z;
255  targetPtr->points[i].label = static_cast<uint32_t>(pBuffer[i].label);
256  }
257  }
258 
259  void
260  convertFromPCL(pcl::PointCloud<pcl::PointXYZI>::Ptr sourcePtr, void** target)
261  {
262  const unsigned int Area = sourcePtr->width * sourcePtr->height;
263  visionx::IntensityPoint3D* pBuffer = new visionx::IntensityPoint3D[Area];
264 
265  for (size_t i = 0; i < sourcePtr->points.size(); i++)
266  {
267  pBuffer[i].point.x = sourcePtr->points[i].x;
268  pBuffer[i].point.y = sourcePtr->points[i].y;
269  pBuffer[i].point.z = sourcePtr->points[i].z;
270  pBuffer[i].intensity = sourcePtr->points[i].intensity;
271  }
272 
273  memcpy(target[0], pBuffer, Area * sizeof(visionx::IntensityPoint3D));
274  delete[] pBuffer;
275  }
276 
277  void
278  convertToPCL(void** source,
279  visionx::MetaPointCloudFormatPtr pointCloudFormat,
280  pcl::PointCloud<pcl::PointXYZI>::Ptr targetPtr)
281  {
282  targetPtr->width = static_cast<uint32_t>(pointCloudFormat->width);
283  targetPtr->height = static_cast<uint32_t>(pointCloudFormat->height);
284  targetPtr->points.resize(pointCloudFormat->size / getBytesPerPoint(pointCloudFormat->type));
285  visionx::IntensityPoint3D* pBuffer = reinterpret_cast<visionx::IntensityPoint3D*>(*source);
286 
287  for (size_t i = 0; i < targetPtr->points.size(); i++)
288  {
289  targetPtr->points[i].x = pBuffer[i].point.x;
290  targetPtr->points[i].y = pBuffer[i].point.y;
291  targetPtr->points[i].z = pBuffer[i].point.z;
292  targetPtr->points[i].intensity = pBuffer[i].intensity;
293  }
294  }
295 
296  void
297  convertFromPCL(pcl::PointCloud<pcl::PointXYZ>::Ptr sourcePtr, void** target)
298  {
299  const unsigned int Area = sourcePtr->width * sourcePtr->height;
300  visionx::Point3D* pBuffer = new visionx::Point3D[Area];
301 
302  for (size_t i = 0; i < sourcePtr->points.size(); i++)
303  {
304  pBuffer[i].x = sourcePtr->points[i].x;
305  pBuffer[i].y = sourcePtr->points[i].y;
306  pBuffer[i].z = sourcePtr->points[i].z;
307  }
308 
309  memcpy(target[0], pBuffer, Area * sizeof(visionx::Point3D));
310  delete[] pBuffer;
311  }
312 
313  void
314  convertToPCL(void** source,
315  visionx::MetaPointCloudFormatPtr pointCloudFormat,
316  pcl::PointCloud<pcl::PointXYZ>::Ptr targetPtr)
317  {
318  targetPtr->width = static_cast<uint32_t>(pointCloudFormat->width);
319  targetPtr->height = static_cast<uint32_t>(pointCloudFormat->height);
320  targetPtr->points.resize(pointCloudFormat->size / getBytesPerPoint(pointCloudFormat->type));
321  visionx::Point3D* pBuffer = reinterpret_cast<visionx::Point3D*>(*source);
322 
323  for (size_t i = 0; i < targetPtr->points.size(); i++)
324  {
325  targetPtr->points[i].x = pBuffer[i].x;
326  targetPtr->points[i].y = pBuffer[i].y;
327  targetPtr->points[i].z = pBuffer[i].z;
328  }
329  }
330 
331  void
332  convertToPCL(const ColoredPointCloud& source, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr targetPtr)
333  {
334  targetPtr->points.resize(targetPtr->size());
335 
336  for (size_t i = 0; i < targetPtr->points.size(); i++)
337  {
338  targetPtr->points[i].x = source.at(i).point.x;
339  targetPtr->points[i].y = source.at(i).point.y;
340  targetPtr->points[i].z = source.at(i).point.z;
341  targetPtr->points[i].r = source.at(i).color.r;
342  targetPtr->points[i].g = source.at(i).color.g;
343  targetPtr->points[i].b = source.at(i).color.b;
344  targetPtr->points[i].a = source.at(i).color.a;
345  }
346  }
347 
348  void
349  convertFromPCL(const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr sourcePtr,
350  ColoredPointCloud& target)
351  {
352  target.resize(sourcePtr->size());
353 
354  for (size_t i = 0; i < target.size(); i++)
355  {
356  target.at(i).point.x = sourcePtr->points[i].x;
357  target.at(i).point.y = sourcePtr->points[i].y;
358  target.at(i).point.z = sourcePtr->points[i].z;
359  target.at(i).color.r = sourcePtr->points[i].r;
360  target.at(i).color.g = sourcePtr->points[i].g;
361  target.at(i).color.b = sourcePtr->points[i].b;
362  target.at(i).color.a = sourcePtr->points[i].a;
363  }
364  }
365 
366  void
367  convertFromPCL(pcl::PointCloud<pcl::PointXYZRGBL>::Ptr sourcePtr, void** target)
368  {
369  const unsigned int Area = sourcePtr->width * sourcePtr->height;
370  visionx::ColoredLabeledPoint3D* pBuffer = new visionx::ColoredLabeledPoint3D[Area];
371 
372  for (size_t i = 0; i < sourcePtr->points.size(); i++)
373  {
374  pBuffer[i].point.x = sourcePtr->points[i].x;
375  pBuffer[i].point.y = sourcePtr->points[i].y;
376  pBuffer[i].point.z = sourcePtr->points[i].z;
377  pBuffer[i].color.r = sourcePtr->points[i].r;
378  pBuffer[i].color.g = sourcePtr->points[i].g;
379  pBuffer[i].color.b = sourcePtr->points[i].b;
380  pBuffer[i].color.a = sourcePtr->points[i].a;
381  pBuffer[i].label = static_cast<int>(sourcePtr->points[i].label);
382  }
383 
384  memcpy(target[0], pBuffer, Area * sizeof(visionx::ColoredLabeledPoint3D));
385  delete[] pBuffer;
386  }
387 
388  void
389  convertFromPCL(const pcl::PointCloud<pcl::PointXYZRGBL>& source,
390  ColoredLabeledPointCloud& target)
391  {
392  target.resize(source.size());
393 
394  for (std::size_t i = 0; i < source.size(); ++i)
395  {
396  const pcl::PointXYZRGBL& p = source.points[i];
397  target[i].point = visionx::Point3D{p.x, p.y, p.z};
398  target[i].color = visionx::RGBA{p.r, p.g, p.b, p.a};
399  target[i].label = static_cast<int>(p.label);
400  }
401  }
402 
403  void
404  convertToPCL(const ColoredLabeledPointCloud& source, pcl::PointCloud<pcl::PointXYZRGBL>& target)
405  {
406  target.resize(source.size());
407 
408  for (std::size_t i = 0; i < source.size(); ++i)
409  {
410  const visionx::ColoredLabeledPoint3D& p = source[i];
411 
412  target[i].x = p.point.x;
413  target[i].y = p.point.y;
414  target[i].z = p.point.z;
415  target[i].r = p.color.r;
416  target[i].g = p.color.g;
417  target[i].b = p.color.b;
418  target[i].a = p.color.a;
419  target[i].label = static_cast<uint32_t>(p.label);
420  }
421  }
422 
423  void
424  convertToPCL(void** source,
425  visionx::MetaPointCloudFormatPtr pointCloudFormat,
426  pcl::PointCloud<pcl::PointXYZRGBL>::Ptr targetPtr)
427  {
428 
429  if (pointCloudFormat->type == visionx::eColoredLabeledPoints)
430  {
431  targetPtr->width = static_cast<uint32_t>(pointCloudFormat->width);
432  targetPtr->height = static_cast<uint32_t>(pointCloudFormat->height);
433  targetPtr->points.resize(pointCloudFormat->size /
434  getBytesPerPoint(pointCloudFormat->type));
435 
436  visionx::ColoredLabeledPoint3D* pBuffer =
437  reinterpret_cast<visionx::ColoredLabeledPoint3D*>(*source);
438 
439  for (size_t i = 0; i < targetPtr->points.size(); i++)
440  {
441  targetPtr->points[i].x = pBuffer[i].point.x;
442  targetPtr->points[i].y = pBuffer[i].point.y;
443  targetPtr->points[i].z = pBuffer[i].point.z;
444  targetPtr->points[i].r = pBuffer[i].color.r;
445  targetPtr->points[i].g = pBuffer[i].color.g;
446  targetPtr->points[i].b = pBuffer[i].color.b;
447  targetPtr->points[i].a = pBuffer[i].color.a;
448  targetPtr->points[i].label = static_cast<uint32_t>(pBuffer[i].label);
449  }
450  }
451  else if (pointCloudFormat->type == visionx::eColoredPoints)
452  {
453 
454  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr temp(new pcl::PointCloud<pcl::PointXYZRGBA>());
455  convertToPCL(source, pointCloudFormat, temp);
456  pcl::copyPointCloud(*temp, *targetPtr);
457  }
458  else if (pointCloudFormat->type == visionx::eLabeledPoints)
459  {
460 
461  pcl::PointCloud<pcl::PointXYZL>::Ptr temp(new pcl::PointCloud<pcl::PointXYZL>());
462  convertToPCL(source, pointCloudFormat, temp);
463  pcl::copyPointCloud(*temp, *targetPtr);
464  }
465  }
466 
467  void
468  convertFromPCL(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& sourcePtr, void** target)
469  {
470  const unsigned int area = sourcePtr->width * sourcePtr->height;
471  std::vector<visionx::ColoredOrientedPoint3D> buffer(area);
472 
473  for (size_t i = 0; i < sourcePtr->points.size(); i++)
474  {
475  buffer[i].point.x = sourcePtr->points[i].x;
476  buffer[i].point.y = sourcePtr->points[i].y;
477  buffer[i].point.z = sourcePtr->points[i].z;
478  buffer[i].color.r = sourcePtr->points[i].r;
479  buffer[i].color.g = sourcePtr->points[i].g;
480  buffer[i].color.b = sourcePtr->points[i].b;
481  buffer[i].normal.nx = sourcePtr->points[i].normal_x;
482  buffer[i].normal.ny = sourcePtr->points[i].normal_y;
483  buffer[i].normal.nz = sourcePtr->points[i].normal_z;
484  }
485 
486  copy(
487  buffer.begin(), buffer.end(), static_cast<visionx::ColoredOrientedPoint3D*>(target[0]));
488  }
489 
490  void
491  convertToPCL(void** source,
492  visionx::MetaPointCloudFormatPtr pointCloudFormat,
493  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr targetPtr)
494  {
495  targetPtr->width = static_cast<uint32_t>(pointCloudFormat->width);
496  targetPtr->height = static_cast<uint32_t>(pointCloudFormat->height);
497  targetPtr->points.resize(pointCloudFormat->size / getBytesPerPoint(pointCloudFormat->type));
498  visionx::ColoredOrientedPoint3D* pBuffer =
499  static_cast<visionx::ColoredOrientedPoint3D*>(*source);
500 
501  for (size_t i = 0; i < targetPtr->points.size(); i++)
502  {
503  targetPtr->points[i].x = pBuffer[i].point.x;
504  targetPtr->points[i].y = pBuffer[i].point.y;
505  targetPtr->points[i].z = pBuffer[i].point.z;
506  targetPtr->points[i].r = pBuffer[i].color.r;
507  targetPtr->points[i].g = pBuffer[i].color.g;
508  targetPtr->points[i].b = pBuffer[i].color.b;
509  targetPtr->points[i].normal_x = pBuffer[i].normal.nx;
510  targetPtr->points[i].normal_y = pBuffer[i].normal.ny;
511  targetPtr->points[i].normal_z = pBuffer[i].normal.nz;
512  }
513  }
514 
515  void
516  convertFromPCL(pcl::PointCloud<pcl::PointXYZRGB>::Ptr sourcePtr, void** target)
517  {
518  ARMARX_CHECK_EXPRESSION(target);
519  ARMARX_CHECK_EXPRESSION(sourcePtr);
520  visionx::ColoredPoint3D* pBuffer = reinterpret_cast<ColoredPoint3D*>(target);
521 
522  size_t size = sourcePtr->points.size();
523  auto& points = sourcePtr->points;
524  for (size_t i = 0; i < size; i++)
525  {
526  auto& p = points[i];
527  auto& pB = pBuffer[i];
528  pB.point.x = p.x;
529  pB.point.y = p.y;
530  pB.point.z = p.z;
531  pB.color.r = p.r;
532  pB.color.g = p.g;
533  pB.color.b = p.b;
534  pB.color.a = 255;
535  }
536  }
537 
538  void
539  convertToPCL(void** source,
540  visionx::MetaPointCloudFormatPtr pointCloudFormat,
541  pcl::PointCloud<pcl::PointXYZRGB>::Ptr targetPtr)
542  {
543  if (pointCloudFormat->type == visionx::eColoredPoints)
544  {
545 
546 
547  targetPtr->width = pointCloudFormat->width;
548  targetPtr->height = pointCloudFormat->height;
549  targetPtr->points.resize(pointCloudFormat->size /
550  getBytesPerPoint(pointCloudFormat->type));
551  visionx::ColoredPoint3D* pBuffer = reinterpret_cast<visionx::ColoredPoint3D*>(*source);
552  size_t size = targetPtr->points.size();
553  auto& points = targetPtr->points;
554  for (size_t i = 0; i < size; i++)
555  {
556  auto& p = points[i];
557  auto& pB = pBuffer[i];
558  p.x = pB.point.x;
559  p.y = pB.point.y;
560  p.z = pB.point.z;
561  p.r = pB.color.r;
562  p.g = pB.color.g;
563  p.b = pB.color.b;
564  }
565  }
566  else if (pointCloudFormat->type == visionx::eColoredLabeledPoints)
567  {
568 
569  pcl::PointCloud<pcl::PointXYZRGBL>::Ptr temp(new pcl::PointCloud<pcl::PointXYZRGBL>());
570  convertToPCL(source, pointCloudFormat, temp);
571  pcl::copyPointCloud(*temp, *targetPtr);
572  }
573  }
574 
575  void
576  convertFromPCLToDebugDrawer(const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& sourcePtr,
577  armarx::DebugDrawer24BitColoredPointCloud& target,
578  PCLToDebugDrawerConversionMode mode,
579  float pointSize)
580  {
581  target.pointSize = pointSize;
582 
583  std::size_t numPoints = sourcePtr->width * sourcePtr->height;
584  target.points.reserve(numPoints);
585 
586  armarx::DebugDrawer24BitColoredPointCloudElement e;
587  for (const auto& p : sourcePtr->points)
588  {
589  if (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z))
590  {
591  e.x = p.x;
592  e.y = p.y;
593  e.z = p.z;
594 
595  if (mode == eConvertAsColors)
596  {
597  e.color.r = p.r;
598  e.color.g = p.g;
599  e.color.b = p.b;
600  }
601  else
602  {
603  // Simple rainbow coloring
604  float value = (static_cast<int>(e.z) / 10) % 256;
605 
606  e.color.r = static_cast<Ice::Byte>((value > 128) ? (value - 128) * 2 : 0);
607  e.color.g = static_cast<Ice::Byte>((value < 128) ? 2 * value
608  : 255 - ((value - 128) * 2));
609  e.color.b = static_cast<Ice::Byte>((value < 128) ? 255 - (2 * value) : 0);
610  }
611 
612  target.points.push_back(e);
613  }
614  }
615  }
616 
617  void
618  convertFromPCLToDebugDrawer(const pcl::PointCloud<pcl::PointXYZL>::Ptr& sourcePtr,
619  armarx::DebugDrawer24BitColoredPointCloud& target,
620  PCLToDebugDrawerConversionMode mode,
621  float pointSize)
622  {
623  target.pointSize = pointSize;
624 
625  std::size_t numPoints = sourcePtr->width * sourcePtr->height;
626  target.points.reserve(numPoints);
627  std::map<uint32_t, pcl::RGB> colorMap;
628 
629  armarx::DebugDrawer24BitColoredPointCloudElement e;
630  for (const auto& p : sourcePtr->points)
631  {
632  if (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z))
633  {
634  e.x = p.x;
635  e.y = p.y;
636  e.z = p.z;
637 
638  if (mode == eConvertAsLabels)
639  {
640  if (!colorMap.count(p.label))
641  {
642  colorMap[p.label] = pcl::GlasbeyLUT::at(p.label % pcl::GlasbeyLUT::size());
643  }
644  const auto c = colorMap[p.label];
645 
646  e.color.r = c.r;
647  e.color.g = c.g;
648  e.color.b = c.b;
649  }
650  else
651  {
652  // Simple rainbow coloring
653  float value = (static_cast<int>(e.z) / 10) % 256;
654 
655  e.color.r = static_cast<Ice::Byte>((value > 128) ? (value - 128) * 2 : 0);
656  e.color.g = static_cast<Ice::Byte>((value < 128) ? 2 * value
657  : 255 - ((value - 128) * 2));
658  e.color.b = static_cast<Ice::Byte>((value < 128) ? 255 - (2 * value) : 0);
659  }
660 
661  target.points.push_back(e);
662  }
663  }
664  }
665 
666  void
667  convertFromPCLToDebugDrawer(const pcl::PointCloud<pcl::PointXYZ>::Ptr& sourcePtr,
668  armarx::DebugDrawer24BitColoredPointCloud& target,
669  PCLToDebugDrawerConversionMode mode,
670  float pointSize)
671  {
672  (void)mode; // unused
673 
674  target.pointSize = pointSize;
675 
676  std::size_t numPoints = sourcePtr->width * sourcePtr->height;
677  target.points.reserve(numPoints);
678 
679  armarx::DebugDrawer24BitColoredPointCloudElement e;
680  for (const auto& p : sourcePtr->points)
681  {
682  if (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z))
683  {
684  e.x = p.x;
685  e.y = p.y;
686  e.z = p.z;
687 
688  // Simple rainbow coloring
689  float value = (static_cast<int>(e.z) / 10) % 256;
690 
691  e.color.r = static_cast<Ice::Byte>((value > 128) ? (value - 128) * 2 : 0);
692  e.color.g =
693  static_cast<Ice::Byte>((value < 128) ? 2 * value : 255 - ((value - 128) * 2));
694  e.color.b = static_cast<Ice::Byte>((value < 128) ? 255 - (2 * value) : 0);
695 
696  target.points.push_back(e);
697  }
698  }
699  }
700 
701  void
702  convertFromPCLToDebugDrawer(const pcl::PointCloud<pcl::PointXYZRGBL>::Ptr& sourcePtr,
703  armarx::DebugDrawer24BitColoredPointCloud& target,
704  PCLToDebugDrawerConversionMode mode,
705  float pointSize)
706  {
707  target.pointSize = pointSize;
708 
709  std::size_t numPoints = sourcePtr->width * sourcePtr->height;
710  target.points.reserve(numPoints);
711  std::map<uint32_t, pcl::RGB> colorMap;
712 
713  armarx::DebugDrawer24BitColoredPointCloudElement e;
714  for (const auto& p : sourcePtr->points)
715  {
716  if (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z))
717  {
718  e.x = p.x;
719  e.y = p.y;
720  e.z = p.z;
721 
722  if (mode == eConvertAsColors)
723  {
724  e.color.r = p.r;
725  e.color.g = p.g;
726  e.color.b = p.b;
727  }
728  else if (mode == eConvertAsLabels)
729  {
730  if (!colorMap.count(p.label))
731  {
732  colorMap[p.label] = pcl::GlasbeyLUT::at(p.label % pcl::GlasbeyLUT::size());
733  }
734  const auto c = colorMap[p.label];
735 
736  e.color.r = c.r;
737  e.color.g = c.g;
738  e.color.b = c.b;
739  }
740  else
741  {
742  // Simple rainbow coloring
743  float value = (static_cast<int>(e.z) / 10) % 256;
744 
745  e.color.r = static_cast<Ice::Byte>((value > 128) ? (value - 128) * 2 : 0);
746  e.color.g = static_cast<Ice::Byte>((value < 128) ? 2 * value
747  : 255 - ((value - 128) * 2));
748  e.color.b = static_cast<Ice::Byte>((value < 128) ? 255 - (2 * value) : 0);
749  }
750 
751  target.points.push_back(e);
752  }
753  }
754  }
755 
756  // pcl::PointXYZ <-> visionx::Point3D
757  template <>
758  void
759  convertPointFromPCL(const pcl::PointXYZ& source, visionx::Point3D& target)
760  {
761  detail::convertXYZ(source, target);
762  }
763 
764  template <>
765  void
766  convertPointToPCL(const visionx::Point3D& source, pcl::PointXYZ& target)
767  {
768  detail::convertXYZ(source, target);
769  }
770 
771  // pcl::PointXYZRGBA <-> visionx::ColoredPoint3D
772  template <>
773  void
774  convertPointFromPCL(const pcl::PointXYZRGBA& source, visionx::ColoredPoint3D& target)
775  {
776  detail::convertXYZ(source, target.point);
777  detail::convertRGBA(source, target.color);
778  }
779 
780  template <>
781  void
782  convertPointToPCL(const visionx::ColoredPoint3D& source, pcl::PointXYZRGBA& target)
783  {
784  detail::convertXYZ(source.point, target);
785  detail::convertRGBA(source.color, target);
786  }
787 
788  // pcl::PointXYZL <-> visionx::LabeledPoint3D
789  template <>
790  void
791  convertPointFromPCL(const pcl::PointXYZL& source, visionx::LabeledPoint3D& target)
792  {
793  detail::convertXYZ(source, target.point);
794  detail::convertLabel(source, target);
795  }
796 
797  template <>
798  void
799  convertPointToPCL(const visionx::LabeledPoint3D& source, pcl::PointXYZL& target)
800  {
801  detail::convertXYZ(source.point, target);
802  detail::convertLabel(source, target);
803  }
804 
805  // pcl::PointXYZRGBL <-> visionx::ColoredLabeledPoint3D
806  template <>
807  void
808  convertPointFromPCL(const pcl::PointXYZRGBL& source, visionx::ColoredLabeledPoint3D& target)
809  {
810  detail::convertXYZ(source, target.point);
811  detail::convertRGBA(source, target.color);
812  detail::convertLabel(source, target);
813  }
814 
815  template <>
816  void
817  convertPointToPCL(const visionx::ColoredLabeledPoint3D& source, pcl::PointXYZRGBL& target)
818  {
819  detail::convertXYZ(source.point, target);
820  detail::convertRGBA(source.color, target);
821  detail::convertLabel(source, target);
822  }
823 
824  static std::vector<std::string>
825  makePointContentTypeNames()
826  {
827  std::vector<std::string> names(7);
828  names.at(ePoints) = "ePoints";
829  names.at(eColoredPoints) = "eColoredPoints";
830  names.at(eOrientedPoints) = "eOrientedPoints";
831  names.at(eColoredOrientedPoints) = "eColoredOrientedPoints";
832  names.at(eLabeledPoints) = "eLabeledPoints";
833  names.at(eColoredLabeledPoints) = "eColoredLabeledPoints";
834  names.at(eIntensity) = "eIntensity";
835  return names;
836  }
837 
838  std::string
839  toString(PointContentType pointContent)
840  {
841  static const std::vector<std::string> names = makePointContentTypeNames();
842  return names.at(pointContent);
843  }
844 
845 } // namespace visionx::tools
Area
ldouble Area(const point2d &a, const point2d &b, const point2d &c)
Definition: gdiam.cpp:1523
visionx::tools::PCLToDebugDrawerConversionMode
PCLToDebugDrawerConversionMode
Definition: PointCloudConversions.h:125
visionx::tools
Definition: PCLUtilities.cpp:3
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:216
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:668
visionx::tools::eConvertAsColors
@ eConvertAsColors
Definition: PointCloudConversions.h:128
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
visionx::tools::isLabeled
bool isLabeled(PointContentType type)
Definition: PointCloudConversions.cpp:131
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:226
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:237
visionx::tools::convertFromPCL
void convertFromPCL(pcl::PointCloud< pcl::PointXYZRGBA >::Ptr sourcePtr, void **target)
Definition: PointCloudConversions.cpp:144
std::isfinite
bool isfinite(const std::vector< T, Ts... > &v)
Definition: algorithm.h:366
visionx::tools::convertPointFromPCL
void convertPointFromPCL(const pcl::PointXYZ &source, visionx::Point3D &target)
Definition: PointCloudConversions.cpp:759
visionx::tools::convertPointToPCL
void convertPointToPCL(const visionx::Point3D &source, pcl::PointXYZ &target)
Definition: PointCloudConversions.cpp:766
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:855
visionx::tools::isColored
bool isColored(PointContentType type)
Definition: PointCloudConversions.cpp:117
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:576
visionx::tools::toString
std::string toString(PointContentType pointContent)
Definition: PointCloudConversions.cpp:839
visionx::tools::eConvertAsLabels
@ eConvertAsLabels
Definition: PointCloudConversions.h:129
boost::source
Vertex source(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:661
visionx::tools::convertToPCL
void convertToPCL(void **source, visionx::MetaPointCloudFormatPtr pointCloudFormat, pcl::PointCloud< pcl::PointXYZRGBA >::Ptr targetPtr)
Definition: PointCloudConversions.cpp:169
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:13
visionx::tools::getBytesPerPoint
size_t getBytesPerPoint(visionx::PointContentType pointContent)
Definition: PointCloudConversions.cpp:85