40 #ifndef SPECKLE_FILTER_H 41 #define SPECKLE_FILTER_H 43 #include <dynamic_reconfigure/server.h> 45 #include <laser_filters/SpeckleFilterConfig.h> 46 #include <sensor_msgs/LaserScan.h> 54 virtual bool checkWindowValid(
const sensor_msgs::LaserScan& scan,
size_t idx,
size_t window,
double max_range_difference) = 0;
59 virtual bool checkWindowValid(
const sensor_msgs::LaserScan& scan,
size_t idx,
size_t window,
double max_range_difference)
61 const float& range = scan.ranges[idx];
66 for (
size_t neighbor_idx_nr = 1; neighbor_idx_nr < window; ++neighbor_idx_nr)
68 size_t neighbor_idx = idx + neighbor_idx_nr;
69 if (neighbor_idx < scan.ranges.size())
71 const float& neighbor_range = scan.ranges[neighbor_idx];
72 if (neighbor_range != neighbor_range || fabs(neighbor_range - range) > max_range_difference)
84 virtual bool checkWindowValid(
const sensor_msgs::LaserScan& scan,
size_t idx,
size_t window,
double max_distance)
86 int num_neighbors = 0;
87 const float& r1 = scan.ranges[idx];
92 for (
int y = -(
int)window; y < (int)window + 1 && num_neighbors < (
int)window; y++)
97 if (j < 0 || j >= static_cast<int>(scan.ranges.size()) || idx == j || std::isnan(r2))
129 const float d =
sqrt(
131 (2 * r1 * r2 * cosf(y * scan.angle_increment)));
134 if (d <= max_distance)
141 if (num_neighbors < window)
161 bool update(
const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& output_scan);
164 std::shared_ptr<dynamic_reconfigure::Server<laser_filters::SpeckleFilterConfig>>
dyn_server_;
165 void reconfigureCB(laser_filters::SpeckleFilterConfig& config, uint32_t level);
168 SpeckleFilterConfig config_ = SpeckleFilterConfig::__getDefault__();
WindowValidator * validator_
virtual bool checkWindowValid(const sensor_msgs::LaserScan &scan, size_t idx, size_t window, double max_range_difference)=0
LaserScanMaskFilter removes points on directions defined in a mask from a laser scan.
boost::recursive_mutex own_mutex_
virtual bool checkWindowValid(const sensor_msgs::LaserScan &scan, size_t idx, size_t window, double max_range_difference)
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
std::shared_ptr< dynamic_reconfigure::Server< laser_filters::SpeckleFilterConfig > > dyn_server_
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
This is a filter that removes speckle points in a laser scan based on consecutive ranges...
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
virtual bool checkWindowValid(const sensor_msgs::LaserScan &scan, size_t idx, size_t window, double max_distance)