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