From 611f244d7ac269ff4a39811b7baa8f7b3bea2a29 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Wed, 5 Jul 2017 13:17:20 +0200 Subject: [PATCH] Remove HybridGrid slicing into a ProbabilityGrid. (#385) This code is currently unused. The feature of slicing a HybridGrid into a Probability grid could be useful to for example collect a map in 3D, then slice the 3D data to create a 2D map. There are simpler ways to implement this, especially in the context of the assets_writer_main. PAIR=wohe --- cartographer/mapping_3d/submaps.cc | 155 ----------------------------- cartographer/mapping_3d/submaps.h | 7 -- 2 files changed, 162 deletions(-) diff --git a/cartographer/mapping_3d/submaps.cc b/cartographer/mapping_3d/submaps.cc index 2485a19..c512448 100644 --- a/cartographer/mapping_3d/submaps.cc +++ b/cartographer/mapping_3d/submaps.cc @@ -28,14 +28,6 @@ namespace mapping_3d { namespace { -constexpr float kSliceHalfHeight = 0.1f; - -struct RaySegment { - Eigen::Vector2f from; - Eigen::Vector2f to; - bool hit; // Whether there is a hit at 'to'. -}; - struct PixelData { int min_z = INT_MAX; int max_z = INT_MIN; @@ -44,139 +36,6 @@ struct PixelData { float max_probability = 0.5f; }; -// We compute a slice around the xy-plane. 'transform' is applied to the rays in -// global map frame to allow choosing an arbitrary slice. -void GenerateSegmentForSlice(const sensor::RangeData& range_data, - const transform::Rigid3f& pose, - const transform::Rigid3f& transform, - std::vector* segments) { - const sensor::RangeData transformed_range_data = - sensor::TransformRangeData(range_data, transform * pose); - segments->reserve(transformed_range_data.returns.size()); - for (const Eigen::Vector3f& hit : transformed_range_data.returns) { - const Eigen::Vector2f origin_xy = transformed_range_data.origin.head<2>(); - const float origin_z = transformed_range_data.origin.z(); - const float delta_z = hit.z() - origin_z; - const Eigen::Vector2f delta_xy = hit.head<2>() - origin_xy; - if (origin_z < -kSliceHalfHeight) { - // Ray originates below the slice. - if (hit.z() > kSliceHalfHeight) { - // Ray is cutting through the slice. - segments->push_back(RaySegment{ - origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy, - origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy, - false}); - } else if (hit.z() > -kSliceHalfHeight) { - // Hit is inside the slice. - segments->push_back(RaySegment{ - origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy, - hit.head<2>(), true}); - } - } else if (origin_z < kSliceHalfHeight) { - // Ray originates inside the slice. - if (hit.z() < -kSliceHalfHeight) { - // Hit is below. - segments->push_back(RaySegment{ - origin_xy, - origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy, - false}); - } else if (hit.z() < kSliceHalfHeight) { - // Full ray is inside the slice. - segments->push_back(RaySegment{origin_xy, hit.head<2>(), true}); - } else { - // Hit is above. - segments->push_back(RaySegment{ - origin_xy, - origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy, - false}); - } - } else { - // Ray originates above the slice. - if (hit.z() < -kSliceHalfHeight) { - // Ray is cutting through the slice. - segments->push_back(RaySegment{ - origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy, - origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy, - false}); - } else if (hit.z() < kSliceHalfHeight) { - // Hit is inside the slice. - segments->push_back(RaySegment{ - origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy, - hit.head<2>(), true}); - } - } - } -} - -void UpdateFreeSpaceFromSegment(const RaySegment& segment, - const std::vector& miss_table, - mapping_2d::ProbabilityGrid* result) { - Eigen::Array2i from = result->limits().GetXYIndexOfCellContainingPoint( - segment.from.x(), segment.from.y()); - Eigen::Array2i to = result->limits().GetXYIndexOfCellContainingPoint( - segment.to.x(), segment.to.y()); - bool large_delta_y = - std::abs(to.y() - from.y()) > std::abs(to.x() - from.x()); - if (large_delta_y) { - std::swap(from.x(), from.y()); - std::swap(to.x(), to.y()); - } - if (from.x() > to.x()) { - std::swap(from, to); - } - const int dx = to.x() - from.x(); - const int dy = std::abs(to.y() - from.y()); - int error = dx / 2; - const int direction = (from.y() < to.y()) ? 1 : -1; - - for (; from.x() < to.x(); ++from.x()) { - if (large_delta_y) { - result->ApplyLookupTable(Eigen::Array2i(from.y(), from.x()), miss_table); - } else { - result->ApplyLookupTable(from, miss_table); - } - error -= dy; - if (error < 0) { - from.y() += direction; - error += dx; - } - } -} - -void InsertSegmentsIntoProbabilityGrid(const std::vector& segments, - const std::vector& hit_table, - const std::vector& miss_table, - mapping_2d::ProbabilityGrid* result) { - result->StartUpdate(); - if (segments.empty()) { - return; - } - Eigen::Vector2f min = segments.front().from; - Eigen::Vector2f max = min; - for (const RaySegment& segment : segments) { - min = min.cwiseMin(segment.from); - min = min.cwiseMin(segment.to); - max = max.cwiseMax(segment.from); - max = max.cwiseMax(segment.to); - } - const float padding = 10. * result->limits().resolution(); - max += Eigen::Vector2f(padding, padding); - min -= Eigen::Vector2f(padding, padding); - result->GrowLimits(min.x(), min.y()); - result->GrowLimits(max.x(), max.y()); - - for (const RaySegment& segment : segments) { - if (segment.hit) { - result->ApplyLookupTable(result->limits().GetXYIndexOfCellContainingPoint( - segment.to.x(), segment.to.y()), - hit_table); - } - } - for (const RaySegment& segment : segments) { - UpdateFreeSpaceFromSegment(segment, miss_table, result); - } -} - // Filters 'range_data', retaining only the returns that have no more than // 'max_range' distance from the origin. Removes misses and reflectivity // information. @@ -289,20 +148,6 @@ string ComputePixelValues( } // namespace -void InsertIntoProbabilityGrid( - const sensor::RangeData& range_data, const transform::Rigid3f& pose, - const float slice_z, - const mapping_2d::RangeDataInserter& range_data_inserter, - mapping_2d::ProbabilityGrid* result) { - std::vector segments; - GenerateSegmentForSlice( - range_data, pose, - transform::Rigid3f::Translation(-slice_z * Eigen::Vector3f::UnitZ()), - &segments); - InsertSegmentsIntoProbabilityGrid(segments, range_data_inserter.hit_table(), - range_data_inserter.miss_table(), result); -} - proto::SubmapsOptions CreateSubmapsOptions( common::LuaParameterDictionary* parameter_dictionary) { proto::SubmapsOptions options; diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submaps.h index 2d4a9e2..a18b1ec 100644 --- a/cartographer/mapping_3d/submaps.h +++ b/cartographer/mapping_3d/submaps.h @@ -27,7 +27,6 @@ #include "cartographer/mapping/proto/serialization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/submaps.h" -#include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/range_data_inserter.h" #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/proto/submaps_options.pb.h" @@ -39,12 +38,6 @@ namespace cartographer { namespace mapping_3d { -void InsertIntoProbabilityGrid( - const sensor::RangeData& range_data, const transform::Rigid3f& pose, - const float slice_z, - const mapping_2d::RangeDataInserter& range_data_inserter, - mapping_2d::ProbabilityGrid* result); - proto::SubmapsOptions CreateSubmapsOptions( common::LuaParameterDictionary* parameter_dictionary);