Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * pcl.cpp - Convert PCL buffer to PointCloud structure 00004 * 00005 * Created: Wed Nov 02 21:17:35 2011 00006 * Copyright 2011 Tim Niemueller [www.niemueller.de] 00007 ****************************************************************************/ 00008 00009 /* This program is free software; you can redistribute it and/or modify 00010 * it under the terms of the GNU General Public License as published by 00011 * the Free Software Foundation; either version 2 of the License, or 00012 * (at your option) any later version. A runtime exception applies to 00013 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00021 */ 00022 00023 #include <fvutils/adapters/pcl.h> 00024 #include <fvutils/ipc/shm_image.h> 00025 #include <fvutils/color/colorspaces.h> 00026 #include <fvutils/base/types.h> 00027 #include <core/exception.h> 00028 00029 using namespace fawkes; 00030 00031 namespace firevision { 00032 #if 0 /* just to make Emacs auto-indent happy */ 00033 } 00034 #endif 00035 00036 void 00037 convert_buffer_to_pcl(const SharedMemoryImageBuffer *buffer, 00038 pcl::PointCloud<pcl::PointXYZ> &pcl) 00039 { 00040 if (buffer->colorspace() != CARTESIAN_3D_FLOAT) { 00041 throw Exception("Invalid colorspace, expected CARTESIAN_3D_FLOAT"); 00042 } 00043 00044 const pcl_point_t *pclbuf = (const pcl_point_t*)buffer->buffer(); 00045 00046 const unsigned int width = buffer->width(); 00047 const unsigned int height = buffer->height(); 00048 00049 pcl.height = height; 00050 pcl.width = width; 00051 pcl.is_dense = false; 00052 pcl.points.resize(width * height); 00053 00054 for (register unsigned int i = 0; i < width * height; ++i) { 00055 pcl::PointXYZ &p = pcl.points[i]; 00056 const pcl_point_t &pt = pclbuf[i]; 00057 p.x = pt.x; 00058 p.y = pt.y; 00059 p.z = pt.z; 00060 } 00061 00062 } 00063 00064 00065 void 00066 convert_buffer_to_pcl(const SharedMemoryImageBuffer *buffer, 00067 pcl::PointCloud<pcl::PointXYZRGB> &pcl) 00068 { 00069 if (buffer->colorspace() != CARTESIAN_3D_FLOAT) { 00070 throw Exception("Invalid colorspace, expected CARTESIAN_3D_FLOAT"); 00071 } 00072 00073 const pcl_point_t *pclbuf = (const pcl_point_t*)buffer->buffer(); 00074 00075 const unsigned int width = buffer->width(); 00076 const unsigned int height = buffer->height(); 00077 00078 pcl.height = height; 00079 pcl.width = width; 00080 pcl.is_dense = false; 00081 pcl.points.resize(width * height); 00082 00083 for (register unsigned int i = 0; i < width * height; ++i) { 00084 pcl::PointXYZRGB &p = pcl.points[i]; 00085 const pcl_point_t &pt = pclbuf[i]; 00086 p.x = pt.x; 00087 p.y = pt.y; 00088 p.z = pt.z; 00089 p.r = p.g = p.b = 255; 00090 } 00091 } 00092 00093 00094 } // end namespace firevision