Fawkes API  Fawkes Development Version
projection.cpp
00001 
00002 /***************************************************************************
00003  *  projection.cpp - Laser data projection filter
00004  *
00005  *  Created: Tue Mar 22 16:30:51 2011
00006  *  Copyright  2011  Christoph Schwering
00007  *             2011  Tim Niemueller
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 "projection.h"
00024 
00025 #include <core/exception.h>
00026 #include <utils/math/angle.h>
00027 #include <geometry/hom_polar.h>
00028 #include <geometry/hom_vector.h>
00029 
00030 #include <cstdlib>
00031 #include <cstring>
00032 #include <sys/types.h>
00033 
00034 using namespace fawkes;
00035 
00036 /** @class LaserProjectionDataFilter "filters/projection.h"
00037  * Projects one laser into another laser's plane.
00038  * This first transforms the laser value into the target frame, and then
00039  * projects it into the X-Y plane (which is assumed to be the laser plane
00040  * of another (virtual) laser. Additionally some sanity filtering in all
00041  * three axes is applied after the transformation, but before the
00042  * projection.
00043  * @author Tim Niemueller, Christoph Schwering
00044  */
00045 
00046 /** Constructor.
00047  * @param tf_listener transform listener to get transform from
00048  * @param target_frame target coordinate fram to project into
00049  * @param not_from_x lower X boundary of ignored rectangle
00050  * @param not_to_x upper X boundary of ignored rectangle
00051  * @param not_from_y lower Y boundary of ignored rectangle
00052  * @param not_to_y upper Y boundary of ignored rectangle
00053  * @param only_from_z minimum Z value for accepted points
00054  * @param only_to_z maximum Z value for accepted points
00055  * @param in_data_size number of entries input value arrays
00056  * @param in vector of input arrays
00057  */
00058 LaserProjectionDataFilter::LaserProjectionDataFilter(
00059     tf::TransformListener *tf_listener,
00060     std::string target_frame,
00061     float not_from_x, float not_to_x,
00062     float not_from_y, float not_to_y,
00063     float only_from_z, float only_to_z,
00064     unsigned int in_data_size,
00065     std::vector<LaserDataFilter::Buffer *> &in)
00066   : LaserDataFilter(in_data_size, in, in.size()),
00067     tf_listener_(tf_listener), target_frame_(target_frame),
00068     not_from_x_(not_from_x), not_to_x_(not_to_x),
00069     not_from_y_(not_from_y), not_to_y_(not_to_y),
00070     only_from_z_(only_from_z), only_to_z_(only_to_z)
00071 {
00072   // Generate lookup tables for sin and cos
00073   for (unsigned int i = 0; i < 360; ++i) {
00074     sin_angles360[i] = sinf(deg2rad(i));
00075     cos_angles360[i] = cosf(deg2rad(i));
00076   }
00077   for (unsigned int i = 0; i < 720; ++i) {
00078     sin_angles720[i] = sinf(deg2rad((float)i / 2.));
00079     cos_angles720[i] = cosf(deg2rad((float)i / 2.));
00080   }
00081 
00082   index_factor_ = out_data_size / 360.;
00083 }
00084 
00085 LaserProjectionDataFilter::~LaserProjectionDataFilter()
00086 {
00087 }
00088 
00089 
00090 /** Set the output buffer applying filtering.
00091  * This checks the given point against the configured bounds. If and
00092  * only if the point satisfies the given criteria it is set at the
00093  * appropriate array index of the buffer.
00094  * @param outbuf buffer in which to set the value conditionally
00095  * @param p point to check and (maybe) set
00096  */
00097 inline void
00098 LaserProjectionDataFilter::set_output(float *outbuf, fawkes::tf::Point &p)
00099 {
00100   if ( ((p.x() >= not_from_x_) && (p.x() <= not_to_x_) &&
00101         (p.y() >= not_from_y_) && (p.y() <= not_to_y_)) ||
00102        (p.z() < only_from_z_) || (p.z() > only_to_z_) )
00103   {
00104     // value is inside "forbidden" robot rectangle or
00105     // below or above Z thresholds
00106     return;
00107   }
00108 
00109   p.setZ(0.);
00110   float phi    = atan2f(p.y(), p.x());
00111     
00112   unsigned int j =
00113     (unsigned int)roundf(rad2deg(normalize_rad(phi)) * index_factor_);
00114   if (j > out_data_size) j = 0; // might happen just at the boundary
00115 
00116   if (outbuf[j] == 0.) {
00117     outbuf[j] = (float)p.length();
00118   } else {
00119     outbuf[j] = std::min(outbuf[j], (float)p.length());
00120   }
00121 }
00122 
00123 void
00124 LaserProjectionDataFilter::filter()
00125 {
00126   const unsigned int vecsize = std::min(in.size(), out.size());
00127   for (unsigned int a = 0; a < vecsize; ++a) {
00128     out[a]->frame = target_frame_;
00129     float* inbuf  = in[a]->values;
00130     float* outbuf = out[a]->values;
00131     memset(outbuf, 0, sizeof(float) * out_data_size);
00132 
00133     tf::StampedTransform t;
00134     tf::Point p;
00135 
00136     tf_listener_->lookup_transform(target_frame_, in[a]->frame,
00137                                    fawkes::Time(0, 0), t);
00138 
00139     if (in_data_size == 360) {
00140       for (unsigned int i = 0; i < 360; ++i) {
00141         if (inbuf[i] == 0.)  continue;
00142         p.setValue(inbuf[i] * cos_angles360[i], inbuf[i] * sin_angles360[i], 0.);
00143         p = t * p;
00144 
00145         set_output(outbuf, p);
00146       }
00147     } else if (in_data_size == 720) {
00148       for (unsigned int i = 0; i < 720; ++i) {
00149         if (inbuf[i] == 0.)  continue;
00150 
00151         p.setValue(inbuf[i] * cos_angles720[i], inbuf[i] * sin_angles720[i], 0.);
00152         p = t * p;
00153 
00154         set_output(outbuf, p);
00155       }
00156     } else {
00157       for (unsigned int i = 0; i < in_data_size; ++i) {
00158         if (inbuf[i] == 0.)  continue;
00159 
00160         float a = deg2rad(360.f / (float)i);
00161         p.setValue(inbuf[i] * cos(a), inbuf[i] * sin(a), 0.);
00162         p = t * p;
00163 
00164         set_output(outbuf, p);
00165       }
00166     }
00167   }
00168 }
00169