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