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
32namespace 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
c
constexpr T c
Definition UnscentedKalmanFilterTest.cpp:46
Area
ldouble Area(const point2d &a, const point2d &b, const point2d &c)
Definition gdiam.cpp:1523
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
std::isfinite
bool isfinite(const std::vector< T, Ts... > &v)
Definition algorithm.h:366
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::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
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::getPointContentType< pcl::PointXYZL >
visionx::PointContentType getPointContentType< pcl::PointXYZL >()
Definition PointCloudConversions.cpp:44
visionx::tools::getPointContentType< pcl::PointXYZRGBA >
visionx::PointContentType getPointContentType< pcl::PointXYZRGBA >()
Definition PointCloudConversions.cpp:65
visionx::tools::getPointContentType< pcl::PointXYZI >
visionx::PointContentType getPointContentType< pcl::PointXYZI >()
Definition PointCloudConversions.cpp:51
visionx::tools::getBytesPerPoint
size_t getBytesPerPoint(visionx::PointContentType pointContent)
Definition PointCloudConversions.cpp:85
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::convertPointFromPCL
void convertPointFromPCL(const pcl::PointXYZ &source, visionx::Point3D &target)
Definition PointCloudConversions.cpp:759
visionx::tools::toString
std::string toString(PointContentType pointContent)
Definition PointCloudConversions.cpp:839
visionx::tools::getPointContentType< pcl::PointXYZRGBL >
visionx::PointContentType getPointContentType< pcl::PointXYZRGBL >()
Definition PointCloudConversions.cpp:72
visionx::tools::isColored
bool isColored(PointContentType type)
Definition PointCloudConversions.cpp:117
visionx::tools::getPointContentType< pcl::PointNormal >
PointContentType getPointContentType< pcl::PointNormal >()
Definition PointCloudConversions.cpp:58
visionx::tools::convertPointToPCL
void convertPointToPCL(const visionx::Point3D &source, pcl::PointXYZ &target)
Definition PointCloudConversions.cpp:766
visionx::tools::convertToPCL
void convertToPCL(void **source, visionx::MetaPointCloudFormatPtr pointCloudFormat, pcl::PointCloud< pcl::PointXYZRGBA >::Ptr targetPtr)
Definition PointCloudConversions.cpp:169
visionx::tools::isLabeled
bool isLabeled(PointContentType type)
Definition PointCloudConversions.cpp:131
visionx::tools::convertFromPCL
void convertFromPCL(pcl::PointCloud< pcl::PointXYZRGBA >::Ptr sourcePtr, void **target)
Definition PointCloudConversions.cpp:144
visionx::tools::getPointContentType< pcl::PointXYZRGBNormal >
visionx::PointContentType getPointContentType< pcl::PointXYZRGBNormal >()
Definition PointCloudConversions.cpp:79
visionx::tools::getPointContentType< pcl::PointXYZ >
visionx::PointContentType getPointContentType< pcl::PointXYZ >()
Definition PointCloudConversions.cpp:37