From a313448bf01cc10f015ad872098d58b5b9e5162c Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Wed, 5 Jul 2017 13:39:44 +0200 Subject: [PATCH] Change CastRays to directly take the ProbabilityGrid. (#386) PAIR=wohe --- .../mapping_2d/range_data_inserter.cc | 14 ++---- cartographer/mapping_2d/range_data_inserter.h | 3 -- cartographer/mapping_2d/ray_casting.cc | 43 +++++++++++-------- cartographer/mapping_2d/ray_casting.h | 17 ++++---- 4 files changed, 37 insertions(+), 40 deletions(-) diff --git a/cartographer/mapping_2d/range_data_inserter.cc b/cartographer/mapping_2d/range_data_inserter.cc index 0974b66..77233e7 100644 --- a/cartographer/mapping_2d/range_data_inserter.cc +++ b/cartographer/mapping_2d/range_data_inserter.cc @@ -53,19 +53,11 @@ RangeDataInserter::RangeDataInserter( void RangeDataInserter::Insert(const sensor::RangeData& range_data, ProbabilityGrid* const probability_grid) const { - CHECK_NOTNULL(probability_grid)->StartUpdate(); - // By not starting a new update after hits are inserted, we give hits priority // (i.e. no hits will be ignored because of a miss in the same cell). - CastRays(range_data, probability_grid->limits(), - [this, &probability_grid](const Eigen::Array2i& hit) { - probability_grid->ApplyLookupTable(hit, hit_table_); - }, - [this, &probability_grid](const Eigen::Array2i& miss) { - if (options_.insert_free_space()) { - probability_grid->ApplyLookupTable(miss, miss_table_); - } - }); + CHECK_NOTNULL(probability_grid)->StartUpdate(); + CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(), + probability_grid); } } // namespace mapping_2d diff --git a/cartographer/mapping_2d/range_data_inserter.h b/cartographer/mapping_2d/range_data_inserter.h index b0c0db6..2b55103 100644 --- a/cartographer/mapping_2d/range_data_inserter.h +++ b/cartographer/mapping_2d/range_data_inserter.h @@ -45,9 +45,6 @@ class RangeDataInserter { void Insert(const sensor::RangeData& range_data, ProbabilityGrid* probability_grid) const; - const std::vector& hit_table() const { return hit_table_; } - const std::vector& miss_table() const { return miss_table_; } - private: const proto::RangeDataInserterOptions options_; const std::vector hit_table_; diff --git a/cartographer/mapping_2d/ray_casting.cc b/cartographer/mapping_2d/ray_casting.cc index 1923dea..fcada10 100644 --- a/cartographer/mapping_2d/ray_casting.cc +++ b/cartographer/mapping_2d/ray_casting.cc @@ -28,10 +28,11 @@ constexpr int kSubpixelScale = 1000; // and 'end' are coordinates at subpixel precision. We compute all pixels in // which some part of the line segment connecting 'begin' and 'end' lies. void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end, - const std::function& visitor) { + const std::vector& miss_table, + ProbabilityGrid* const probability_grid) { // For simplicity, we order 'begin' and 'end' by their x coordinate. if (begin.x() > end.x()) { - CastRay(end, begin, visitor); + CastRay(end, begin, miss_table, probability_grid); return; } @@ -46,7 +47,7 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end, std::min(begin.y(), end.y()) / kSubpixelScale); const int end_y = std::max(begin.y(), end.y()) / kSubpixelScale; for (; current.y() <= end_y; ++current.y()) { - visitor(current); + probability_grid->ApplyLookupTable(current, miss_table); } return; } @@ -86,11 +87,11 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end, sub_y += dy * first_pixel; if (dy > 0) { while (true) { - visitor(current); + probability_grid->ApplyLookupTable(current, miss_table); while (sub_y > denominator) { sub_y -= denominator; ++current.y(); - visitor(current); + probability_grid->ApplyLookupTable(current, miss_table); } ++current.x(); if (sub_y == denominator) { @@ -105,11 +106,11 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end, } // Move from the pixel border on the right to 'end'. sub_y += dy * last_pixel; - visitor(current); + probability_grid->ApplyLookupTable(current, miss_table); while (sub_y > denominator) { sub_y -= denominator; ++current.y(); - visitor(current); + probability_grid->ApplyLookupTable(current, miss_table); } CHECK_NE(sub_y, denominator); CHECK_EQ(current.y(), end.y() / kSubpixelScale); @@ -118,11 +119,11 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end, // Same for lines non-ascending in y coordinates. while (true) { - visitor(current); + probability_grid->ApplyLookupTable(current, miss_table); while (sub_y < 0) { sub_y += denominator; --current.y(); - visitor(current); + probability_grid->ApplyLookupTable(current, miss_table); } ++current.x(); if (sub_y == 0) { @@ -135,11 +136,11 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end, sub_y += dy * 2 * kSubpixelScale; } sub_y += dy * last_pixel; - visitor(current); + probability_grid->ApplyLookupTable(current, miss_table); while (sub_y < 0) { sub_y += denominator; --current.y(); - visitor(current); + probability_grid->ApplyLookupTable(current, miss_table); } CHECK_NE(sub_y, 0); CHECK_EQ(current.y(), end.y() / kSubpixelScale); @@ -147,9 +148,12 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end, } // namespace -void CastRays(const sensor::RangeData& range_data, const MapLimits& limits, - const std::function& hit_visitor, - const std::function& miss_visitor) { +void CastRays(const sensor::RangeData& range_data, + const std::vector& hit_table, + const std::vector& miss_table, + const bool insert_free_space, + ProbabilityGrid* const probability_grid) { + const MapLimits& limits = probability_grid->limits(); const double superscaled_resolution = limits.resolution() / kSubpixelScale; const MapLimits superscaled_limits( superscaled_resolution, limits.max(), @@ -158,19 +162,22 @@ void CastRays(const sensor::RangeData& range_data, const MapLimits& limits, const Eigen::Array2i begin = superscaled_limits.GetXYIndexOfCellContainingPoint(range_data.origin.x(), range_data.origin.y()); - // Compute and add the end points. std::vector ends; ends.reserve(range_data.returns.size()); for (const Eigen::Vector3f& hit : range_data.returns) { ends.push_back( superscaled_limits.GetXYIndexOfCellContainingPoint(hit.x(), hit.y())); - hit_visitor(ends.back() / kSubpixelScale); + probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table); + } + + if (!insert_free_space) { + return; } // Now add the misses. for (const Eigen::Array2i& end : ends) { - CastRay(begin, end, miss_visitor); + CastRay(begin, end, miss_table, probability_grid); } // Finally, compute and add empty rays based on misses in the scan. @@ -178,7 +185,7 @@ void CastRays(const sensor::RangeData& range_data, const MapLimits& limits, CastRay(begin, superscaled_limits.GetXYIndexOfCellContainingPoint( missing_echo.x(), missing_echo.y()), - miss_visitor); + miss_table, probability_grid); } } diff --git a/cartographer/mapping_2d/ray_casting.h b/cartographer/mapping_2d/ray_casting.h index b829ad1..e8a4482 100644 --- a/cartographer/mapping_2d/ray_casting.h +++ b/cartographer/mapping_2d/ray_casting.h @@ -17,10 +17,10 @@ #ifndef CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_ #define CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_ -#include +#include -#include "cartographer/mapping_2d/map_limits.h" -#include "cartographer/mapping_2d/xy_index.h" +#include "cartographer/common/port.h" +#include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/transform.h" @@ -28,11 +28,12 @@ namespace cartographer { namespace mapping_2d { -// For each ray in 'range_data', calls 'hit_visitor' and 'miss_visitor' on the -// appropriate cells. Hits are handled before misses. -void CastRays(const sensor::RangeData& range_data, const MapLimits& limits, - const std::function& hit_visitor, - const std::function& miss_visitor); +// For each ray in 'range_data', inserts hits and misses into +// 'probability_grid'. Hits are handled before misses. +void CastRays(const sensor::RangeData& range_data, + const std::vector& hit_table, + const std::vector& miss_table, bool insert_free_space, + ProbabilityGrid* probability_grid); } // namespace mapping_2d } // namespace cartographer