From: <ei...@us...> - 2009-09-04 21:18:08
|
Revision: 23875 http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23875&view=rev Author: eitanme Date: 2009-09-04 21:18:00 +0000 (Fri, 04 Sep 2009) Log Message: ----------- Adding a filter to interpolate between laser readings to generate range readings for scans that return errors Modified Paths: -------------- pkg/trunk/stacks/laser_pipeline/laser_filters/examples/shadow_filter_example.yaml pkg/trunk/stacks/laser_pipeline/laser_filters/laser_filters_plugins.xml pkg/trunk/stacks/laser_pipeline/laser_filters/src/laser_scan_filters.cpp Added Paths: ----------- pkg/trunk/stacks/laser_pipeline/laser_filters/include/laser_filters/interpolation_filter.h Modified: pkg/trunk/stacks/laser_pipeline/laser_filters/examples/shadow_filter_example.yaml =================================================================== --- pkg/trunk/stacks/laser_pipeline/laser_filters/examples/shadow_filter_example.yaml 2009-09-04 21:10:23 UTC (rev 23874) +++ pkg/trunk/stacks/laser_pipeline/laser_filters/examples/shadow_filter_example.yaml 2009-09-04 21:18:00 UTC (rev 23875) @@ -12,3 +12,5 @@ lower_threshold: 100 upper_threshold: 10000 disp_histogram: 0 +#- name: interpolation +# type: InterpolationFilter Added: pkg/trunk/stacks/laser_pipeline/laser_filters/include/laser_filters/interpolation_filter.h =================================================================== --- pkg/trunk/stacks/laser_pipeline/laser_filters/include/laser_filters/interpolation_filter.h (rev 0) +++ pkg/trunk/stacks/laser_pipeline/laser_filters/include/laser_filters/interpolation_filter.h 2009-09-04 21:18:00 UTC (rev 23875) @@ -0,0 +1,115 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +#ifndef INTERPOLATION_FILTER_H +#define INTERPOLATION_FILTER_H +/** +\author Eitan Marder-Eppstein +@b InterpolationFilter takes input scans and for readings that come back as errors, interpolates between valid readings to generate range values for them. + +**/ + + +#include "filters/filter_base.h" +#include "sensor_msgs/LaserScan.h" + +namespace laser_filters +{ + +class InterpolationFilter : public filters::FilterBase<sensor_msgs::LaserScan> +{ +public: + + bool configure() + { + return true; + } + + virtual ~InterpolationFilter() + { + } + + bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan) + { + double previous_valid_range = input_scan.range_max - .01; + double next_valid_range = input_scan.range_max - .01; + filtered_scan= input_scan; + + unsigned int i = 0; + while(i < input_scan.ranges.size()) // Need to check every reading in the current scan + { + //check if the reading is out of range for some reason + if (filtered_scan.ranges[i] <= input_scan.range_min || + filtered_scan.ranges[i] >= input_scan.range_max){ + + //we need to find the next valid range reading + unsigned int j = i + 1; + unsigned int start_index = i; + unsigned int end_index = i; + while(j < input_scan.ranges.size()){ + if (filtered_scan.ranges[j] <= input_scan.range_min || + filtered_scan.ranges[j] >= input_scan.range_max){ + end_index = j; + } + else{ + next_valid_range = filtered_scan.ranges[j]; + break; + } + ++j; + } + + //for now, we'll just take the average between the two valid range readings + double average_range = (previous_valid_range + next_valid_range) / 2.0; + + for(unsigned int k = start_index; k <= end_index; k++){ + filtered_scan.ranges[k] = average_range; + } + + //make sure to update our previous valid range reading + previous_valid_range = next_valid_range; + i = j + 1; + } + else{ + previous_valid_range = filtered_scan.ranges[i]; + ++i; + } + + } + return true; + } +}; + +} + +#endif // LASER_SCAN_INTENSITY_FILTER_H Modified: pkg/trunk/stacks/laser_pipeline/laser_filters/laser_filters_plugins.xml =================================================================== --- pkg/trunk/stacks/laser_pipeline/laser_filters/laser_filters_plugins.xml 2009-09-04 21:10:23 UTC (rev 23874) +++ pkg/trunk/stacks/laser_pipeline/laser_filters/laser_filters_plugins.xml 2009-09-04 21:18:00 UTC (rev 23875) @@ -27,6 +27,12 @@ the veiling effect. </description> </class> + <class name="InterpolationFilter" type="laser_filters::InterpolationFilter" + base_class_type="filters::FilterBase<sensor_msgs::LaserScan>"> + <description> + This is a filter that will generate range readings for error readings in a scan by interpolating between valid readings on either side of the error + </description> + </class> </library> <library path="lib/libpointcloud_filters"> <class name="PointCloudFootprintFilter" type="laser_filters::PointCloudFootprintFilter" Modified: pkg/trunk/stacks/laser_pipeline/laser_filters/src/laser_scan_filters.cpp =================================================================== --- pkg/trunk/stacks/laser_pipeline/laser_filters/src/laser_scan_filters.cpp 2009-09-04 21:10:23 UTC (rev 23874) +++ pkg/trunk/stacks/laser_pipeline/laser_filters/src/laser_scan_filters.cpp 2009-09-04 21:18:00 UTC (rev 23875) @@ -31,6 +31,7 @@ #include "laser_filters/intensity_filter.h" #include "laser_filters/scan_shadows_filter.h" #include "laser_filters/footprint_filter.h" +#include "laser_filters/interpolation_filter.h" #include "sensor_msgs/LaserScan.h" #include "filters/filter_base.h" @@ -41,3 +42,4 @@ PLUGINLIB_REGISTER_CLASS(LaserScanIntensityFilter, laser_filters::LaserScanIntensityFilter, filters::FilterBase<sensor_msgs::LaserScan>) PLUGINLIB_REGISTER_CLASS(LaserScanFootprintFilter, laser_filters::LaserScanFootprintFilter, filters::FilterBase<sensor_msgs::LaserScan>) PLUGINLIB_REGISTER_CLASS(ScanShadowsFilter, laser_filters::ScanShadowsFilter, filters::FilterBase<sensor_msgs::LaserScan>) +PLUGINLIB_REGISTER_CLASS(InterpolationFilter, laser_filters::InterpolationFilter, filters::FilterBase<sensor_msgs::LaserScan>) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |