Fawkes API
Fawkes Development Version
|
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