From d29153a7443132c514dd84f447bbb572851fd174 Mon Sep 17 00:00:00 2001 From: Kevin Daun Date: Fri, 20 Apr 2018 22:58:46 +0200 Subject: [PATCH] Replace ProbabilityGrid in Submap2D by Grid2D (#1097) --- cartographer/mapping/2d/grid_2d.h | 7 ++ cartographer/mapping/2d/probability_grid.cc | 62 ++++++++++++++ cartographer/mapping/2d/probability_grid.h | 4 + .../mapping/2d/range_data_inserter_2d.cc | 9 +-- .../mapping/2d/range_data_inserter_2d.h | 3 +- cartographer/mapping/2d/submap_2d.cc | 80 +++---------------- cartographer/mapping/2d/submap_2d.h | 9 +-- cartographer/mapping/2d/submap_2d_test.cc | 12 +-- .../2d/local_trajectory_builder_2d.cc | 8 +- .../2d/overlapping_submaps_trimmer_2d.cc | 11 ++- .../2d/pose_graph/constraint_builder_2d.cc | 4 +- .../2d/scan_matching/ceres_scan_matcher_2d.h | 2 +- .../fast_correlative_scan_matcher_2d.h | 2 +- 13 files changed, 112 insertions(+), 101 deletions(-) diff --git a/cartographer/mapping/2d/grid_2d.h b/cartographer/mapping/2d/grid_2d.h index 530ffef..d8a3bdf 100644 --- a/cartographer/mapping/2d/grid_2d.h +++ b/cartographer/mapping/2d/grid_2d.h @@ -21,6 +21,7 @@ #include "cartographer/mapping/2d/map_limits.h" #include "cartographer/mapping/2d/proto/grid_2d.pb.h" +#include "cartographer/mapping/proto/submap_visualization.pb.h" namespace cartographer { namespace mapping { @@ -58,8 +59,14 @@ class Grid2D { // after 'FinishUpdate', before any calls to 'ApplyLookupTable'. void GrowLimits(const Eigen::Vector2f& point); + virtual std::unique_ptr ComputeCroppedGrid() const = 0; + virtual proto::Grid2D ToProto() const; + virtual bool DrawToSubmapTexture( + proto::SubmapQuery::Response::SubmapTexture* const texture, + transform::Rigid3d local_pose) const = 0; + protected: const std::vector& correspondence_cost_cells() const { return correspondence_cost_cells_; diff --git a/cartographer/mapping/2d/probability_grid.cc b/cartographer/mapping/2d/probability_grid.cc index 448eb31..05a9aca 100644 --- a/cartographer/mapping/2d/probability_grid.cc +++ b/cartographer/mapping/2d/probability_grid.cc @@ -17,7 +17,9 @@ #include +#include "cartographer/common/make_unique.h" #include "cartographer/mapping/probability_values.h" +#include "cartographer/mapping/submaps.h" namespace cartographer { namespace mapping { @@ -76,5 +78,65 @@ proto::Grid2D ProbabilityGrid::ToProto() const { return result; } +std::unique_ptr ProbabilityGrid::ComputeCroppedGrid() const { + Eigen::Array2i offset; + CellLimits cell_limits; + ComputeCroppedLimits(&offset, &cell_limits); + const double resolution = limits().resolution(); + const Eigen::Vector2d max = + limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x()); + std::unique_ptr cropped_grid = + common::make_unique( + MapLimits(resolution, max, cell_limits)); + for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) { + if (!IsKnown(xy_index + offset)) continue; + cropped_grid->SetProbability(xy_index, GetProbability(xy_index + offset)); + } + + return std::unique_ptr(cropped_grid.release()); +} + +bool ProbabilityGrid::DrawToSubmapTexture( + proto::SubmapQuery::Response::SubmapTexture* const texture, + transform::Rigid3d local_pose) const { + Eigen::Array2i offset; + CellLimits cell_limits; + ComputeCroppedLimits(&offset, &cell_limits); + + std::string cells; + for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) { + if (!IsKnown(xy_index + offset)) { + cells.push_back(0 /* unknown log odds value */); + cells.push_back(0 /* alpha */); + continue; + } + // We would like to add 'delta' but this is not possible using a value and + // alpha. We use premultiplied alpha, so when 'delta' is positive we can + // add it by setting 'alpha' to zero. If it is negative, we set 'value' to + // zero, and use 'alpha' to subtract. This is only correct when the pixel + // is currently white, so walls will look too gray. This should be hard to + // detect visually for the user, though. + const int delta = + 128 - ProbabilityToLogOddsInteger(GetProbability(xy_index + offset)); + const uint8 alpha = delta > 0 ? 0 : -delta; + const uint8 value = delta > 0 ? delta : 0; + cells.push_back(value); + cells.push_back((value || alpha) ? alpha : 1); + } + + common::FastGzipString(cells, texture->mutable_cells()); + texture->set_width(cell_limits.num_x_cells); + texture->set_height(cell_limits.num_y_cells); + const double resolution = limits().resolution(); + texture->set_resolution(resolution); + const double max_x = limits().max().x() - resolution * offset.y(); + const double max_y = limits().max().y() - resolution * offset.x(); + *texture->mutable_slice_pose() = transform::ToProto( + local_pose.inverse() * + transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.))); + + return true; +} + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/2d/probability_grid.h b/cartographer/mapping/2d/probability_grid.h index 1764e57..540f47e 100644 --- a/cartographer/mapping/2d/probability_grid.h +++ b/cartographer/mapping/2d/probability_grid.h @@ -52,6 +52,10 @@ class ProbabilityGrid : public Grid2D { float GetProbability(const Eigen::Array2i& cell_index) const; virtual proto::Grid2D ToProto() const override; + virtual std::unique_ptr ComputeCroppedGrid() const override; + virtual bool DrawToSubmapTexture( + proto::SubmapQuery::Response::SubmapTexture* const texture, + transform::Rigid3d local_pose) const override; }; } // namespace mapping diff --git a/cartographer/mapping/2d/range_data_inserter_2d.cc b/cartographer/mapping/2d/range_data_inserter_2d.cc index aa85884..54817c8 100644 --- a/cartographer/mapping/2d/range_data_inserter_2d.cc +++ b/cartographer/mapping/2d/range_data_inserter_2d.cc @@ -52,14 +52,13 @@ RangeDataInserter2D::RangeDataInserter2D( miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds( Odds(options.miss_probability()))) {} -void RangeDataInserter2D::Insert( - const sensor::RangeData& range_data, - ProbabilityGrid* const probability_grid) const { +void RangeDataInserter2D::Insert(const sensor::RangeData& range_data, + Grid2D* const grid) const { // By not finishing the 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, hit_table_, miss_table_, options_.insert_free_space(), - CHECK_NOTNULL(probability_grid)); - probability_grid->FinishUpdate(); + CHECK_NOTNULL(static_cast(grid))); + grid->FinishUpdate(); } } // namespace mapping diff --git a/cartographer/mapping/2d/range_data_inserter_2d.h b/cartographer/mapping/2d/range_data_inserter_2d.h index ff6e0eb..a905b40 100644 --- a/cartographer/mapping/2d/range_data_inserter_2d.h +++ b/cartographer/mapping/2d/range_data_inserter_2d.h @@ -43,8 +43,7 @@ class RangeDataInserter2D { RangeDataInserter2D& operator=(const RangeDataInserter2D&) = delete; // Inserts 'range_data' into 'probability_grid'. - void Insert(const sensor::RangeData& range_data, - ProbabilityGrid* probability_grid) const; + void Insert(const sensor::RangeData& range_data, Grid2D* grid) const; private: const proto::RangeDataInserterOptions2D options_; diff --git a/cartographer/mapping/2d/submap_2d.cc b/cartographer/mapping/2d/submap_2d.cc index a7faa37..2763e53 100644 --- a/cartographer/mapping/2d/submap_2d.cc +++ b/cartographer/mapping/2d/submap_2d.cc @@ -30,25 +30,6 @@ namespace cartographer { namespace mapping { -ProbabilityGrid ComputeCroppedProbabilityGrid( - const ProbabilityGrid& probability_grid) { - Eigen::Array2i offset; - CellLimits limits; - probability_grid.ComputeCroppedLimits(&offset, &limits); - const double resolution = probability_grid.limits().resolution(); - const Eigen::Vector2d max = - probability_grid.limits().max() - - resolution * Eigen::Vector2d(offset.y(), offset.x()); - ProbabilityGrid cropped_grid(MapLimits(resolution, max, limits)); - for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) { - if (probability_grid.IsKnown(xy_index + offset)) { - cropped_grid.SetProbability( - xy_index, probability_grid.GetProbability(xy_index + offset)); - } - } - return cropped_grid; -} - proto::SubmapsOptions2D CreateSubmapsOptions2D( common::LuaParameterDictionary* const parameter_dictionary) { proto::SubmapsOptions2D options; @@ -65,13 +46,13 @@ proto::SubmapsOptions2D CreateSubmapsOptions2D( Submap2D::Submap2D(const MapLimits& limits, const Eigen::Vector2f& origin) : Submap(transform::Rigid3d::Translation( Eigen::Vector3d(origin.x(), origin.y(), 0.))), - probability_grid_(common::make_unique(limits)) {} + grid_(common::make_unique(limits)) {} Submap2D::Submap2D(const proto::Submap2D& proto) : Submap(transform::ToRigid3(proto.local_pose())) { if (proto.has_grid()) { CHECK(proto.grid().has_probability_grid_2d()); - probability_grid_ = common::make_unique(proto.grid()); + grid_ = common::make_unique(proto.grid()); } set_num_range_data(proto.num_range_data()); set_finished(proto.finished()); @@ -84,8 +65,8 @@ void Submap2D::ToProto(proto::Submap* const proto, submap_2d->set_num_range_data(num_range_data()); submap_2d->set_finished(finished()); if (include_probability_grid_data) { - CHECK(probability_grid_); - *submap_2d->mutable_grid() = probability_grid_->ToProto(); + CHECK(grid_); + *submap_2d->mutable_grid() = grid_->ToProto(); } } @@ -96,71 +77,32 @@ void Submap2D::UpdateFromProto(const proto::Submap& proto) { set_finished(submap_2d.finished()); if (proto.submap_2d().has_grid()) { CHECK(proto.submap_2d().grid().has_probability_grid_2d()); - probability_grid_ = common::make_unique(submap_2d.grid()); + grid_ = common::make_unique(submap_2d.grid()); } } void Submap2D::ToResponseProto( const transform::Rigid3d&, proto::SubmapQuery::Response* const response) const { - if (!probability_grid_) return; + if (!grid_) return; response->set_submap_version(num_range_data()); - - Eigen::Array2i offset; - CellLimits limits; - probability_grid_->ComputeCroppedLimits(&offset, &limits); - - std::string cells; - for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) { - if (probability_grid_->IsKnown(xy_index + offset)) { - // We would like to add 'delta' but this is not possible using a value and - // alpha. We use premultiplied alpha, so when 'delta' is positive we can - // add it by setting 'alpha' to zero. If it is negative, we set 'value' to - // zero, and use 'alpha' to subtract. This is only correct when the pixel - // is currently white, so walls will look too gray. This should be hard to - // detect visually for the user, though. - const int delta = - 128 - ProbabilityToLogOddsInteger( - probability_grid_->GetProbability(xy_index + offset)); - const uint8 alpha = delta > 0 ? 0 : -delta; - const uint8 value = delta > 0 ? delta : 0; - cells.push_back(value); - cells.push_back((value || alpha) ? alpha : 1); - } else { - constexpr uint8 kUnknownLogOdds = 0; - cells.push_back(static_cast(kUnknownLogOdds)); // value - cells.push_back(0); // alpha - } - } proto::SubmapQuery::Response::SubmapTexture* const texture = response->add_textures(); - common::FastGzipString(cells, texture->mutable_cells()); - - texture->set_width(limits.num_x_cells); - texture->set_height(limits.num_y_cells); - const double resolution = probability_grid_->limits().resolution(); - texture->set_resolution(resolution); - const double max_x = - probability_grid_->limits().max().x() - resolution * offset.y(); - const double max_y = - probability_grid_->limits().max().y() - resolution * offset.x(); - *texture->mutable_slice_pose() = transform::ToProto( - local_pose().inverse() * - transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.))); + grid()->DrawToSubmapTexture(texture, local_pose()); } void Submap2D::InsertRangeData(const sensor::RangeData& range_data, const RangeDataInserter2D& range_data_inserter) { - CHECK(probability_grid_); + CHECK(grid_); CHECK(!finished()); - range_data_inserter.Insert(range_data, probability_grid_.get()); + range_data_inserter.Insert(range_data, grid_.get()); set_num_range_data(num_range_data() + 1); } void Submap2D::Finish() { - CHECK(probability_grid_); + CHECK(grid_); CHECK(!finished()); - *probability_grid_ = ComputeCroppedProbabilityGrid(*probability_grid_); + grid_ = grid_->ComputeCroppedGrid(); set_finished(true); } diff --git a/cartographer/mapping/2d/submap_2d.h b/cartographer/mapping/2d/submap_2d.h index 7772e0a..bfb3287 100644 --- a/cartographer/mapping/2d/submap_2d.h +++ b/cartographer/mapping/2d/submap_2d.h @@ -22,8 +22,8 @@ #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/2d/grid_2d.h" #include "cartographer/mapping/2d/map_limits.h" -#include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/proto/submaps_options_2d.pb.h" #include "cartographer/mapping/2d/range_data_inserter_2d.h" #include "cartographer/mapping/proto/serialization.pb.h" @@ -36,9 +36,6 @@ namespace cartographer { namespace mapping { -ProbabilityGrid ComputeCroppedProbabilityGrid( - const ProbabilityGrid& probability_grid); - proto::SubmapsOptions2D CreateSubmapsOptions2D( common::LuaParameterDictionary* parameter_dictionary); @@ -54,7 +51,7 @@ class Submap2D : public Submap { void ToResponseProto(const transform::Rigid3d& global_submap_pose, proto::SubmapQuery::Response* response) const override; - const ProbabilityGrid& probability_grid() const { return *probability_grid_; } + const Grid2D* grid() const { return grid_.get(); } // Insert 'range_data' into this submap using 'range_data_inserter'. The // submap must not be finished yet. @@ -63,7 +60,7 @@ class Submap2D : public Submap { void Finish(); private: - std::unique_ptr probability_grid_; + std::unique_ptr grid_; }; // Except during initialization when only a single submap exists, there are diff --git a/cartographer/mapping/2d/submap_2d_test.cc b/cartographer/mapping/2d/submap_2d_test.cc index 64380c1..eed665a 100644 --- a/cartographer/mapping/2d/submap_2d_test.cc +++ b/cartographer/mapping/2d/submap_2d_test.cc @@ -83,12 +83,12 @@ TEST(Submap2DTest, ToFromProto) { actual.local_pose().rotation(), 1e-6)); EXPECT_EQ(expected.num_range_data(), actual.num_range_data()); EXPECT_EQ(expected.finished(), actual.finished()); - EXPECT_NEAR(expected.probability_grid().limits().resolution(), - actual.probability_grid().limits().resolution(), 1e-6); - EXPECT_TRUE(expected.probability_grid().limits().max().isApprox( - actual.probability_grid().limits().max(), 1e-6)); - EXPECT_EQ(expected.probability_grid().limits().cell_limits().num_x_cells, - actual.probability_grid().limits().cell_limits().num_x_cells); + EXPECT_NEAR(expected.grid()->limits().resolution(), + actual.grid()->limits().resolution(), 1e-6); + EXPECT_TRUE(expected.grid()->limits().max().isApprox( + actual.grid()->limits().max(), 1e-6)); + EXPECT_EQ(expected.grid()->limits().cell_limits().num_x_cells, + actual.grid()->limits().cell_limits().num_x_cells); } } // namespace diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc index 81fef4f..30de6d8 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc @@ -76,9 +76,11 @@ std::unique_ptr LocalTrajectoryBuilder2D::ScanMatch( return nullptr; } if (options_.use_online_correlative_scan_matching()) { + // todo(kdaun) add CHECK on options to guarantee grid is a probability grid double score = real_time_correlative_scan_matcher_.Match( pose_prediction, filtered_gravity_aligned_point_cloud, - matching_submap->probability_grid(), &initial_ceres_pose); + *static_cast(matching_submap->grid()), + &initial_ceres_pose); kFastCorrelativeScanMatcherScoreMetric->Observe(score); } @@ -86,8 +88,8 @@ std::unique_ptr LocalTrajectoryBuilder2D::ScanMatch( ceres::Solver::Summary summary; ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose, filtered_gravity_aligned_point_cloud, - matching_submap->probability_grid(), - pose_observation.get(), &summary); + *matching_submap->grid(), pose_observation.get(), + &summary); if (pose_observation) { kCeresScanMatcherCostMetric->Observe(summary.final_cost); double residual_distance = diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc index 883d0f8..11c8e5e 100644 --- a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc +++ b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc @@ -51,7 +51,7 @@ Eigen::Vector2d GetCornerOfFirstSubmap( const MapById& submap_data) { auto submap_2d = std::static_pointer_cast( submap_data.begin()->data.submap); - return submap_2d->probability_grid().limits().max(); + return submap_2d->grid()->limits().max(); } // Iterates over every cell in a submap, transforms the center of the cell to @@ -68,13 +68,12 @@ std::set AddSubmapsToSubmapCoverageGrid2D( if (freshness == submap_freshness.end()) continue; if (!submap.data.submap->finished()) continue; all_submap_ids.insert(submap.id); - const ProbabilityGrid& probability_grid = - std::static_pointer_cast(submap.data.submap) - ->probability_grid(); + const Grid2D& grid = + *std::static_pointer_cast(submap.data.submap)->grid(); // Iterate over every cell in a submap. Eigen::Array2i offset; CellLimits cell_limits; - probability_grid.ComputeCroppedLimits(&offset, &cell_limits); + grid.ComputeCroppedLimits(&offset, &cell_limits); if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) { LOG(WARNING) << "Empty grid found in submap ID = " << submap.id; continue; @@ -83,7 +82,7 @@ std::set AddSubmapsToSubmapCoverageGrid2D( transform::Project2D(submap.data.pose); for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) { const Eigen::Array2i index = xy_index + offset; - if (!probability_grid.IsKnown(index)) continue; + if (!grid.IsKnown(index)) continue; const transform::Rigid2d center_of_cell_in_global_frame = projected_submap_pose * transform::Rigid2d::Translation({index.x() + 0.5, index.y() + 0.5}); diff --git a/cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.cc b/cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.cc index e7df860..57b6ead 100644 --- a/cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.cc @@ -85,7 +85,7 @@ void ConstraintBuilder2D::MaybeAddConstraint( ++pending_computations_[current_computation_]; const int current_computation = current_computation_; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( - submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) { + submap_id, submap->grid(), [=]() EXCLUDES(mutex_) { ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */ constant_data, initial_relative_pose, constraint); @@ -104,7 +104,7 @@ void ConstraintBuilder2D::MaybeAddGlobalConstraint( ++pending_computations_[current_computation_]; const int current_computation = current_computation_; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( - submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) { + submap_id, submap->grid(), [=]() EXCLUDES(mutex_) { ComputeConstraint( submap_id, submap, node_id, true, /* match_full_submap */ constant_data, transform::Rigid2d::Identity(), constraint); diff --git a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h index d3f5e35..fcd83e6 100644 --- a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h +++ b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h @@ -43,7 +43,7 @@ class CeresScanMatcher2D { CeresScanMatcher2D(const CeresScanMatcher2D&) = delete; CeresScanMatcher2D& operator=(const CeresScanMatcher2D&) = delete; - // Aligns 'point_cloud' within the 'probability_grid' given an + // Aligns 'point_cloud' within the 'grid' given an // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver // 'summary'. void Match(const Eigen::Vector2d& target_translation, diff --git a/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h b/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h index a625df5..d6fb855 100644 --- a/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h +++ b/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h @@ -78,7 +78,7 @@ class PrecomputationGrid2D { private: uint8 ComputeCellValue(float probability) const; - // Offset of the precomputation grid in relation to the 'probability_grid' + // Offset of the precomputation grid in relation to the 'grid' // including the additional 'width' - 1 cells. const Eigen::Array2i offset_;