Fawkes API  Fawkes Development Version
qa_projection.cpp
00001 
00002 /***************************************************************************
00003  *  qa_projection.cpp - QA for Projection Filter
00004  *
00005  *  Created: Wed Dec 30 12:00:00 2009
00006  *  Copyright  2005-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. A runtime exception applies to
00014  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00022  */
00023 
00024 /// @cond QA
00025 
00026 #include "../filters/projection.h"
00027 #include <geometry/hom_polar.h>
00028 #include <utils/time/tracker.h>
00029 #include <utils/math/angle.h>
00030 #include <utils/math/coord.h>
00031 
00032 #include <cstdio>
00033 #include <cstring>
00034 #include <unistd.h>
00035 
00036 using namespace fawkes;
00037 
00038 int
00039 main(int argc, char **argv)
00040 {
00041   int data_size = 360;
00042   float* readings = new float[data_size];
00043 
00044   for (int i = 0; i < data_size; ++i) {
00045     readings[i] = 0.0f;
00046   }
00047   //readings[0] = 2.934f;
00048   readings[0] = 1.50f;
00049   readings[90] = 1.50f;
00050   readings[135] = 1.50f;
00051   readings[270] = 1.50f;
00052   readings[180] = 1.50f;
00053   readings[40] = 1.50f; // this one is too low (Z_THRESHOLD)
00054   readings[39] = 0.40f; // this one is in the robot
00055 
00056   std::vector<float*> in;
00057   in.push_back(readings);
00058 
00059   for (std::vector<float*>::const_iterator it = in.begin();
00060        it != in.end(); ++it) {
00061     float* inbuf = *it;
00062     for (int i = 0; i < data_size; ++i) {
00063       if (inbuf[i] != 0.0) {
00064         const float angle = static_cast<float>(i);
00065         const float length = inbuf[i];
00066         const HomPolar p = HomPolar(length, deg2rad(angle));
00067         printf("IN  %lf / %lf (%.2f, %.2f, %.2f)\n",
00068                angle, length, p.x(), p.y(), p.z());
00069       }
00070     }
00071   }
00072 
00073   const bool LEFT = false;
00074   const LaserProjectionDataFilter::Rotation LASER_ROT(0.0f, -90.0f, (LEFT ? 90.0f : -90.0f));
00075   const LaserProjectionDataFilter::Rotation FIXTURE_ROT((LEFT ? -39.0f : 39.0f), 0.0f, (LEFT ? -39.0f : 39.0f));
00076   const LaserProjectionDataFilter::Translation TRANS(0.08f, (LEFT ? 0.20f : -0.20f), 1.17f);
00077   const LaserProjectionDataFilter::Rectangle ROBOT_RECT(-0.07, 0.31, -0.20f, 0.20f);
00078   const float Z_THRESHOLD = -0.05;
00079   LaserProjectionDataFilter filter(LASER_ROT, FIXTURE_ROT, TRANS, ROBOT_RECT, Z_THRESHOLD , data_size, in);
00080   filter.filter();
00081 
00082   const std::vector<float*> out = filter.get_out_vector();
00083   for (std::vector<float*>::const_iterator it = out.begin();
00084        it != out.end(); ++it) {
00085     float* outbuf = *it;
00086     for (int i = 0; i < data_size; ++i) {
00087       if (outbuf[i] != 0.0f) {
00088         const float new_angle = static_cast<float>(i);
00089         const float new_length = outbuf[i];
00090         const HomPolar p = HomPolar(new_length, deg2rad(new_angle));
00091         printf("OUT %lf / %lf (%.2f, %.2f, %.2f)\n",
00092                new_angle, new_length, p.x(), p.y(), p.z());
00093       }
00094     }
00095   }
00096 
00097   delete[] readings;
00098   return 0;
00099 }
00100 
00101 /// @endcond