Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * 720to360.cpp - Laser data data filter to downsample 720 to 360 values 00004 * 00005 * Created: Tue Jun 23 14:37:36 2009 00006 * Copyright 2006-2009 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 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 file in the doc directory. 00021 */ 00022 00023 #include "720to360.h" 00024 00025 #include <core/exception.h> 00026 #include <utils/math/angle.h> 00027 #include <cstdlib> 00028 00029 /** @class Laser720to360DataFilter "720to360.h" 00030 * Downsample filter from 720 to 360 values. 00031 * @author Tim Niemueller 00032 */ 00033 00034 /** Constructor. 00035 * @param average if true, beams will be averaged by left and right neighbours, 00036 * otherwise every second beam will be used 00037 * @param in_data_size number of entries input value arrays 00038 * @param in vector of input arrays 00039 */ 00040 Laser720to360DataFilter::Laser720to360DataFilter(bool average, 00041 unsigned int in_data_size, 00042 std::vector<LaserDataFilter::Buffer *> &in) 00043 : LaserDataFilter(in_data_size, in, in.size()) 00044 { 00045 if (in_data_size != 720) { 00046 throw fawkes::Exception("720to360 filter needs input array size of " 00047 "720 entries"); 00048 } 00049 set_out_data_size(360); 00050 __average = average; 00051 } 00052 00053 void 00054 Laser720to360DataFilter::filter() 00055 { 00056 const unsigned int vecsize = std::min(in.size(), out.size()); 00057 for (unsigned int a = 0; a < vecsize; ++a) { 00058 out[a]->frame = in[a]->frame; 00059 float *inbuf = in[a]->values; 00060 float *outbuf = out[a]->values; 00061 00062 if (__average) { 00063 outbuf[0] = (inbuf[719] + inbuf[0]) / 2.0; 00064 for (unsigned int i = 1; i < 360; ++i) { 00065 outbuf[i] = (inbuf[i * 2 - 1] + inbuf[i * 2 + 1]) / 2.0; 00066 } 00067 } else { 00068 for (unsigned int i = 0; i < 360; ++i) { 00069 outbuf[i] = inbuf[i * 2]; 00070 } 00071 } 00072 } 00073 }