diff --git a/cartographer/mapping_3d/submaps.cc b/cartographer/mapping_3d/submaps.cc index 7a4f94d..8b1842b 100644 --- a/cartographer/mapping_3d/submaps.cc +++ b/cartographer/mapping_3d/submaps.cc @@ -178,6 +178,20 @@ void InsertSegmentsIntoProbabilityGrid( } } +// Filters 'laser_fan', retaining only the returns that have no more than +// 'max_range' distance from the laser origin. Removes misses and reflectivity +// information. +sensor::LaserFan FilterLaserFanByMaxRange(const sensor::LaserFan& laser_fan, + const float max_range) { + sensor::LaserFan result{laser_fan.origin, {}, {}, {}}; + for (const Eigen::Vector3f& return_ : laser_fan.returns) { + if ((return_ - laser_fan.origin).norm() <= max_range) { + result.returns.push_back(return_); + } + } + return result; +} + } // namespace void InsertIntoProbabilityGrid( @@ -273,8 +287,8 @@ void Submaps::InsertLaserFan(const sensor::LaserFan& laser_fan) { for (const int index : insertion_indices()) { Submap* submap = submaps_[index].get(); laser_fan_inserter_.Insert( - sensor::FilterLaserFanByMaxRange(laser_fan, - options_.high_resolution_max_range()), + FilterLaserFanByMaxRange(laser_fan, + options_.high_resolution_max_range()), &submap->high_resolution_hybrid_grid); laser_fan_inserter_.Insert(laser_fan, &submap->low_resolution_hybrid_grid); submap->end_laser_fan_index = num_laser_fans_; diff --git a/cartographer/sensor/laser.cc b/cartographer/sensor/laser.cc index 46a81e6..36bbb10 100644 --- a/cartographer/sensor/laser.cc +++ b/cartographer/sensor/laser.cc @@ -94,17 +94,6 @@ LaserFan TransformLaserFan(const LaserFan& laser_fan, }; } -LaserFan FilterLaserFanByMaxRange(const LaserFan& laser_fan, - const float max_range) { - LaserFan result{laser_fan.origin, {}, {}, {}}; - for (const Eigen::Vector3f& return_ : laser_fan.returns) { - if ((return_ - laser_fan.origin).norm() <= max_range) { - result.returns.push_back(return_); - } - } - return result; -} - LaserFan CropLaserFan(const LaserFan& laser_fan, const float min_z, const float max_z) { return LaserFan{laser_fan.origin, Crop(laser_fan.returns, min_z, max_z), diff --git a/cartographer/sensor/laser.h b/cartographer/sensor/laser.h index 0d511f3..0a023be 100644 --- a/cartographer/sensor/laser.h +++ b/cartographer/sensor/laser.h @@ -54,11 +54,6 @@ LaserFan FromProto(const proto::LaserFan& proto); LaserFan TransformLaserFan(const LaserFan& laser_fan, const transform::Rigid3f& transform); -// Filter a 'laser_fan', retaining only the returns that have no more than -// 'max_range' distance from the laser origin. Removes misses and reflectivity -// information. -LaserFan FilterLaserFanByMaxRange(const LaserFan& laser_fan, float max_range); - // Crops 'laser_fan' according to the region defined by 'min_z' and 'max_z'. LaserFan CropLaserFan(const LaserFan& laser_fan, float min_z, float max_z);