Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * min_circle.cpp - Laser data min circle data filter 00004 * 00005 * Created: Sat Feb 19 00:23:27 2011 00006 * Copyright 2006-2011 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 "min_circle.h" 00024 00025 #include <utils/math/angle.h> 00026 #include <cstdlib> 00027 00028 /** @class LaserMinCircleDataFilter "min_circle.h" 00029 * Erase beams below a certain minimum distance distance. 00030 * All beams shorter than a given radius are erase (set to 0). 00031 * @author Tim Niemueller 00032 */ 00033 00034 /** Constructor. 00035 * @param radius radius of cut-off circle in meters 00036 * @param in_data_size number of entries input value arrays 00037 * @param in vector of input arrays 00038 */ 00039 LaserMinCircleDataFilter::LaserMinCircleDataFilter(float radius, 00040 unsigned int in_data_size, 00041 std::vector<LaserDataFilter::Buffer *> &in) 00042 : LaserDataFilter(in_data_size, in, in.size()) 00043 { 00044 __radius = radius; 00045 } 00046 00047 00048 void 00049 LaserMinCircleDataFilter::filter() 00050 { 00051 const unsigned int vecsize = std::min(in.size(), out.size()); 00052 const unsigned int arrsize = std::min(in_data_size, out_data_size); 00053 for (unsigned int a = 0; a < vecsize; ++a) { 00054 out[a]->frame = in[a]->frame; 00055 float *inbuf = in[a]->values; 00056 float *outbuf = out[a]->values; 00057 for (unsigned int i = 0; i < arrsize; ++i) { 00058 if (inbuf[i] < __radius) { 00059 outbuf[i] = 0; 00060 } else { 00061 outbuf[i] = inbuf[i]; 00062 } 00063 } 00064 } 00065 }