Fawkes API Fawkes Development Version
pcl.cpp
1
2/***************************************************************************
3 * pcl.cpp - Convert PCL buffer to PointCloud structure
4 *
5 * Created: Wed Nov 02 21:17:35 2011
6 * Copyright 2011 Tim Niemueller [www.niemueller.de]
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version. A runtime exception applies to
13 * this software (see LICENSE.GPL_WRE file mentioned below for details).
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
21 */
22
23#include <core/exception.h>
24#include <fvutils/adapters/pcl.h>
25#include <fvutils/base/types.h>
26#include <fvutils/color/colorspaces.h>
27#include <fvutils/ipc/shm_image.h>
28
29using namespace fawkes;
30
31namespace firevision {
32
33void
34convert_buffer_to_pcl(const SharedMemoryImageBuffer *buffer, pcl::PointCloud<pcl::PointXYZ> &pcl)
35{
36 if (buffer->colorspace() != CARTESIAN_3D_FLOAT) {
37 throw Exception("Invalid colorspace, expected CARTESIAN_3D_FLOAT");
38 }
39
40 const pcl_point_t *pclbuf = (const pcl_point_t *)buffer->buffer();
41
42 const unsigned int width = buffer->width();
43 const unsigned int height = buffer->height();
44
45 pcl.height = height;
46 pcl.width = width;
47 pcl.is_dense = false;
48 pcl.points.resize((size_t)width * (size_t)height);
49
50 for (unsigned int i = 0; i < width * height; ++i) {
51 pcl::PointXYZ & p = pcl.points[i];
52 const pcl_point_t &pt = pclbuf[i];
53 p.x = pt.x;
54 p.y = pt.y;
55 p.z = pt.z;
56 }
57}
58
59void
60convert_buffer_to_pcl(const SharedMemoryImageBuffer *buffer, pcl::PointCloud<pcl::PointXYZRGB> &pcl)
61{
62 if (buffer->colorspace() != CARTESIAN_3D_FLOAT) {
63 throw Exception("Invalid colorspace, expected CARTESIAN_3D_FLOAT");
64 }
65
66 const pcl_point_t *pclbuf = (const pcl_point_t *)buffer->buffer();
67
68 const unsigned int width = buffer->width();
69 const unsigned int height = buffer->height();
70
71 pcl.height = height;
72 pcl.width = width;
73 pcl.is_dense = false;
74 pcl.points.resize((size_t)width * (size_t)height);
75
76 for (unsigned int i = 0; i < width * height; ++i) {
77 pcl::PointXYZRGB & p = pcl.points[i];
78 const pcl_point_t &pt = pclbuf[i];
79 p.x = pt.x;
80 p.y = pt.y;
81 p.z = pt.z;
82 p.r = p.g = p.b = 255;
83 }
84}
85
86} // end namespace firevision
Base class for exceptions in Fawkes.
Definition: exception.h:36
Fawkes library namespace.