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>
39 main(
int argc,
char **argv)
42 float* readings =
new float[data_size];
44 for (
int i = 0; i < data_size; ++i) {
50 readings[135] = 1.50f;
51 readings[270] = 1.50f;
52 readings[180] = 1.50f;
56 std::vector<float*> in;
57 in.push_back(readings);
59 for (std::vector<float*>::const_iterator it = in.begin();
60 it != in.end(); ++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];
67 printf(
"IN %lf / %lf (%.2f, %.2f, %.2f)\n",
68 angle, length, p.
x(), p.
y(), p.
z());
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;
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) {
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];
91 printf(
"OUT %lf / %lf (%.2f, %.2f, %.2f)\n",
92 new_angle, new_length, p.
x(), p.
y(), p.
z());
virtual float y() const
RO-getter for y.
A homogeneous representation of a polar coordinate.
Fawkes library namespace.
virtual float z() const
RO-getter for z.
Projects one laser into another laser's plane.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
virtual float x() const
RO-getter for x.