From 77886cb53b3978f46a731c9e57df827e3b013f45 Mon Sep 17 00:00:00 2001 From: Kevin Daun Date: Fri, 3 Aug 2018 11:56:35 +0200 Subject: [PATCH] Integrate TSDF components into SLAM pipeline. (#1361) [RFC=0019](https://github.com/googlecartographer/rfcs/blob/master/text/0019-probability-grid-and-submap2d-restructuring.md) --- .../cloud/internal/client_server_test.cc | 99 +++++++++-- cartographer/mapping/2d/grid_2d.cc | 2 - cartographer/mapping/2d/grid_2d.h | 9 +- cartographer/mapping/2d/probability_grid.cc | 9 +- cartographer/mapping/2d/probability_grid.h | 8 +- .../mapping/2d/probability_grid_test.cc | 4 +- cartographer/mapping/2d/submap_2d.cc | 81 +++++++-- cartographer/mapping/2d/submap_2d.h | 2 +- cartographer/mapping/2d/tsdf_2d.cc | 7 +- cartographer/mapping/2d/tsdf_2d.h | 7 +- cartographer/mapping/2d/tsdf_2d_test.cc | 4 +- .../2d/scan_matching/ceres_scan_matcher_2d.cc | 24 ++- cartographer/mapping/map_builder_test.cc | 166 ++++++++++-------- .../mapping/proto/2d/grid_2d_options.proto | 1 + cartographer/mapping/submaps.h | 2 +- configuration_files/trajectory_builder_2d.lua | 6 +- 16 files changed, 294 insertions(+), 137 deletions(-) diff --git a/cartographer/cloud/internal/client_server_test.cc b/cartographer/cloud/internal/client_server_test.cc index d1a4d13..148e276 100644 --- a/cartographer/cloud/internal/client_server_test.cc +++ b/cartographer/cloud/internal/client_server_test.cc @@ -75,7 +75,8 @@ constexpr char kOdometryDataProtoString[] = "odometry_data {}"; constexpr char kFixedFramePoseDataProtoString[] = "fixed_frame_pose_data {}"; constexpr char kLandmarkDataProtoString[] = "landmark_data {}"; -class ClientServerTest : public ::testing::Test { +template +class ClientServerTestBase : public T { protected: void SetUp() override { // TODO(cschuet): Due to the hard-coded addresses these tests will become @@ -178,6 +179,32 @@ class ClientServerTest : public ::testing::Test { EXPECT_TRUE(stub_for_uploading_server_ != nullptr); } + void SetOptionsToTSDF2D() { + trajectory_builder_options_.mutable_trajectory_builder_2d_options() + ->mutable_submaps_options() + ->mutable_range_data_inserter_options() + ->set_range_data_inserter_type( + ::cartographer::mapping::proto::RangeDataInserterOptions:: + TSDF_INSERTER_2D); + trajectory_builder_options_.mutable_trajectory_builder_2d_options() + ->mutable_submaps_options() + ->mutable_grid_options_2d() + ->set_grid_type(::cartographer::mapping::proto::GridOptions2D::TSDF); + trajectory_builder_options_.mutable_trajectory_builder_2d_options() + ->mutable_ceres_scan_matcher_options() + ->set_occupied_space_weight(10.0); + map_builder_server_options_.mutable_map_builder_options() + ->mutable_pose_graph_options() + ->mutable_constraint_builder_options() + ->mutable_ceres_scan_matcher_options() + ->set_occupied_space_weight(50.0); + uploading_map_builder_server_options_.mutable_map_builder_options() + ->mutable_pose_graph_options() + ->mutable_constraint_builder_options() + ->mutable_ceres_scan_matcher_options() + ->set_occupied_space_weight(50.0); + } + void WaitForLocalSlamResults(size_t size) { std::unique_lock lock(local_slam_result_mutex_); local_slam_result_condition_.wait( @@ -212,13 +239,26 @@ class ClientServerTest : public ::testing::Test { int number_of_insertion_results_; }; +class ClientServerTest : public ClientServerTestBase<::testing::Test> {}; +class ClientServerTestByGridType + : public ClientServerTestBase< + ::testing::TestWithParam<::cartographer::mapping::GridType>> {}; + +INSTANTIATE_TEST_CASE_P( + ClientServerTestByGridType, ClientServerTestByGridType, + ::testing::Values(::cartographer::mapping::GridType::PROBABILITY_GRID, + ::cartographer::mapping::GridType::TSDF)); + TEST_F(ClientServerTest, StartAndStopServer) { InitializeRealServer(); server_->Start(); server_->Shutdown(); } -TEST_F(ClientServerTest, AddTrajectoryBuilder) { +TEST_P(ClientServerTestByGridType, AddTrajectoryBuilder) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } InitializeRealServer(); server_->Start(); InitializeStub(); @@ -231,7 +271,10 @@ TEST_F(ClientServerTest, AddTrajectoryBuilder) { server_->Shutdown(); } -TEST_F(ClientServerTest, AddTrajectoryBuilderWithMock) { +TEST_P(ClientServerTestByGridType, AddTrajectoryBuilderWithMock) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } InitializeServerWithMockMapBuilder(); server_->Start(); InitializeStub(); @@ -250,7 +293,10 @@ TEST_F(ClientServerTest, AddTrajectoryBuilderWithMock) { server_->Shutdown(); } -TEST_F(ClientServerTest, AddSensorData) { +TEST_P(ClientServerTestByGridType, AddSensorData) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } trajectory_builder_options_.mutable_trajectory_builder_2d_options() ->set_use_imu_data(true); InitializeRealServer(); @@ -268,7 +314,10 @@ TEST_F(ClientServerTest, AddSensorData) { server_->Shutdown(); } -TEST_F(ClientServerTest, AddSensorDataWithMock) { +TEST_P(ClientServerTestByGridType, AddSensorDataWithMock) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } InitializeServerWithMockMapBuilder(); server_->Start(); InitializeStub(); @@ -297,7 +346,10 @@ TEST_F(ClientServerTest, AddSensorDataWithMock) { server_->Shutdown(); } -TEST_F(ClientServerTest, LocalSlam2D) { +TEST_P(ClientServerTestByGridType, LocalSlam2D) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } InitializeRealServer(); server_->Start(); InitializeStub(); @@ -328,7 +380,10 @@ TEST_F(ClientServerTest, LocalSlam2D) { server_->Shutdown(); } -TEST_F(ClientServerTest, LocalSlamAndDelete2D) { +TEST_P(ClientServerTestByGridType, LocalSlamAndDelete2D) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } InitializeRealServer(); server_->Start(); InitializeStub(); @@ -401,7 +456,10 @@ TEST_F(ClientServerTest, GlobalSlam3D) { server_->Shutdown(); } -TEST_F(ClientServerTest, StartAndStopUploadingServerAndServer) { +TEST_P(ClientServerTestByGridType, StartAndStopUploadingServerAndServer) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } InitializeRealServer(); server_->Start(); InitializeRealUploadingServer(); @@ -410,7 +468,10 @@ TEST_F(ClientServerTest, StartAndStopUploadingServerAndServer) { server_->Shutdown(); } -TEST_F(ClientServerTest, AddTrajectoryBuilderWithUploadingServer) { +TEST_P(ClientServerTestByGridType, AddTrajectoryBuilderWithUploadingServer) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } InitializeRealServer(); server_->Start(); InitializeRealUploadingServer(); @@ -433,7 +494,10 @@ TEST_F(ClientServerTest, AddTrajectoryBuilderWithUploadingServer) { server_->Shutdown(); } -TEST_F(ClientServerTest, LocalSlam2DWithUploadingServer) { +TEST_P(ClientServerTestByGridType, LocalSlam2DWithUploadingServer) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } InitializeRealServer(); server_->Start(); InitializeStub(); @@ -482,7 +546,10 @@ TEST_F(ClientServerTest, LocalSlam2DWithUploadingServer) { server_->Shutdown(); } -TEST_F(ClientServerTest, LocalSlam2DUplinkServerRestarting) { +TEST_P(ClientServerTestByGridType, LocalSlam2DUplinkServerRestarting) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } InitializeRealServer(); server_->Start(); InitializeStub(); @@ -528,7 +595,10 @@ TEST_F(ClientServerTest, LocalSlam2DUplinkServerRestarting) { server_->WaitForShutdown(); } -TEST_F(ClientServerTest, LoadStateAndDelete) { +TEST_P(ClientServerTestByGridType, LoadStateAndDelete) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } InitializeRealServer(); server_->Start(); InitializeStub(); @@ -568,7 +638,10 @@ TEST_F(ClientServerTest, LoadStateAndDelete) { // TODO(gaschler): Test-cover LoadStateFromFile. -TEST_F(ClientServerTest, LocalSlam2DHandlesInvalidRequests) { +TEST_P(ClientServerTestByGridType, LocalSlam2DHandlesInvalidRequests) { + if (GetParam() == ::cartographer::mapping::GridType::TSDF) { + SetOptionsToTSDF2D(); + } InitializeRealServer(); server_->Start(); InitializeStub(); diff --git a/cartographer/mapping/2d/grid_2d.cc b/cartographer/mapping/2d/grid_2d.cc index f726a2f..d64d897 100644 --- a/cartographer/mapping/2d/grid_2d.cc +++ b/cartographer/mapping/2d/grid_2d.cc @@ -63,7 +63,6 @@ Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost, float max_correspondence_cost, ValueConversionTables* conversion_tables) : limits_(limits), - grid_type_(GridType::NONE), correspondence_cost_cells_( limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells, kUnknownCorrespondenceValue), @@ -78,7 +77,6 @@ Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost, Grid2D::Grid2D(const proto::Grid2D& proto, ValueConversionTables* conversion_tables) : limits_(proto.limits()), - grid_type_(GridType::NONE), correspondence_cost_cells_(), min_correspondence_cost_(MinCorrespondenceCostFromProto(proto)), max_correspondence_cost_(MaxCorrespondenceCostFromProto(proto)), diff --git a/cartographer/mapping/2d/grid_2d.h b/cartographer/mapping/2d/grid_2d.h index 7fdb35d..c62f55a 100644 --- a/cartographer/mapping/2d/grid_2d.h +++ b/cartographer/mapping/2d/grid_2d.h @@ -32,7 +32,7 @@ namespace mapping { proto::GridOptions2D CreateGridOptions2D( common::LuaParameterDictionary* const parameter_dictionary); -enum class GridType { NONE, PROBABILITY_GRID, TSDF }; +enum class GridType { PROBABILITY_GRID, TSDF }; class Grid2D : public GridInterface { public: @@ -45,13 +45,14 @@ class Grid2D : public GridInterface { // Returns the limits of this Grid2D. const MapLimits& limits() const { return limits_; } - GridType grid_type() const { return grid_type_; } - // Finishes the update sequence. void FinishUpdate(); + // Returns the correspondence cost of the cell with 'cell_index'. float GetCorrespondenceCost(const Eigen::Array2i& cell_index) const; + virtual GridType GetGridType() const = 0; + // Returns the minimum possible correspondence cost. float GetMinCorrespondenceCost() const { return min_correspondence_cost_; } @@ -96,7 +97,6 @@ class Grid2D : public GridInterface { return &correspondence_cost_cells_; } - GridType* mutable_grid_type() { return &grid_type_; } std::vector* mutable_update_indices() { return &update_indices_; } Eigen::AlignedBox2i* mutable_known_cells_box() { return &known_cells_box_; } @@ -105,7 +105,6 @@ class Grid2D : public GridInterface { private: MapLimits limits_; - GridType grid_type_; std::vector correspondence_cost_cells_; float min_correspondence_cost_; float max_correspondence_cost_; diff --git a/cartographer/mapping/2d/probability_grid.cc b/cartographer/mapping/2d/probability_grid.cc index 85a9981..406b1f0 100644 --- a/cartographer/mapping/2d/probability_grid.cc +++ b/cartographer/mapping/2d/probability_grid.cc @@ -28,15 +28,12 @@ ProbabilityGrid::ProbabilityGrid(const MapLimits& limits, ValueConversionTables* conversion_tables) : Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost, conversion_tables), - conversion_tables_(conversion_tables) { - *mutable_grid_type() = GridType::PROBABILITY_GRID; -} + conversion_tables_(conversion_tables) {} ProbabilityGrid::ProbabilityGrid(const proto::Grid2D& proto, ValueConversionTables* conversion_tables) : Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) { CHECK(proto.has_probability_grid_2d()); - *mutable_grid_type() = GridType::PROBABILITY_GRID; } // Sets the probability of the cell at 'cell_index' to the given @@ -73,6 +70,10 @@ bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index, return true; } +GridType ProbabilityGrid::GetGridType() const { + return GridType::PROBABILITY_GRID; +} + // Returns the probability of the cell with 'cell_index'. float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const { if (!limits().Contains(cell_index)) return kMinProbability; diff --git a/cartographer/mapping/2d/probability_grid.h b/cartographer/mapping/2d/probability_grid.h index dd2b767..ecb2b7d 100644 --- a/cartographer/mapping/2d/probability_grid.h +++ b/cartographer/mapping/2d/probability_grid.h @@ -50,12 +50,14 @@ class ProbabilityGrid : public Grid2D { bool ApplyLookupTable(const Eigen::Array2i& cell_index, const std::vector& table); + GridType GetGridType() const override; + // Returns the probability of the cell with 'cell_index'. 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::Grid2D ToProto() const override; + std::unique_ptr ComputeCroppedGrid() const override; + bool DrawToSubmapTexture( proto::SubmapQuery::Response::SubmapTexture* const texture, transform::Rigid3d local_pose) const override; diff --git a/cartographer/mapping/2d/probability_grid_test.cc b/cartographer/mapping/2d/probability_grid_test.cc index bf49156..334a0b4 100644 --- a/cartographer/mapping/2d/probability_grid_test.cc +++ b/cartographer/mapping/2d/probability_grid_test.cc @@ -44,7 +44,7 @@ TEST(ProbabilityGridTest, ProtoConstructor) { ValueConversionTables conversion_tables; ProbabilityGrid grid(proto, &conversion_tables); EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString()); - EXPECT_EQ(grid.grid_type(), GridType::PROBABILITY_GRID); + EXPECT_EQ(grid.GetGridType(), GridType::PROBABILITY_GRID); // TODO(macmason): Figure out how to test the contents of cells_ and // {min, max}_{x, y}_ gracefully. @@ -55,7 +55,7 @@ TEST(ProbabilityGridTest, ConstructorGridType) { ProbabilityGrid probability_grid( MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), &conversion_tables); - EXPECT_EQ(probability_grid.grid_type(), GridType::PROBABILITY_GRID); + EXPECT_EQ(probability_grid.GetGridType(), GridType::PROBABILITY_GRID); } TEST(ProbabilityGridTest, ToProto) { diff --git a/cartographer/mapping/2d/submap_2d.cc b/cartographer/mapping/2d/submap_2d.cc index 5ac0827..82591cf 100644 --- a/cartographer/mapping/2d/submap_2d.cc +++ b/cartographer/mapping/2d/submap_2d.cc @@ -26,6 +26,7 @@ #include "absl/memory/memory.h" #include "cartographer/common/port.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" +#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h" #include "cartographer/mapping/range_data_inserter_interface.h" #include "glog/logging.h" @@ -54,6 +55,11 @@ proto::SubmapsOptions2D CreateSubmapsOptions2D( proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D) { valid_range_data_inserter_grid_combination = true; } + if (grid_type == proto::GridOptions2D::TSDF && + range_data_inserter_type == + proto::RangeDataInserterOptions::TSDF_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; @@ -74,22 +80,26 @@ Submap2D::Submap2D(const proto::Submap2D& proto, : Submap(transform::ToRigid3(proto.local_pose())), conversion_tables_(conversion_tables) { if (proto.has_grid()) { - CHECK(proto.grid().has_probability_grid_2d()); - grid_ = - absl::make_unique(proto.grid(), conversion_tables_); + if (proto.grid().has_probability_grid_2d()) { + grid_ = + absl::make_unique(proto.grid(), conversion_tables_); + } else if (proto.grid().has_tsdf_2d()) { + grid_ = absl::make_unique(proto.grid(), conversion_tables_); + } else { + LOG(FATAL) << "proto::Submap2D has grid with unknown type."; + } } set_num_range_data(proto.num_range_data()); set_finished(proto.finished()); } -proto::Submap Submap2D::ToProto( - const bool include_probability_grid_data) const { +proto::Submap Submap2D::ToProto(const bool include_grid_data) const { proto::Submap proto; auto* const submap_2d = proto.mutable_submap_2d(); *submap_2d->mutable_local_pose() = transform::ToProto(local_pose()); submap_2d->set_num_range_data(num_range_data()); submap_2d->set_finished(finished()); - if (include_probability_grid_data) { + if (include_grid_data) { CHECK(grid_); *submap_2d->mutable_grid() = grid_->ToProto(); } @@ -102,9 +112,15 @@ void Submap2D::UpdateFromProto(const proto::Submap& proto) { set_num_range_data(submap_2d.num_range_data()); set_finished(submap_2d.finished()); if (proto.submap_2d().has_grid()) { - CHECK(proto.submap_2d().grid().has_probability_grid_2d()); - grid_ = absl::make_unique(submap_2d.grid(), - conversion_tables_); + if (proto.submap_2d().grid().has_probability_grid_2d()) { + grid_ = absl::make_unique(proto.submap_2d().grid(), + conversion_tables_); + } else if (proto.submap_2d().grid().has_tsdf_2d()) { + grid_ = absl::make_unique(proto.submap_2d().grid(), + conversion_tables_); + } else { + LOG(FATAL) << "proto::Submap2D has grid with unknown type."; + } } } @@ -159,21 +175,50 @@ std::vector> ActiveSubmaps2D::InsertRangeData( std::unique_ptr ActiveSubmaps2D::CreateRangeDataInserter() { - return absl::make_unique( - options_.range_data_inserter_options() - .probability_grid_range_data_inserter_options_2d()); + switch (options_.range_data_inserter_options().range_data_inserter_type()) { + case proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D: + return absl::make_unique( + options_.range_data_inserter_options() + .probability_grid_range_data_inserter_options_2d()); + case proto::RangeDataInserterOptions::TSDF_INSERTER_2D: + return absl::make_unique( + options_.range_data_inserter_options() + .tsdf_range_data_inserter_options_2d()); + default: + LOG(FATAL) << "Unknown RangeDataInserterType."; + } } std::unique_ptr ActiveSubmaps2D::CreateGrid( const Eigen::Vector2f& origin) { constexpr int kInitialSubmapSize = 100; float resolution = options_.grid_options_2d().resolution(); - return absl::make_unique( - MapLimits(resolution, - origin.cast() + 0.5 * kInitialSubmapSize * resolution * - Eigen::Vector2d::Ones(), - CellLimits(kInitialSubmapSize, kInitialSubmapSize)), - &conversion_tables_); + switch (options_.grid_options_2d().grid_type()) { + case proto::GridOptions2D::PROBABILITY_GRID: + return absl::make_unique( + MapLimits(resolution, + origin.cast() + 0.5 * kInitialSubmapSize * + resolution * + Eigen::Vector2d::Ones(), + CellLimits(kInitialSubmapSize, kInitialSubmapSize)), + &conversion_tables_); + case proto::GridOptions2D::TSDF: + return absl::make_unique( + MapLimits(resolution, + origin.cast() + 0.5 * kInitialSubmapSize * + resolution * + Eigen::Vector2d::Ones(), + CellLimits(kInitialSubmapSize, kInitialSubmapSize)), + options_.range_data_inserter_options() + .tsdf_range_data_inserter_options_2d() + .truncation_distance(), + options_.range_data_inserter_options() + .tsdf_range_data_inserter_options_2d() + .maximum_weight(), + &conversion_tables_); + default: + LOG(FATAL) << "Unknown GridType."; + } } void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) { diff --git a/cartographer/mapping/2d/submap_2d.h b/cartographer/mapping/2d/submap_2d.h index 6c668d0..8ea9097 100644 --- a/cartographer/mapping/2d/submap_2d.h +++ b/cartographer/mapping/2d/submap_2d.h @@ -47,7 +47,7 @@ class Submap2D : public Submap { explicit Submap2D(const proto::Submap2D& proto, ValueConversionTables* conversion_tables); - proto::Submap ToProto(bool include_probability_grid_data) const override; + proto::Submap ToProto(bool include_grid_data) const override; void UpdateFromProto(const proto::Submap& proto) override; void ToResponseProto(const transform::Rigid3d& global_submap_pose, diff --git a/cartographer/mapping/2d/tsdf_2d.cc b/cartographer/mapping/2d/tsdf_2d.cc index f139ea9..32bdb59 100644 --- a/cartographer/mapping/2d/tsdf_2d.cc +++ b/cartographer/mapping/2d/tsdf_2d.cc @@ -30,15 +30,12 @@ TSDF2D::TSDF2D(const MapLimits& limits, float truncation_distance, truncation_distance, max_weight, conversion_tables_)), weight_cells_( limits.cell_limits().num_x_cells * limits.cell_limits().num_y_cells, - value_converter_->getUnknownWeightValue()) { - *mutable_grid_type() = GridType::TSDF; -} + value_converter_->getUnknownWeightValue()) {} TSDF2D::TSDF2D(const proto::Grid2D& proto, ValueConversionTables* conversion_tables) : Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) { CHECK(proto.has_tsdf_2d()); - *mutable_grid_type() = GridType::TSDF; value_converter_ = absl::make_unique( proto.tsdf_2d().truncation_distance(), proto.tsdf_2d().max_weight(), conversion_tables_); @@ -70,6 +67,8 @@ void TSDF2D::SetCell(const Eigen::Array2i& cell_index, float tsd, *weight_cell = value_converter_->WeightToValue(weight); } +GridType TSDF2D::GetGridType() const { return GridType::TSDF; } + float TSDF2D::GetTSD(const Eigen::Array2i& cell_index) const { if (limits().Contains(cell_index)) { return value_converter_->ValueToTSD( diff --git a/cartographer/mapping/2d/tsdf_2d.h b/cartographer/mapping/2d/tsdf_2d.h index 415eeb3..f040711 100644 --- a/cartographer/mapping/2d/tsdf_2d.h +++ b/cartographer/mapping/2d/tsdf_2d.h @@ -37,15 +37,16 @@ class TSDF2D : public Grid2D { void SetCell(const Eigen::Array2i& cell_index, const float tsd, const float weight); + GridType GetGridType() const override; float GetTSD(const Eigen::Array2i& cell_index) const; float GetWeight(const Eigen::Array2i& cell_index) const; std::pair GetTSDAndWeight( const Eigen::Array2i& cell_index) const; - virtual void GrowLimits(const Eigen::Vector2f& point) override; + void GrowLimits(const Eigen::Vector2f& point) override; proto::Grid2D ToProto() const override; - virtual std::unique_ptr ComputeCroppedGrid() const override; - virtual bool DrawToSubmapTexture( + std::unique_ptr ComputeCroppedGrid() const override; + bool DrawToSubmapTexture( proto::SubmapQuery::Response::SubmapTexture* const texture, transform::Rigid3d local_pose) const override; bool CellIsUpdated(const Eigen::Array2i& cell_index) const; diff --git a/cartographer/mapping/2d/tsdf_2d_test.cc b/cartographer/mapping/2d/tsdf_2d_test.cc index 7182c82..5e50343 100644 --- a/cartographer/mapping/2d/tsdf_2d_test.cc +++ b/cartographer/mapping/2d/tsdf_2d_test.cc @@ -48,7 +48,7 @@ TEST(TSDF2DTest, ProtoConstructor) { ValueConversionTables conversion_tables; TSDF2D grid(proto, &conversion_tables); - EXPECT_EQ(grid.grid_type(), GridType::TSDF); + EXPECT_EQ(grid.GetGridType(), GridType::TSDF); EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString()); } @@ -56,7 +56,7 @@ TEST(TSDF2DTest, ConstructorGridType) { ValueConversionTables conversion_tables; TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), 1.0f, 10.0f, &conversion_tables); - EXPECT_EQ(tsdf.grid_type(), GridType::TSDF); + EXPECT_EQ(tsdf.GetGridType(), GridType::TSDF); } TEST(TSDF2DTest, ToProto) { diff --git a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc index 4809bac..e0691b6 100644 --- a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc +++ b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc @@ -26,6 +26,7 @@ #include "cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h" #include "cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h" #include "cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h" +#include "cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" #include "glog/logging.h" @@ -70,12 +71,23 @@ void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation, initial_pose_estimate.rotation().angle()}; ceres::Problem problem; CHECK_GT(options_.occupied_space_weight(), 0.); - problem.AddResidualBlock( - CreateOccupiedSpaceCostFunction2D( - options_.occupied_space_weight() / - std::sqrt(static_cast(point_cloud.size())), - point_cloud, grid), - nullptr /* loss function */, ceres_pose_estimate); + if (grid.GetGridType() == GridType::PROBABILITY_GRID) { + problem.AddResidualBlock( + CreateOccupiedSpaceCostFunction2D( + options_.occupied_space_weight() / + std::sqrt(static_cast(point_cloud.size())), + point_cloud, grid), + nullptr /* loss function */, ceres_pose_estimate); + } else if (grid.GetGridType() == GridType::TSDF) { + problem.AddResidualBlock( + CreateTSDFMatchCostFunction2D( + options_.occupied_space_weight() / + std::sqrt(static_cast(point_cloud.size())), + point_cloud, static_cast(grid)), + nullptr /* loss function */, ceres_pose_estimate); + } else { + LOG(FATAL) << "Unknown GridType."; + } CHECK_GT(options_.translation_weight(), 0.); problem.AddResidualBlock( TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction( diff --git a/cartographer/mapping/map_builder_test.cc b/cartographer/mapping/map_builder_test.cc index 717e225..a3b79f8 100644 --- a/cartographer/mapping/map_builder_test.cc +++ b/cartographer/mapping/map_builder_test.cc @@ -18,6 +18,7 @@ #include "cartographer/common/config.h" #include "cartographer/io/proto_stream.h" +#include "cartographer/mapping/2d/grid_2d.h" #include "cartographer/mapping/internal/testing/test_helpers.h" #include "gmock/gmock.h" #include "gtest/gtest.h" @@ -34,7 +35,8 @@ constexpr double kDuration = 4.; // Seconds. constexpr double kTimeStep = 0.1; // Seconds. constexpr double kTravelDistance = 1.2; // Meters. -class MapBuilderTest : public ::testing::Test { +template +class MapBuilderTestBase : public T { protected: void SetUp() override { // Global SLAM optimization is not executed. @@ -70,6 +72,25 @@ class MapBuilderTest : public ::testing::Test { map_builder_options_.set_use_trajectory_builder_3d(true); } + void SetOptionsToTSDF2D() { + trajectory_builder_options_.mutable_trajectory_builder_2d_options() + ->mutable_submaps_options() + ->mutable_range_data_inserter_options() + ->set_range_data_inserter_type( + proto::RangeDataInserterOptions::TSDF_INSERTER_2D); + trajectory_builder_options_.mutable_trajectory_builder_2d_options() + ->mutable_submaps_options() + ->mutable_grid_options_2d() + ->set_grid_type(proto::GridOptions2D::TSDF); + trajectory_builder_options_.mutable_trajectory_builder_2d_options() + ->mutable_ceres_scan_matcher_options() + ->set_occupied_space_weight(10.0); + map_builder_options_.mutable_pose_graph_options() + ->mutable_constraint_builder_options() + ->mutable_ceres_scan_matcher_options() + ->set_occupied_space_weight(50.0); + } + void SetOptionsEnableGlobalOptimization() { map_builder_options_.mutable_pose_graph_options() ->set_optimize_every_n_nodes(3); @@ -111,7 +132,26 @@ class MapBuilderTest : public ::testing::Test { std::vector<::cartographer::transform::Rigid3d> local_slam_result_poses_; }; -TEST_F(MapBuilderTest, TrajectoryAddFinish2D) { +class MapBuilderTest : public MapBuilderTestBase<::testing::Test> {}; +class MapBuilderTestByGridType + : public MapBuilderTestBase<::testing::TestWithParam> {}; +class MapBuilderTestByGridTypeAndDimensions + : public MapBuilderTestBase< + ::testing::TestWithParam>> { +}; +INSTANTIATE_TEST_CASE_P(MapBuilderTestByGridType, MapBuilderTestByGridType, + ::testing::Values(GridType::PROBABILITY_GRID, + GridType::TSDF)); +INSTANTIATE_TEST_CASE_P( + MapBuilderTestByGridTypeAndDimensions, + MapBuilderTestByGridTypeAndDimensions, + ::testing::Values(std::make_pair(GridType::PROBABILITY_GRID, 2), + std::make_pair(GridType::PROBABILITY_GRID, 3), + std::make_pair(GridType::TSDF, 2))); + +TEST_P(MapBuilderTestByGridTypeAndDimensions, TrajectoryAddFinish) { + if (GetParam().second == 3) SetOptionsTo3D(); + if (GetParam().first == GridType::TSDF) SetOptionsToTSDF2D(); BuildMapBuilder(); int trajectory_id = map_builder_->AddTrajectoryBuilder( {kRangeSensorId}, trajectory_builder_options_, @@ -124,21 +164,8 @@ TEST_F(MapBuilderTest, TrajectoryAddFinish2D) { EXPECT_TRUE(map_builder_->pose_graph()->IsTrajectoryFinished(trajectory_id)); } -TEST_F(MapBuilderTest, TrajectoryAddFinish3D) { - SetOptionsTo3D(); - BuildMapBuilder(); - int trajectory_id = map_builder_->AddTrajectoryBuilder( - {kRangeSensorId}, trajectory_builder_options_, - nullptr /* GetLocalSlamResultCallbackForSubscriptions */); - EXPECT_EQ(1, map_builder_->num_trajectory_builders()); - EXPECT_TRUE(map_builder_->GetTrajectoryBuilder(trajectory_id) != nullptr); - EXPECT_TRUE(map_builder_->pose_graph() != nullptr); - map_builder_->FinishTrajectory(trajectory_id); - map_builder_->pose_graph()->RunFinalOptimization(); - EXPECT_TRUE(map_builder_->pose_graph()->IsTrajectoryFinished(trajectory_id)); -} - -TEST_F(MapBuilderTest, LocalSlam2D) { +TEST_P(MapBuilderTestByGridType, LocalSlam2D) { + if (GetParam() == GridType::TSDF) SetOptionsToTSDF2D(); BuildMapBuilder(); int trajectory_id = map_builder_->AddTrajectoryBuilder( {kRangeSensorId}, trajectory_builder_options_, @@ -187,7 +214,8 @@ TEST_F(MapBuilderTest, LocalSlam3D) { 0.1 * kTravelDistance); } -TEST_F(MapBuilderTest, GlobalSlam2D) { +TEST_P(MapBuilderTestByGridType, GlobalSlam2D) { + if (GetParam() == GridType::TSDF) SetOptionsToTSDF2D(); SetOptionsEnableGlobalOptimization(); BuildMapBuilder(); int trajectory_id = map_builder_->AddTrajectoryBuilder( @@ -268,7 +296,8 @@ TEST_F(MapBuilderTest, GlobalSlam3D) { 0.1 * kTravelDistance); } -TEST_F(MapBuilderTest, DeleteFinishedTrajectory2D) { +TEST_P(MapBuilderTestByGridType, DeleteFinishedTrajectory2D) { + if (GetParam() == GridType::TSDF) SetOptionsToTSDF2D(); SetOptionsEnableGlobalOptimization(); BuildMapBuilder(); int trajectory_id = CreateTrajectoryWithFakeData(); @@ -309,60 +338,57 @@ TEST_F(MapBuilderTest, DeleteFinishedTrajectory2D) { 0); } -TEST_F(MapBuilderTest, SaveLoadState) { - for (int dimensions : {2, 3}) { - if (dimensions == 3) { - SetOptionsTo3D(); - } - trajectory_builder_options_.mutable_trajectory_builder_2d_options() - ->set_use_imu_data(true); - BuildMapBuilder(); - int trajectory_id = map_builder_->AddTrajectoryBuilder( - {kRangeSensorId, kIMUSensorId}, trajectory_builder_options_, - GetLocalSlamResultCallback()); - TrajectoryBuilderInterface* trajectory_builder = - map_builder_->GetTrajectoryBuilder(trajectory_id); - const auto measurements = testing::GenerateFakeRangeMeasurements( - kTravelDistance, kDuration, kTimeStep); - for (const auto& measurement : measurements) { - trajectory_builder->AddSensorData(kRangeSensorId.id, measurement); - trajectory_builder->AddSensorData( - kIMUSensorId.id, - sensor::ImuData{measurement.time, Eigen::Vector3d(0., 0., 9.8), - Eigen::Vector3d::Zero()}); - } - map_builder_->FinishTrajectory(trajectory_id); - map_builder_->pose_graph()->RunFinalOptimization(); - int num_constraints = map_builder_->pose_graph()->constraints().size(); - int num_nodes = - map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero( - trajectory_id); - EXPECT_GT(num_constraints, 0); - EXPECT_GT(num_nodes, 0); - // TODO(gaschler): Consider using in-memory to avoid side effects. - const std::string filename = "temp-SaveLoadState.pbstream"; - io::ProtoStreamWriter writer(filename); - map_builder_->SerializeState(/*include_unfinished_submaps=*/true, &writer); - writer.Close(); - - // Reset 'map_builder_'. - BuildMapBuilder(); - io::ProtoStreamReader reader(filename); - auto trajectory_remapping = - map_builder_->LoadState(&reader, false /* load_frozen_state */); - map_builder_->pose_graph()->RunFinalOptimization(); - EXPECT_EQ(num_constraints, - map_builder_->pose_graph()->constraints().size()); - ASSERT_EQ(trajectory_remapping.size(), 1); - int new_trajectory_id = trajectory_remapping.begin()->second; - EXPECT_EQ( - num_nodes, - map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero( - new_trajectory_id)); +TEST_P(MapBuilderTestByGridTypeAndDimensions, SaveLoadState) { + if (GetParam().second == 3) SetOptionsTo3D(); + if (GetParam().first == GridType::TSDF) SetOptionsToTSDF2D(); + trajectory_builder_options_.mutable_trajectory_builder_2d_options() + ->set_use_imu_data(true); + BuildMapBuilder(); + int trajectory_id = map_builder_->AddTrajectoryBuilder( + {kRangeSensorId, kIMUSensorId}, trajectory_builder_options_, + GetLocalSlamResultCallback()); + TrajectoryBuilderInterface* trajectory_builder = + map_builder_->GetTrajectoryBuilder(trajectory_id); + const auto measurements = testing::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); + for (const auto& measurement : measurements) { + trajectory_builder->AddSensorData(kRangeSensorId.id, measurement); + trajectory_builder->AddSensorData( + kIMUSensorId.id, + sensor::ImuData{measurement.time, Eigen::Vector3d(0., 0., 9.8), + Eigen::Vector3d::Zero()}); } + map_builder_->FinishTrajectory(trajectory_id); + map_builder_->pose_graph()->RunFinalOptimization(); + int num_constraints = map_builder_->pose_graph()->constraints().size(); + int num_nodes = + map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero( + trajectory_id); + EXPECT_GT(num_constraints, 0); + EXPECT_GT(num_nodes, 0); + // TODO(gaschler): Consider using in-memory to avoid side effects. + const std::string filename = "temp-SaveLoadState.pbstream"; + io::ProtoStreamWriter writer(filename); + map_builder_->SerializeState(/*include_unfinished_submaps=*/true, &writer); + writer.Close(); + + // Reset 'map_builder_'. + BuildMapBuilder(); + io::ProtoStreamReader reader(filename); + auto trajectory_remapping = + map_builder_->LoadState(&reader, false /* load_frozen_state */); + map_builder_->pose_graph()->RunFinalOptimization(); + EXPECT_EQ(num_constraints, map_builder_->pose_graph()->constraints().size()); + ASSERT_EQ(trajectory_remapping.size(), 1); + int new_trajectory_id = trajectory_remapping.begin()->second; + EXPECT_EQ( + num_nodes, + map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero( + new_trajectory_id)); } -TEST_F(MapBuilderTest, LocalizationOnFrozenTrajectory2D) { +TEST_P(MapBuilderTestByGridType, LocalizationOnFrozenTrajectory2D) { + if (GetParam() == GridType::TSDF) SetOptionsToTSDF2D(); BuildMapBuilder(); int temp_trajectory_id = CreateTrajectoryWithFakeData(); map_builder_->pose_graph()->RunFinalOptimization(); diff --git a/cartographer/mapping/proto/2d/grid_2d_options.proto b/cartographer/mapping/proto/2d/grid_2d_options.proto index 5040d79..4dcc506 100644 --- a/cartographer/mapping/proto/2d/grid_2d_options.proto +++ b/cartographer/mapping/proto/2d/grid_2d_options.proto @@ -20,6 +20,7 @@ message GridOptions2D { enum GridType { INVALID_GRID = 0; PROBABILITY_GRID = 1; + TSDF = 2; } GridType grid_type = 1; diff --git a/cartographer/mapping/submaps.h b/cartographer/mapping/submaps.h index d464f78..76e6c9a 100644 --- a/cartographer/mapping/submaps.h +++ b/cartographer/mapping/submaps.h @@ -62,7 +62,7 @@ class Submap { : local_pose_(local_submap_pose) {} virtual ~Submap() {} - virtual proto::Submap ToProto(bool include_probability_grid_data) const = 0; + virtual proto::Submap ToProto(bool include_grid_data) const = 0; virtual void UpdateFromProto(const proto::Submap& proto) = 0; // Fills data into the 'response'. diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index 1009a9c..9d0499d 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/configuration_files/trajectory_builder_2d.lua @@ -82,10 +82,10 @@ TRAJECTORY_BUILDER_2D = { num_normal_samples = 4, sample_radius = 0.5, }, - project_sdf_distance_to_scan_normal = false, + project_sdf_distance_to_scan_normal = true, update_weight_range_exponent = 0, - update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0, - update_weight_distance_cell_to_hit_kernel_bandwith = 0, + update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0.5, + update_weight_distance_cell_to_hit_kernel_bandwith = 0.5, }, }, },