diff --git a/cartographer/io/probability_grid_points_processor.cc b/cartographer/io/probability_grid_points_processor.cc index 358ef74..f404fb2 100644 --- a/cartographer/io/probability_grid_points_processor.cc +++ b/cartographer/io/probability_grid_points_processor.cc @@ -57,8 +57,8 @@ uint8 ProbabilityToColor(float probability_from_grid) { ProbabilityGridPointsProcessor::ProbabilityGridPointsProcessor( const double resolution, - const mapping::proto::RangeDataInserterOptions2D& - range_data_inserter_options, + const mapping::proto::ProbabilityGridRangeDataInserterOptions2D& + probability_grid_range_data_inserter_options, const DrawTrajectories& draw_trajectories, std::unique_ptr file_writer, const std::vector& trajectories, @@ -67,7 +67,7 @@ ProbabilityGridPointsProcessor::ProbabilityGridPointsProcessor( trajectories_(trajectories), file_writer_(std::move(file_writer)), next_(next), - range_data_inserter_(range_data_inserter_options), + range_data_inserter_(probability_grid_range_data_inserter_options), probability_grid_(CreateProbabilityGrid(resolution)) {} std::unique_ptr @@ -82,7 +82,7 @@ ProbabilityGridPointsProcessor::FromDictionary( : DrawTrajectories::kNo; return common::make_unique( dictionary->GetDouble("resolution"), - mapping::CreateRangeDataInserterOptions2D( + mapping::CreateProbabilityGridRangeDataInserterOptions2D( dictionary->GetDictionary("range_data_inserter").get()), draw_trajectories, file_writer_factory(dictionary->GetString("filename") + ".png"), diff --git a/cartographer/io/probability_grid_points_processor.h b/cartographer/io/probability_grid_points_processor.h index 3c0cd53..4d94736 100644 --- a/cartographer/io/probability_grid_points_processor.h +++ b/cartographer/io/probability_grid_points_processor.h @@ -24,8 +24,8 @@ #include "cartographer/io/points_batch.h" #include "cartographer/io/points_processor.h" #include "cartographer/mapping/2d/probability_grid.h" -#include "cartographer/mapping/2d/proto/range_data_inserter_options_2d.pb.h" -#include "cartographer/mapping/2d/range_data_inserter_2d.h" +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" +#include "cartographer/mapping/2d/proto/probability_grid_range_data_inserter_options_2d.pb.h" #include "cartographer/mapping/proto/trajectory.pb.h" namespace cartographer { @@ -42,8 +42,8 @@ class ProbabilityGridPointsProcessor : public PointsProcessor { enum class DrawTrajectories { kNo, kYes }; ProbabilityGridPointsProcessor( double resolution, - const mapping::proto::RangeDataInserterOptions2D& - range_data_inserter_options, + const mapping::proto::ProbabilityGridRangeDataInserterOptions2D& + probability_grid_range_data_inserter_options, const DrawTrajectories& draw_trajectories, std::unique_ptr file_writer, const std::vector& trajectorios, @@ -68,7 +68,7 @@ class ProbabilityGridPointsProcessor : public PointsProcessor { const std::vector trajectories_; std::unique_ptr file_writer_; PointsProcessor* const next_; - mapping::RangeDataInserter2D range_data_inserter_; + mapping::ProbabilityGridRangeDataInserter2D range_data_inserter_; mapping::ProbabilityGrid probability_grid_; }; diff --git a/cartographer/mapping/2d/grid_2d.cc b/cartographer/mapping/2d/grid_2d.cc index ed9870d..43221df 100644 --- a/cartographer/mapping/2d/grid_2d.cc +++ b/cartographer/mapping/2d/grid_2d.cc @@ -20,6 +20,19 @@ namespace cartographer { namespace mapping { +proto::GridOptions2D CreateGridOptions2D( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::GridOptions2D options; + const std::string grid_type_string = + parameter_dictionary->GetString("grid_type"); + proto::GridOptions2D_GridType grid_type; + CHECK(proto::GridOptions2D_GridType_Parse(grid_type_string, &grid_type)) + << "Unknown GridOptions2D_GridType kind: " << grid_type_string; + options.set_grid_type(grid_type); + options.set_resolution(parameter_dictionary->GetDouble("resolution")); + return options; +} + Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost, float max_correspondence_cost) : limits_(limits), diff --git a/cartographer/mapping/2d/grid_2d.h b/cartographer/mapping/2d/grid_2d.h index d8a3bdf..1f8c590 100644 --- a/cartographer/mapping/2d/grid_2d.h +++ b/cartographer/mapping/2d/grid_2d.h @@ -21,12 +21,17 @@ #include "cartographer/mapping/2d/map_limits.h" #include "cartographer/mapping/2d/proto/grid_2d.pb.h" +#include "cartographer/mapping/2d/proto/submaps_options_2d.pb.h" +#include "cartographer/mapping/grid_interface.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" namespace cartographer { namespace mapping { -class Grid2D { +proto::GridOptions2D CreateGridOptions2D( + common::LuaParameterDictionary* const parameter_dictionary); + +class Grid2D : public GridInterface { public: explicit Grid2D(const MapLimits& limits, float min_correspondence_cost, float max_correspondence_cost); @@ -57,7 +62,7 @@ class Grid2D { // Grows the map as necessary to include 'point'. This changes the meaning of // these coordinates going forward. This method must be called immediately // after 'FinishUpdate', before any calls to 'ApplyLookupTable'. - void GrowLimits(const Eigen::Vector2f& point); + virtual void GrowLimits(const Eigen::Vector2f& point); virtual std::unique_ptr ComputeCroppedGrid() const = 0; diff --git a/cartographer/mapping/2d/probability_grid.cc b/cartographer/mapping/2d/probability_grid.cc index 05a9aca..82761d4 100644 --- a/cartographer/mapping/2d/probability_grid.cc +++ b/cartographer/mapping/2d/probability_grid.cc @@ -71,6 +71,7 @@ float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const { return CorrespondenceCostToProbability(ValueToCorrespondenceCost( correspondence_cost_cells()[ToFlatIndex(cell_index)])); } + proto::Grid2D ProbabilityGrid::ToProto() const { proto::Grid2D result; result = Grid2D::ToProto(); diff --git a/cartographer/mapping/2d/range_data_inserter_2d.cc b/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc similarity index 72% rename from cartographer/mapping/2d/range_data_inserter_2d.cc rename to cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc index 54817c8..f94ce4e 100644 --- a/cartographer/mapping/2d/range_data_inserter_2d.cc +++ b/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/2d/range_data_inserter_2d.h" +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" #include @@ -28,9 +28,10 @@ namespace cartographer { namespace mapping { -proto::RangeDataInserterOptions2D CreateRangeDataInserterOptions2D( - common::LuaParameterDictionary* const parameter_dictionary) { - proto::RangeDataInserterOptions2D options; +proto::ProbabilityGridRangeDataInserterOptions2D +CreateProbabilityGridRangeDataInserterOptions2D( + common::LuaParameterDictionary* parameter_dictionary) { + proto::ProbabilityGridRangeDataInserterOptions2D options; options.set_hit_probability( parameter_dictionary->GetDouble("hit_probability")); options.set_miss_probability( @@ -44,21 +45,22 @@ proto::RangeDataInserterOptions2D CreateRangeDataInserterOptions2D( return options; } -RangeDataInserter2D::RangeDataInserter2D( - const proto::RangeDataInserterOptions2D& options) +ProbabilityGridRangeDataInserter2D::ProbabilityGridRangeDataInserter2D( + const proto::ProbabilityGridRangeDataInserterOptions2D& options) : options_(options), hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds( Odds(options.hit_probability()))), miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds( Odds(options.miss_probability()))) {} -void RangeDataInserter2D::Insert(const sensor::RangeData& range_data, - Grid2D* const grid) const { +void ProbabilityGridRangeDataInserter2D::Insert( + const sensor::RangeData& range_data, GridInterface* const grid) const { + ProbabilityGrid* const probability_grid = static_cast(grid); // 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(static_cast(grid))); - grid->FinishUpdate(); + CHECK_NOTNULL(probability_grid)); + probability_grid->FinishUpdate(); } } // namespace mapping diff --git a/cartographer/mapping/2d/range_data_inserter_2d.h b/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h similarity index 53% rename from cartographer/mapping/2d/range_data_inserter_2d.h rename to cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h index a905b40..756ce1c 100644 --- a/cartographer/mapping/2d/range_data_inserter_2d.h +++ b/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_H_ -#define CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_H_ +#ifndef CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_PROBABILITY_GRID_H_ +#define CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_PROBABILITY_GRID_H_ #include #include @@ -23,30 +23,35 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/port.h" #include "cartographer/mapping/2d/probability_grid.h" -#include "cartographer/mapping/2d/proto/range_data_inserter_options_2d.pb.h" +#include "cartographer/mapping/2d/proto/probability_grid_range_data_inserter_options_2d.pb.h" #include "cartographer/mapping/2d/xy_index.h" +#include "cartographer/mapping/range_data_inserter_interface.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" namespace cartographer { namespace mapping { -proto::RangeDataInserterOptions2D CreateRangeDataInserterOptions2D( +proto::ProbabilityGridRangeDataInserterOptions2D +CreateProbabilityGridRangeDataInserterOptions2D( common::LuaParameterDictionary* parameter_dictionary); -class RangeDataInserter2D { +class ProbabilityGridRangeDataInserter2D : public RangeDataInserterInterface { public: - explicit RangeDataInserter2D( - const proto::RangeDataInserterOptions2D& options); + explicit ProbabilityGridRangeDataInserter2D( + const proto::ProbabilityGridRangeDataInserterOptions2D& options); - RangeDataInserter2D(const RangeDataInserter2D&) = delete; - RangeDataInserter2D& operator=(const RangeDataInserter2D&) = delete; + ProbabilityGridRangeDataInserter2D( + const ProbabilityGridRangeDataInserter2D&) = delete; + ProbabilityGridRangeDataInserter2D& operator=( + const ProbabilityGridRangeDataInserter2D&) = delete; // Inserts 'range_data' into 'probability_grid'. - void Insert(const sensor::RangeData& range_data, Grid2D* grid) const; + virtual void Insert(const sensor::RangeData& range_data, + GridInterface* grid) const override; private: - const proto::RangeDataInserterOptions2D options_; + const proto::ProbabilityGridRangeDataInserterOptions2D options_; const std::vector hit_table_; const std::vector miss_table_; }; @@ -54,4 +59,4 @@ class RangeDataInserter2D { } // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_H_ +#endif // CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_2D_PROBABILITY_GRID_H_ diff --git a/cartographer/mapping/2d/proto/grid_2d_options.proto b/cartographer/mapping/2d/proto/grid_2d_options.proto new file mode 100644 index 0000000..5040d79 --- /dev/null +++ b/cartographer/mapping/2d/proto/grid_2d_options.proto @@ -0,0 +1,27 @@ +// Copyright 2018 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +message GridOptions2D { + enum GridType { + INVALID_GRID = 0; + PROBABILITY_GRID = 1; + } + + GridType grid_type = 1; + float resolution = 2; +} diff --git a/cartographer/mapping/2d/proto/range_data_inserter_options_2d.proto b/cartographer/mapping/2d/proto/probability_grid_range_data_inserter_options_2d.proto similarity index 95% rename from cartographer/mapping/2d/proto/range_data_inserter_options_2d.proto rename to cartographer/mapping/2d/proto/probability_grid_range_data_inserter_options_2d.proto index 67c6288..c5140ec 100644 --- a/cartographer/mapping/2d/proto/range_data_inserter_options_2d.proto +++ b/cartographer/mapping/2d/proto/probability_grid_range_data_inserter_options_2d.proto @@ -16,7 +16,7 @@ syntax = "proto3"; package cartographer.mapping.proto; -message RangeDataInserterOptions2D { +message ProbabilityGridRangeDataInserterOptions2D { // Probability change for a hit (this will be converted to odds and therefore // must be greater than 0.5). double hit_probability = 1; diff --git a/cartographer/mapping/2d/proto/submaps_options_2d.proto b/cartographer/mapping/2d/proto/submaps_options_2d.proto index 96f11bd..7826716 100644 --- a/cartographer/mapping/2d/proto/submaps_options_2d.proto +++ b/cartographer/mapping/2d/proto/submaps_options_2d.proto @@ -14,18 +14,17 @@ syntax = "proto3"; -import "cartographer/mapping/2d/proto/range_data_inserter_options_2d.proto"; +import "cartographer/mapping/2d/proto/grid_2d_options.proto"; +import "cartographer/mapping/2d/proto/probability_grid_range_data_inserter_options_2d.proto"; +import "cartographer/mapping/proto/range_data_inserter_options.proto"; package cartographer.mapping.proto; message SubmapsOptions2D { - // Resolution of the map in meters. - double resolution = 1; - // Number of range data before adding a new submap. Each submap will get twice // the number of range data inserted: First for initialization without being // matched against, then while being matched. - int32 num_range_data = 3; - - RangeDataInserterOptions2D range_data_inserter_options = 5; + int32 num_range_data = 1; + GridOptions2D grid_options_2d = 2; + RangeDataInserterOptions range_data_inserter_options = 3; } diff --git a/cartographer/mapping/2d/range_data_inserter_2d_test.cc b/cartographer/mapping/2d/range_data_inserter_2d_test.cc index 5a1b934..f3b5227 100644 --- a/cartographer/mapping/2d/range_data_inserter_2d_test.cc +++ b/cartographer/mapping/2d/range_data_inserter_2d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/2d/range_data_inserter_2d.h" +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" #include @@ -40,8 +40,10 @@ class RangeDataInserterTest2D : public ::testing::Test { "hit_probability = 0.7, " "miss_probability = 0.4, " "}"); - options_ = CreateRangeDataInserterOptions2D(parameter_dictionary.get()); - range_data_inserter_ = common::make_unique(options_); + options_ = CreateProbabilityGridRangeDataInserterOptions2D( + parameter_dictionary.get()); + range_data_inserter_ = + common::make_unique(options_); } void InsertPointCloud() { @@ -57,8 +59,8 @@ class RangeDataInserterTest2D : public ::testing::Test { } ProbabilityGrid probability_grid_; - std::unique_ptr range_data_inserter_; - proto::RangeDataInserterOptions2D options_; + std::unique_ptr range_data_inserter_; + proto::ProbabilityGridRangeDataInserterOptions2D options_; }; TEST_F(RangeDataInserterTest2D, InsertPointCloud) { diff --git a/cartographer/mapping/2d/submap_2d.cc b/cartographer/mapping/2d/submap_2d.cc index 2763e53..4bb97f6 100644 --- a/cartographer/mapping/2d/submap_2d.cc +++ b/cartographer/mapping/2d/submap_2d.cc @@ -25,6 +25,8 @@ #include "Eigen/Geometry" #include "cartographer/common/make_unique.h" #include "cartographer/common/port.h" +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" +#include "cartographer/mapping/range_data_inserter_interface.h" #include "glog/logging.h" namespace cartographer { @@ -33,20 +35,37 @@ namespace mapping { proto::SubmapsOptions2D CreateSubmapsOptions2D( common::LuaParameterDictionary* const parameter_dictionary) { proto::SubmapsOptions2D options; - options.set_resolution(parameter_dictionary->GetDouble("resolution")); options.set_num_range_data( parameter_dictionary->GetNonNegativeInt("num_range_data")); + *options.mutable_grid_options_2d() = CreateGridOptions2D( + parameter_dictionary->GetDictionary("grid_options_2d").get()); *options.mutable_range_data_inserter_options() = - CreateRangeDataInserterOptions2D( + CreateRangeDataInserterOptions( parameter_dictionary->GetDictionary("range_data_inserter").get()); + + bool valid_range_data_inserter_grid_combination = false; + const proto::GridOptions2D_GridType& grid_type = + options.grid_options_2d().grid_type(); + const proto::RangeDataInserterOptions_RangeDataInserterType& + range_data_inserter_type = + options.range_data_inserter_options().range_data_inserter_type(); + if (grid_type == proto::GridOptions2D::PROBABILITY_GRID && + range_data_inserter_type == + proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D) { + valid_range_data_inserter_grid_combination = true; + } + CHECK(valid_range_data_inserter_grid_combination) + << "Invalid combination grid_type " << grid_type + << " with range_data_inserter_type " << range_data_inserter_type; CHECK_GT(options.num_range_data(), 0); return options; } -Submap2D::Submap2D(const MapLimits& limits, const Eigen::Vector2f& origin) +Submap2D::Submap2D(const Eigen::Vector2f& origin, std::unique_ptr grid) : Submap(transform::Rigid3d::Translation( - Eigen::Vector3d(origin.x(), origin.y(), 0.))), - grid_(common::make_unique(limits)) {} + Eigen::Vector3d(origin.x(), origin.y(), 0.))) { + grid_ = std::move(grid); +} Submap2D::Submap2D(const proto::Submap2D& proto) : Submap(transform::ToRigid3(proto.local_pose())) { @@ -91,11 +110,12 @@ void Submap2D::ToResponseProto( grid()->DrawToSubmapTexture(texture, local_pose()); } -void Submap2D::InsertRangeData(const sensor::RangeData& range_data, - const RangeDataInserter2D& range_data_inserter) { +void Submap2D::InsertRangeData( + const sensor::RangeData& range_data, + const RangeDataInserterInterface* range_data_inserter) { CHECK(grid_); CHECK(!finished()); - range_data_inserter.Insert(range_data, grid_.get()); + range_data_inserter->Insert(range_data, grid_.get()); set_num_range_data(num_range_data() + 1); } @@ -108,27 +128,45 @@ void Submap2D::Finish() { ActiveSubmaps2D::ActiveSubmaps2D(const proto::SubmapsOptions2D& options) : options_(options), - range_data_inserter_(options.range_data_inserter_options()) { + range_data_inserter_(std::move(CreateRangeDataInserter())) { // We always want to have at least one likelihood field which we can return, // and will create it at the origin in absence of a better choice. AddSubmap(Eigen::Vector2f::Zero()); } -void ActiveSubmaps2D::InsertRangeData(const sensor::RangeData& range_data) { - for (auto& submap : submaps_) { - submap->InsertRangeData(range_data, range_data_inserter_); - } - if (submaps_.back()->num_range_data() == options_.num_range_data()) { - AddSubmap(range_data.origin.head<2>()); - } -} - std::vector> ActiveSubmaps2D::submaps() const { return submaps_; } int ActiveSubmaps2D::matching_index() const { return matching_submap_index_; } +void ActiveSubmaps2D::InsertRangeData(const sensor::RangeData& range_data) { + for (auto& submap : submaps_) { + submap->InsertRangeData(range_data, range_data_inserter_.get()); + } + if (submaps_.back()->num_range_data() == options_.num_range_data()) { + AddSubmap(range_data.origin.head<2>()); + } +} + +std::unique_ptr +ActiveSubmaps2D::CreateRangeDataInserter() { + return common::make_unique( + options_.range_data_inserter_options() + .probability_grid_range_data_inserter_options_2d()); +} + +std::unique_ptr ActiveSubmaps2D::CreateGrid( + const Eigen::Vector2f& origin) { + constexpr int kInitialSubmapSize = 100; + float resolution = options_.grid_options_2d().resolution(); + return common::make_unique( + MapLimits(resolution, + origin.cast() + 0.5 * kInitialSubmapSize * resolution * + Eigen::Vector2d::Ones(), + CellLimits(kInitialSubmapSize, kInitialSubmapSize))); +} + void ActiveSubmaps2D::FinishSubmap() { Submap2D* submap = submaps_.front().get(); submap->Finish(); @@ -142,14 +180,10 @@ void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) { // reduce peak memory usage a bit. FinishSubmap(); } - constexpr int kInitialSubmapSize = 100; + submaps_.push_back(common::make_unique( - MapLimits(options_.resolution(), - origin.cast() + 0.5 * kInitialSubmapSize * - options_.resolution() * - Eigen::Vector2d::Ones(), - CellLimits(kInitialSubmapSize, kInitialSubmapSize)), - origin)); + origin, std::unique_ptr( + static_cast(CreateGrid(origin).release())))); LOG(INFO) << "Added submap " << matching_submap_index_ + submaps_.size(); } diff --git a/cartographer/mapping/2d/submap_2d.h b/cartographer/mapping/2d/submap_2d.h index bfb3287..5e3e807 100644 --- a/cartographer/mapping/2d/submap_2d.h +++ b/cartographer/mapping/2d/submap_2d.h @@ -25,9 +25,9 @@ #include "cartographer/mapping/2d/grid_2d.h" #include "cartographer/mapping/2d/map_limits.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" #include "cartographer/mapping/proto/submap_visualization.pb.h" +#include "cartographer/mapping/range_data_inserter_interface.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_node.h" #include "cartographer/sensor/range_data.h" @@ -41,7 +41,7 @@ proto::SubmapsOptions2D CreateSubmapsOptions2D( class Submap2D : public Submap { public: - Submap2D(const MapLimits& limits, const Eigen::Vector2f& origin); + Submap2D(const Eigen::Vector2f& origin, std::unique_ptr grid); explicit Submap2D(const proto::Submap2D& proto); void ToProto(proto::Submap* proto, @@ -56,7 +56,7 @@ class Submap2D : public Submap { // Insert 'range_data' into this submap using 'range_data_inserter'. The // submap must not be finished yet. void InsertRangeData(const sensor::RangeData& range_data, - const RangeDataInserter2D& range_data_inserter); + const RangeDataInserterInterface* range_data_inserter); void Finish(); private: @@ -89,13 +89,15 @@ class ActiveSubmaps2D { std::vector> submaps() const; private: + std::unique_ptr CreateRangeDataInserter(); + std::unique_ptr CreateGrid(const Eigen::Vector2f& origin); void FinishSubmap(); void AddSubmap(const Eigen::Vector2f& origin); const proto::SubmapsOptions2D options_; int matching_submap_index_ = 0; std::vector> submaps_; - RangeDataInserter2D range_data_inserter_; + std::unique_ptr range_data_inserter_; }; } // namespace mapping diff --git a/cartographer/mapping/2d/submap_2d_test.cc b/cartographer/mapping/2d/submap_2d_test.cc index eed665a..c662b16 100644 --- a/cartographer/mapping/2d/submap_2d_test.cc +++ b/cartographer/mapping/2d/submap_2d_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/mapping/2d/submap_2d.h" +#include "cartographer/mapping/2d/probability_grid.h" #include #include @@ -35,15 +36,21 @@ TEST(Submap2DTest, TheRightNumberOfRangeDataAreInserted) { constexpr int kNumRangeData = 10; auto parameter_dictionary = common::MakeDictionary( "return {" - "resolution = 0.05, " "num_range_data = " + std::to_string(kNumRangeData) + ", " + "grid_options_2d = {" + "grid_type = \"PROBABILITY_GRID\"," + "resolution = 0.05, " + "}," "range_data_inserter = {" + "range_data_inserter_type = \"PROBABILITY_GRID_INSERTER_2D\"," + "probability_grid_range_data_inserter = {" "insert_free_space = true, " "hit_probability = 0.53, " "miss_probability = 0.495, " "}," + "}," "}"); ActiveSubmaps2D submaps{CreateSubmapsOptions2D(parameter_dictionary.get())}; std::set> all_submaps; @@ -69,9 +76,10 @@ TEST(Submap2DTest, TheRightNumberOfRangeDataAreInserted) { } TEST(Submap2DTest, ToFromProto) { - Submap2D expected( - MapLimits(1., Eigen::Vector2d(2., 3.), CellLimits(100, 110)), - Eigen::Vector2f(4.f, 5.f)); + MapLimits expected_map_limits(1., Eigen::Vector2d(2., 3.), + CellLimits(100, 110)); + Submap2D expected(Eigen::Vector2f(4.f, 5.f), + common::make_unique(expected_map_limits)); proto::Submap proto; expected.ToProto(&proto, true /* include_probability_grid_data */); EXPECT_TRUE(proto.has_submap_2d()); diff --git a/cartographer/mapping/3d/hybrid_grid.h b/cartographer/mapping/3d/hybrid_grid.h index 84b296b..23bd7bd 100644 --- a/cartographer/mapping/3d/hybrid_grid.h +++ b/cartographer/mapping/3d/hybrid_grid.h @@ -407,13 +407,13 @@ class DynamicGrid { }; template -using Grid = DynamicGrid, 3>>; +using GridBase = DynamicGrid, 3>>; // Represents a 3D grid as a wide, shallow tree. template -class HybridGridBase : public Grid { +class HybridGridBase : public GridBase { public: - using Iterator = typename Grid::Iterator; + using Iterator = typename GridBase::Iterator; // Creates a new tree-based probability grid with voxels having edge length // 'resolution' around the origin which becomes the center of the cell at diff --git a/cartographer/mapping/grid_interface.h b/cartographer/mapping/grid_interface.h new file mode 100644 index 0000000..29b2e80 --- /dev/null +++ b/cartographer/mapping/grid_interface.h @@ -0,0 +1,33 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_GRID_H_ +#define CARTOGRAPHER_MAPPING_GRID_H_ + +#include +#include + +namespace cartographer { +namespace mapping { + +class GridInterface { + // todo(kdaun) move mutual functions of Grid2D/3D here +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_GRID_H_ diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc index 30de6d8..85da564 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc @@ -76,7 +76,8 @@ 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 + CHECK_EQ(options_.submaps_options().grid_options_2d().grid_type(), + proto::GridOptions2D_GridType_PROBABILITY_GRID); double score = real_time_correlative_scan_matcher_.Match( pose_prediction, filtered_gravity_aligned_point_cloud, *static_cast(matching_submap->grid()), diff --git a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc index 3c60f55..bceb588 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc @@ -24,7 +24,7 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/thread_pool.h" #include "cartographer/common/time.h" -#include "cartographer/mapping/2d/range_data_inserter_2d.h" +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" #include "cartographer/mapping/2d/submap_2d.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform_test_helpers.h" @@ -50,12 +50,18 @@ class PoseGraph2DTest : public ::testing::Test { { auto parameter_dictionary = common::MakeDictionary(R"text( return { - resolution = 0.05, num_range_data = 1, + grid_options_2d = { + grid_type = "PROBABILITY_GRID", + resolution = 0.05, + }, range_data_inserter = { - insert_free_space = true, - hit_probability = 0.53, - miss_probability = 0.495, + range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D", + probability_grid_range_data_inserter = { + insert_free_space = true, + hit_probability = 0.53, + miss_probability = 0.495, + }, }, })text"); active_submaps_ = common::make_unique( diff --git a/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc index 9a48bbb..d4ab3a2 100644 --- a/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc @@ -24,7 +24,7 @@ #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/2d/probability_grid.h" -#include "cartographer/mapping/2d/range_data_inserter_2d.h" +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/transform.h" #include "gtest/gtest.h" @@ -125,7 +125,7 @@ CreateFastCorrelativeScanMatcherTestOptions2D( return CreateFastCorrelativeScanMatcherOptions2D(parameter_dictionary.get()); } -mapping::proto::RangeDataInserterOptions2D +mapping::proto::ProbabilityGridRangeDataInserterOptions2D CreateRangeDataInserterTestOptions2D() { auto parameter_dictionary = common::MakeDictionary(R"text( return { @@ -133,13 +133,14 @@ CreateRangeDataInserterTestOptions2D() { hit_probability = 0.7, miss_probability = 0.4, })text"); - return mapping::CreateRangeDataInserterOptions2D(parameter_dictionary.get()); + return mapping::CreateProbabilityGridRangeDataInserterOptions2D( + parameter_dictionary.get()); } TEST(FastCorrelativeScanMatcherTest, CorrectPose) { std::mt19937 prng(42); std::uniform_real_distribution distribution(-1.f, 1.f); - RangeDataInserter2D range_data_inserter( + ProbabilityGridRangeDataInserter2D range_data_inserter( CreateRangeDataInserterTestOptions2D()); constexpr float kMinScore = 0.1f; const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(3); @@ -187,7 +188,7 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { std::mt19937 prng(42); std::uniform_real_distribution distribution(-1.f, 1.f); - RangeDataInserter2D range_data_inserter( + ProbabilityGridRangeDataInserter2D range_data_inserter( CreateRangeDataInserterTestOptions2D()); constexpr float kMinScore = 0.1f; const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(6); diff --git a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc index 61842e1..5b3d136 100644 --- a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc @@ -23,7 +23,7 @@ #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/make_unique.h" #include "cartographer/mapping/2d/probability_grid.h" -#include "cartographer/mapping/2d/range_data_inserter_2d.h" +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" #include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/transform.h" @@ -46,8 +46,10 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { "hit_probability = 0.7, " "miss_probability = 0.4, " "}"); - range_data_inserter_ = common::make_unique( - CreateRangeDataInserterOptions2D(parameter_dictionary.get())); + range_data_inserter_ = + common::make_unique( + CreateProbabilityGridRangeDataInserterOptions2D( + parameter_dictionary.get())); } point_cloud_.emplace_back(0.025f, 0.175f, 0.f); point_cloud_.emplace_back(-0.025f, 0.175f, 0.f); @@ -76,7 +78,7 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { } ProbabilityGrid probability_grid_; - std::unique_ptr range_data_inserter_; + std::unique_ptr range_data_inserter_; sensor::PointCloud point_cloud_; std::unique_ptr real_time_correlative_scan_matcher_; diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc b/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc index 82e8deb..c13cfbb 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/mapping/internal/constraints/constraint_builder_2d.h" +#include "cartographer/mapping/2d/probability_grid.h" #include @@ -71,8 +72,9 @@ TEST_F(ConstraintBuilder2DTest, FindsConstraints) { node_data.gravity_alignment = Eigen::Quaterniond::Identity(); node_data.local_pose = transform::Rigid3d::Identity(); SubmapId submap_id{0, 1}; - Submap2D submap(MapLimits(1., Eigen::Vector2d(2., 3.), CellLimits(100, 110)), - Eigen::Vector2f(4.f, 5.f)); + MapLimits map_limits(1., Eigen::Vector2d(2., 3.), CellLimits(100, 110)); + Submap2D submap(Eigen::Vector2f(4.f, 5.f), + common::make_unique(map_limits)); int expected_nodes = 0; for (int i = 0; i < 2; ++i) { EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), expected_nodes); diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index cc3359c..ec00b52 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -130,6 +130,7 @@ int MapBuilder::AddTrajectoryBuilder( trimmer_options.min_covered_area() / common::Pow2(trajectory_options.trajectory_builder_2d_options() .submaps_options() + .grid_options_2d() .resolution()), trimmer_options.min_added_submaps_count())); } diff --git a/cartographer/mapping/proto/range_data_inserter_options.proto b/cartographer/mapping/proto/range_data_inserter_options.proto new file mode 100644 index 0000000..ff68263 --- /dev/null +++ b/cartographer/mapping/proto/range_data_inserter_options.proto @@ -0,0 +1,30 @@ +// Copyright 2018 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +import "cartographer/mapping/2d/proto/probability_grid_range_data_inserter_options_2d.proto"; + +package cartographer.mapping.proto; + +message RangeDataInserterOptions { + enum RangeDataInserterType { + INVALID_INSERTER = 0; + PROBABILITY_GRID_INSERTER_2D = 1; + } + + RangeDataInserterType range_data_inserter_type = 1; + ProbabilityGridRangeDataInserterOptions2D + probability_grid_range_data_inserter_options_2d = 2; +} diff --git a/cartographer/mapping/range_data_inserter_interface.cc b/cartographer/mapping/range_data_inserter_interface.cc new file mode 100644 index 0000000..10c3c0b --- /dev/null +++ b/cartographer/mapping/range_data_inserter_interface.cc @@ -0,0 +1,43 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/range_data_inserter_interface.h" +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" + +namespace cartographer { +namespace mapping { + +proto::RangeDataInserterOptions CreateRangeDataInserterOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::RangeDataInserterOptions options; + const std::string range_data_inserter_type_string = + parameter_dictionary->GetString("range_data_inserter_type"); + proto::RangeDataInserterOptions_RangeDataInserterType + range_data_inserter_type; + CHECK(proto::RangeDataInserterOptions_RangeDataInserterType_Parse( + range_data_inserter_type_string, &range_data_inserter_type)) + << "Unknown RangeDataInserterOptions_RangeDataInserterType kind: " + << range_data_inserter_type_string; + options.set_range_data_inserter_type(range_data_inserter_type); + *options.mutable_probability_grid_range_data_inserter_options_2d() = + CreateProbabilityGridRangeDataInserterOptions2D( + parameter_dictionary + ->GetDictionary("probability_grid_range_data_inserter") + .get()); + return options; +} +} // namespace mapping +} // namespace cartographer \ No newline at end of file diff --git a/cartographer/mapping/range_data_inserter_interface.h b/cartographer/mapping/range_data_inserter_interface.h new file mode 100644 index 0000000..b0074ca --- /dev/null +++ b/cartographer/mapping/range_data_inserter_interface.h @@ -0,0 +1,43 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_RANGE_DATA_INSERTER_H_ +#define CARTOGRAPHER_MAPPING_RANGE_DATA_INSERTER_H_ + +#include +#include + +#include "cartographer/mapping/2d/proto/submaps_options_2d.pb.h" +#include "cartographer/mapping/grid_interface.h" +#include "cartographer/sensor/range_data.h" + +namespace cartographer { +namespace mapping { + +proto::RangeDataInserterOptions CreateRangeDataInserterOptions( + common::LuaParameterDictionary* const parameter_dictionary); + +class RangeDataInserterInterface { + public: + // Inserts 'range_data' into 'grid'. + virtual void Insert(const sensor::RangeData& range_data, + GridInterface* grid) const = 0; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_RANGE_DATA_INSERTER_H_ diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index 7779719..4bb55c6 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/configuration_files/trajectory_builder_2d.lua @@ -62,12 +62,18 @@ TRAJECTORY_BUILDER_2D = { imu_gravity_time_constant = 10., submaps = { - resolution = 0.05, num_range_data = 90, + grid_options_2d = { + grid_type = "PROBABILITY_GRID", + resolution = 0.05, + }, range_data_inserter = { - insert_free_space = true, - hit_probability = 0.55, - miss_probability = 0.49, + range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D", + probability_grid_range_data_inserter = { + insert_free_space = true, + hit_probability = 0.55, + miss_probability = 0.49, + }, }, }, }