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