Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
qa_projection.cpp
1 
2 /***************************************************************************
3  * qa_projection.cpp - QA for Projection Filter
4  *
5  * Created: Wed Dec 30 12:00:00 2009
6  * Copyright 2005-2009 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22  */
23 
24 /// @cond QA
25 
26 #include "../filters/projection.h"
27 #include <geometry/hom_polar.h>
28 #include <utils/time/tracker.h>
29 #include <utils/math/angle.h>
30 #include <utils/math/coord.h>
31 
32 #include <cstdio>
33 #include <cstring>
34 #include <unistd.h>
35 
36 using namespace fawkes;
37 
38 int
39 main(int argc, char **argv)
40 {
41  int data_size = 360;
42  float* readings = new float[data_size];
43 
44  for (int i = 0; i < data_size; ++i) {
45  readings[i] = 0.0f;
46  }
47  //readings[0] = 2.934f;
48  readings[0] = 1.50f;
49  readings[90] = 1.50f;
50  readings[135] = 1.50f;
51  readings[270] = 1.50f;
52  readings[180] = 1.50f;
53  readings[40] = 1.50f; // this one is too low (Z_THRESHOLD)
54  readings[39] = 0.40f; // this one is in the robot
55 
56  std::vector<float*> in;
57  in.push_back(readings);
58 
59  for (std::vector<float*>::const_iterator it = in.begin();
60  it != in.end(); ++it) {
61  float* inbuf = *it;
62  for (int i = 0; i < data_size; ++i) {
63  if (inbuf[i] != 0.0) {
64  const float angle = static_cast<float>(i);
65  const float length = inbuf[i];
66  const HomPolar p = HomPolar(length, deg2rad(angle));
67  printf("IN %lf / %lf (%.2f, %.2f, %.2f)\n",
68  angle, length, p.x(), p.y(), p.z());
69  }
70  }
71  }
72 
73  const bool LEFT = false;
74  const LaserProjectionDataFilter::Rotation LASER_ROT(0.0f, -90.0f, (LEFT ? 90.0f : -90.0f));
75  const LaserProjectionDataFilter::Rotation FIXTURE_ROT((LEFT ? -39.0f : 39.0f), 0.0f, (LEFT ? -39.0f : 39.0f));
76  const LaserProjectionDataFilter::Translation TRANS(0.08f, (LEFT ? 0.20f : -0.20f), 1.17f);
77  const LaserProjectionDataFilter::Rectangle ROBOT_RECT(-0.07, 0.31, -0.20f, 0.20f);
78  const float Z_THRESHOLD = -0.05;
79  LaserProjectionDataFilter filter(LASER_ROT, FIXTURE_ROT, TRANS, ROBOT_RECT, Z_THRESHOLD , data_size, in);
80  filter.filter();
81 
82  const std::vector<float*> out = filter.get_out_vector();
83  for (std::vector<float*>::const_iterator it = out.begin();
84  it != out.end(); ++it) {
85  float* outbuf = *it;
86  for (int i = 0; i < data_size; ++i) {
87  if (outbuf[i] != 0.0f) {
88  const float new_angle = static_cast<float>(i);
89  const float new_length = outbuf[i];
90  const HomPolar p = HomPolar(new_length, deg2rad(new_angle));
91  printf("OUT %lf / %lf (%.2f, %.2f, %.2f)\n",
92  new_angle, new_length, p.x(), p.y(), p.z());
93  }
94  }
95  }
96 
97  delete[] readings;
98  return 0;
99 }
100 
101 /// @endcond
virtual float y() const
RO-getter for y.
Definition: hom_coord.cpp:115
A homogeneous representation of a polar coordinate.
Definition: hom_polar.h:31
virtual float z() const
RO-getter for z.
Definition: hom_coord.cpp:145
Projects one laser into another laser&#39;s plane.
Definition: projection.h:41
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
virtual float x() const
RO-getter for x.
Definition: hom_coord.cpp:85