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 
29 using namespace fawkes;
30 
31 namespace firevision {
32 
33 void
34 convert_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 
59 void
60 convert_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
fawkes
pcl::PointCloud< pcl::PointXYZ >
fawkes::Exception
Definition: exception.h:41