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  */
00038 Laser720to360DataFilter::Laser720to360DataFilter(bool average)
00039 {
00040   __average           = average;
00041   _filtered_data      = (float *)malloc(sizeof(float) * 360);
00042   _filtered_data_size = 360;
00043 }
00044 
00045 void
00046 Laser720to360DataFilter::filter(const float *data, unsigned int data_size)
00047 {
00048   if ( data_size != 720 ) {
00049     throw fawkes::Exception("Expected 720 values, but got %u", data_size);
00050   }
00051 
00052   if (__average) {
00053     _filtered_data[0] = (data[719] / data[0]) / 2.0;
00054     for (unsigned int i = 1; i < 360; ++i) {
00055       _filtered_data[i] = (data[i * 2 - 1] + data[i * 2 + 1]) / 2.0;
00056     }
00057   } else {
00058     for (unsigned int i = 0; i < 360; ++i) {
00059       _filtered_data[i] = data[i * 2];
00060     }
00061   }
00062 }

Generated on Tue Feb 22 13:32:30 2011 for Fawkes API by  doxygen 1.4.7