Fawkes API  Fawkes Development Version
pcl.cpp
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