Kevin Daun 2018-08-03 11:56:35 +02:00 committed by Wally B. Feed
parent 12e11856af
commit 77886cb53b
16 changed files with 294 additions and 137 deletions

View File

@ -75,7 +75,8 @@ constexpr char kOdometryDataProtoString[] = "odometry_data {}";
constexpr char kFixedFramePoseDataProtoString[] = "fixed_frame_pose_data {}"; constexpr char kFixedFramePoseDataProtoString[] = "fixed_frame_pose_data {}";
constexpr char kLandmarkDataProtoString[] = "landmark_data {}"; constexpr char kLandmarkDataProtoString[] = "landmark_data {}";
class ClientServerTest : public ::testing::Test { template <class T>
class ClientServerTestBase : public T {
protected: protected:
void SetUp() override { void SetUp() override {
// TODO(cschuet): Due to the hard-coded addresses these tests will become // 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); 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) { void WaitForLocalSlamResults(size_t size) {
std::unique_lock<std::mutex> lock(local_slam_result_mutex_); std::unique_lock<std::mutex> lock(local_slam_result_mutex_);
local_slam_result_condition_.wait( local_slam_result_condition_.wait(
@ -212,13 +239,26 @@ class ClientServerTest : public ::testing::Test {
int number_of_insertion_results_; 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) { TEST_F(ClientServerTest, StartAndStopServer) {
InitializeRealServer(); InitializeRealServer();
server_->Start(); server_->Start();
server_->Shutdown(); server_->Shutdown();
} }
TEST_F(ClientServerTest, AddTrajectoryBuilder) { TEST_P(ClientServerTestByGridType, AddTrajectoryBuilder) {
if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
SetOptionsToTSDF2D();
}
InitializeRealServer(); InitializeRealServer();
server_->Start(); server_->Start();
InitializeStub(); InitializeStub();
@ -231,7 +271,10 @@ TEST_F(ClientServerTest, AddTrajectoryBuilder) {
server_->Shutdown(); server_->Shutdown();
} }
TEST_F(ClientServerTest, AddTrajectoryBuilderWithMock) { TEST_P(ClientServerTestByGridType, AddTrajectoryBuilderWithMock) {
if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
SetOptionsToTSDF2D();
}
InitializeServerWithMockMapBuilder(); InitializeServerWithMockMapBuilder();
server_->Start(); server_->Start();
InitializeStub(); InitializeStub();
@ -250,7 +293,10 @@ TEST_F(ClientServerTest, AddTrajectoryBuilderWithMock) {
server_->Shutdown(); 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() trajectory_builder_options_.mutable_trajectory_builder_2d_options()
->set_use_imu_data(true); ->set_use_imu_data(true);
InitializeRealServer(); InitializeRealServer();
@ -268,7 +314,10 @@ TEST_F(ClientServerTest, AddSensorData) {
server_->Shutdown(); server_->Shutdown();
} }
TEST_F(ClientServerTest, AddSensorDataWithMock) { TEST_P(ClientServerTestByGridType, AddSensorDataWithMock) {
if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
SetOptionsToTSDF2D();
}
InitializeServerWithMockMapBuilder(); InitializeServerWithMockMapBuilder();
server_->Start(); server_->Start();
InitializeStub(); InitializeStub();
@ -297,7 +346,10 @@ TEST_F(ClientServerTest, AddSensorDataWithMock) {
server_->Shutdown(); server_->Shutdown();
} }
TEST_F(ClientServerTest, LocalSlam2D) { TEST_P(ClientServerTestByGridType, LocalSlam2D) {
if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
SetOptionsToTSDF2D();
}
InitializeRealServer(); InitializeRealServer();
server_->Start(); server_->Start();
InitializeStub(); InitializeStub();
@ -328,7 +380,10 @@ TEST_F(ClientServerTest, LocalSlam2D) {
server_->Shutdown(); server_->Shutdown();
} }
TEST_F(ClientServerTest, LocalSlamAndDelete2D) { TEST_P(ClientServerTestByGridType, LocalSlamAndDelete2D) {
if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
SetOptionsToTSDF2D();
}
InitializeRealServer(); InitializeRealServer();
server_->Start(); server_->Start();
InitializeStub(); InitializeStub();
@ -401,7 +456,10 @@ TEST_F(ClientServerTest, GlobalSlam3D) {
server_->Shutdown(); server_->Shutdown();
} }
TEST_F(ClientServerTest, StartAndStopUploadingServerAndServer) { TEST_P(ClientServerTestByGridType, StartAndStopUploadingServerAndServer) {
if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
SetOptionsToTSDF2D();
}
InitializeRealServer(); InitializeRealServer();
server_->Start(); server_->Start();
InitializeRealUploadingServer(); InitializeRealUploadingServer();
@ -410,7 +468,10 @@ TEST_F(ClientServerTest, StartAndStopUploadingServerAndServer) {
server_->Shutdown(); server_->Shutdown();
} }
TEST_F(ClientServerTest, AddTrajectoryBuilderWithUploadingServer) { TEST_P(ClientServerTestByGridType, AddTrajectoryBuilderWithUploadingServer) {
if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
SetOptionsToTSDF2D();
}
InitializeRealServer(); InitializeRealServer();
server_->Start(); server_->Start();
InitializeRealUploadingServer(); InitializeRealUploadingServer();
@ -433,7 +494,10 @@ TEST_F(ClientServerTest, AddTrajectoryBuilderWithUploadingServer) {
server_->Shutdown(); server_->Shutdown();
} }
TEST_F(ClientServerTest, LocalSlam2DWithUploadingServer) { TEST_P(ClientServerTestByGridType, LocalSlam2DWithUploadingServer) {
if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
SetOptionsToTSDF2D();
}
InitializeRealServer(); InitializeRealServer();
server_->Start(); server_->Start();
InitializeStub(); InitializeStub();
@ -482,7 +546,10 @@ TEST_F(ClientServerTest, LocalSlam2DWithUploadingServer) {
server_->Shutdown(); server_->Shutdown();
} }
TEST_F(ClientServerTest, LocalSlam2DUplinkServerRestarting) { TEST_P(ClientServerTestByGridType, LocalSlam2DUplinkServerRestarting) {
if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
SetOptionsToTSDF2D();
}
InitializeRealServer(); InitializeRealServer();
server_->Start(); server_->Start();
InitializeStub(); InitializeStub();
@ -528,7 +595,10 @@ TEST_F(ClientServerTest, LocalSlam2DUplinkServerRestarting) {
server_->WaitForShutdown(); server_->WaitForShutdown();
} }
TEST_F(ClientServerTest, LoadStateAndDelete) { TEST_P(ClientServerTestByGridType, LoadStateAndDelete) {
if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
SetOptionsToTSDF2D();
}
InitializeRealServer(); InitializeRealServer();
server_->Start(); server_->Start();
InitializeStub(); InitializeStub();
@ -568,7 +638,10 @@ TEST_F(ClientServerTest, LoadStateAndDelete) {
// TODO(gaschler): Test-cover LoadStateFromFile. // TODO(gaschler): Test-cover LoadStateFromFile.
TEST_F(ClientServerTest, LocalSlam2DHandlesInvalidRequests) { TEST_P(ClientServerTestByGridType, LocalSlam2DHandlesInvalidRequests) {
if (GetParam() == ::cartographer::mapping::GridType::TSDF) {
SetOptionsToTSDF2D();
}
InitializeRealServer(); InitializeRealServer();
server_->Start(); server_->Start();
InitializeStub(); InitializeStub();

View File

@ -63,7 +63,6 @@ Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost,
float max_correspondence_cost, float max_correspondence_cost,
ValueConversionTables* conversion_tables) ValueConversionTables* conversion_tables)
: limits_(limits), : limits_(limits),
grid_type_(GridType::NONE),
correspondence_cost_cells_( correspondence_cost_cells_(
limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells, limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells,
kUnknownCorrespondenceValue), kUnknownCorrespondenceValue),
@ -78,7 +77,6 @@ Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost,
Grid2D::Grid2D(const proto::Grid2D& proto, Grid2D::Grid2D(const proto::Grid2D& proto,
ValueConversionTables* conversion_tables) ValueConversionTables* conversion_tables)
: limits_(proto.limits()), : limits_(proto.limits()),
grid_type_(GridType::NONE),
correspondence_cost_cells_(), correspondence_cost_cells_(),
min_correspondence_cost_(MinCorrespondenceCostFromProto(proto)), min_correspondence_cost_(MinCorrespondenceCostFromProto(proto)),
max_correspondence_cost_(MaxCorrespondenceCostFromProto(proto)), max_correspondence_cost_(MaxCorrespondenceCostFromProto(proto)),

View File

@ -32,7 +32,7 @@ namespace mapping {
proto::GridOptions2D CreateGridOptions2D( proto::GridOptions2D CreateGridOptions2D(
common::LuaParameterDictionary* const parameter_dictionary); common::LuaParameterDictionary* const parameter_dictionary);
enum class GridType { NONE, PROBABILITY_GRID, TSDF }; enum class GridType { PROBABILITY_GRID, TSDF };
class Grid2D : public GridInterface { class Grid2D : public GridInterface {
public: public:
@ -45,13 +45,14 @@ class Grid2D : public GridInterface {
// Returns the limits of this Grid2D. // Returns the limits of this Grid2D.
const MapLimits& limits() const { return limits_; } const MapLimits& limits() const { return limits_; }
GridType grid_type() const { return grid_type_; }
// Finishes the update sequence. // Finishes the update sequence.
void FinishUpdate(); void FinishUpdate();
// Returns the correspondence cost of the cell with 'cell_index'. // Returns the correspondence cost of the cell with 'cell_index'.
float GetCorrespondenceCost(const Eigen::Array2i& cell_index) const; float GetCorrespondenceCost(const Eigen::Array2i& cell_index) const;
virtual GridType GetGridType() const = 0;
// Returns the minimum possible correspondence cost. // Returns the minimum possible correspondence cost.
float GetMinCorrespondenceCost() const { return min_correspondence_cost_; } float GetMinCorrespondenceCost() const { return min_correspondence_cost_; }
@ -96,7 +97,6 @@ class Grid2D : public GridInterface {
return &correspondence_cost_cells_; return &correspondence_cost_cells_;
} }
GridType* mutable_grid_type() { return &grid_type_; }
std::vector<int>* mutable_update_indices() { return &update_indices_; } std::vector<int>* mutable_update_indices() { return &update_indices_; }
Eigen::AlignedBox2i* mutable_known_cells_box() { return &known_cells_box_; } Eigen::AlignedBox2i* mutable_known_cells_box() { return &known_cells_box_; }
@ -105,7 +105,6 @@ class Grid2D : public GridInterface {
private: private:
MapLimits limits_; MapLimits limits_;
GridType grid_type_;
std::vector<uint16> correspondence_cost_cells_; std::vector<uint16> correspondence_cost_cells_;
float min_correspondence_cost_; float min_correspondence_cost_;
float max_correspondence_cost_; float max_correspondence_cost_;

View File

@ -28,15 +28,12 @@ ProbabilityGrid::ProbabilityGrid(const MapLimits& limits,
ValueConversionTables* conversion_tables) ValueConversionTables* conversion_tables)
: Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost, : Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost,
conversion_tables), conversion_tables),
conversion_tables_(conversion_tables) { conversion_tables_(conversion_tables) {}
*mutable_grid_type() = GridType::PROBABILITY_GRID;
}
ProbabilityGrid::ProbabilityGrid(const proto::Grid2D& proto, ProbabilityGrid::ProbabilityGrid(const proto::Grid2D& proto,
ValueConversionTables* conversion_tables) ValueConversionTables* conversion_tables)
: Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) { : Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) {
CHECK(proto.has_probability_grid_2d()); CHECK(proto.has_probability_grid_2d());
*mutable_grid_type() = GridType::PROBABILITY_GRID;
} }
// Sets the probability of the cell at 'cell_index' to the given // 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; return true;
} }
GridType ProbabilityGrid::GetGridType() const {
return GridType::PROBABILITY_GRID;
}
// Returns the probability of the cell with 'cell_index'. // Returns the probability of the cell with 'cell_index'.
float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const { float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const {
if (!limits().Contains(cell_index)) return kMinProbability; if (!limits().Contains(cell_index)) return kMinProbability;

View File

@ -50,12 +50,14 @@ class ProbabilityGrid : public Grid2D {
bool ApplyLookupTable(const Eigen::Array2i& cell_index, bool ApplyLookupTable(const Eigen::Array2i& cell_index,
const std::vector<uint16>& table); const std::vector<uint16>& table);
GridType GetGridType() const override;
// Returns the probability of the cell with 'cell_index'. // Returns the probability of the cell with 'cell_index'.
float GetProbability(const Eigen::Array2i& cell_index) const; float GetProbability(const Eigen::Array2i& cell_index) const;
virtual proto::Grid2D ToProto() const override; proto::Grid2D ToProto() const override;
virtual std::unique_ptr<Grid2D> ComputeCroppedGrid() const override; std::unique_ptr<Grid2D> ComputeCroppedGrid() const override;
virtual bool DrawToSubmapTexture( bool DrawToSubmapTexture(
proto::SubmapQuery::Response::SubmapTexture* const texture, proto::SubmapQuery::Response::SubmapTexture* const texture,
transform::Rigid3d local_pose) const override; transform::Rigid3d local_pose) const override;

View File

@ -44,7 +44,7 @@ TEST(ProbabilityGridTest, ProtoConstructor) {
ValueConversionTables conversion_tables; ValueConversionTables conversion_tables;
ProbabilityGrid grid(proto, &conversion_tables); ProbabilityGrid grid(proto, &conversion_tables);
EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString()); 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 // TODO(macmason): Figure out how to test the contents of cells_ and
// {min, max}_{x, y}_ gracefully. // {min, max}_{x, y}_ gracefully.
@ -55,7 +55,7 @@ TEST(ProbabilityGridTest, ConstructorGridType) {
ProbabilityGrid probability_grid( ProbabilityGrid probability_grid(
MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)),
&conversion_tables); &conversion_tables);
EXPECT_EQ(probability_grid.grid_type(), GridType::PROBABILITY_GRID); EXPECT_EQ(probability_grid.GetGridType(), GridType::PROBABILITY_GRID);
} }
TEST(ProbabilityGridTest, ToProto) { TEST(ProbabilityGridTest, ToProto) {

View File

@ -26,6 +26,7 @@
#include "absl/memory/memory.h" #include "absl/memory/memory.h"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.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 "cartographer/mapping/range_data_inserter_interface.h"
#include "glog/logging.h" #include "glog/logging.h"
@ -54,6 +55,11 @@ proto::SubmapsOptions2D CreateSubmapsOptions2D(
proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D) { proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D) {
valid_range_data_inserter_grid_combination = true; 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) CHECK(valid_range_data_inserter_grid_combination)
<< "Invalid combination grid_type " << grid_type << "Invalid combination grid_type " << grid_type
<< " with range_data_inserter_type " << range_data_inserter_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())), : Submap(transform::ToRigid3(proto.local_pose())),
conversion_tables_(conversion_tables) { conversion_tables_(conversion_tables) {
if (proto.has_grid()) { if (proto.has_grid()) {
CHECK(proto.grid().has_probability_grid_2d()); if (proto.grid().has_probability_grid_2d()) {
grid_ = grid_ =
absl::make_unique<ProbabilityGrid>(proto.grid(), conversion_tables_); absl::make_unique<ProbabilityGrid>(proto.grid(), conversion_tables_);
} else if (proto.grid().has_tsdf_2d()) {
grid_ = absl::make_unique<TSDF2D>(proto.grid(), conversion_tables_);
} else {
LOG(FATAL) << "proto::Submap2D has grid with unknown type.";
}
} }
set_num_range_data(proto.num_range_data()); set_num_range_data(proto.num_range_data());
set_finished(proto.finished()); set_finished(proto.finished());
} }
proto::Submap Submap2D::ToProto( proto::Submap Submap2D::ToProto(const bool include_grid_data) const {
const bool include_probability_grid_data) const {
proto::Submap proto; proto::Submap proto;
auto* const submap_2d = proto.mutable_submap_2d(); auto* const submap_2d = proto.mutable_submap_2d();
*submap_2d->mutable_local_pose() = transform::ToProto(local_pose()); *submap_2d->mutable_local_pose() = transform::ToProto(local_pose());
submap_2d->set_num_range_data(num_range_data()); submap_2d->set_num_range_data(num_range_data());
submap_2d->set_finished(finished()); submap_2d->set_finished(finished());
if (include_probability_grid_data) { if (include_grid_data) {
CHECK(grid_); CHECK(grid_);
*submap_2d->mutable_grid() = grid_->ToProto(); *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_num_range_data(submap_2d.num_range_data());
set_finished(submap_2d.finished()); set_finished(submap_2d.finished());
if (proto.submap_2d().has_grid()) { if (proto.submap_2d().has_grid()) {
CHECK(proto.submap_2d().grid().has_probability_grid_2d()); if (proto.submap_2d().grid().has_probability_grid_2d()) {
grid_ = absl::make_unique<ProbabilityGrid>(submap_2d.grid(), grid_ = absl::make_unique<ProbabilityGrid>(proto.submap_2d().grid(),
conversion_tables_); conversion_tables_);
} else if (proto.submap_2d().grid().has_tsdf_2d()) {
grid_ = absl::make_unique<TSDF2D>(proto.submap_2d().grid(),
conversion_tables_);
} else {
LOG(FATAL) << "proto::Submap2D has grid with unknown type.";
}
} }
} }
@ -159,21 +175,50 @@ std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
std::unique_ptr<RangeDataInserterInterface> std::unique_ptr<RangeDataInserterInterface>
ActiveSubmaps2D::CreateRangeDataInserter() { ActiveSubmaps2D::CreateRangeDataInserter() {
return absl::make_unique<ProbabilityGridRangeDataInserter2D>( switch (options_.range_data_inserter_options().range_data_inserter_type()) {
options_.range_data_inserter_options() case proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D:
.probability_grid_range_data_inserter_options_2d()); return absl::make_unique<ProbabilityGridRangeDataInserter2D>(
options_.range_data_inserter_options()
.probability_grid_range_data_inserter_options_2d());
case proto::RangeDataInserterOptions::TSDF_INSERTER_2D:
return absl::make_unique<TSDFRangeDataInserter2D>(
options_.range_data_inserter_options()
.tsdf_range_data_inserter_options_2d());
default:
LOG(FATAL) << "Unknown RangeDataInserterType.";
}
} }
std::unique_ptr<GridInterface> ActiveSubmaps2D::CreateGrid( std::unique_ptr<GridInterface> ActiveSubmaps2D::CreateGrid(
const Eigen::Vector2f& origin) { const Eigen::Vector2f& origin) {
constexpr int kInitialSubmapSize = 100; constexpr int kInitialSubmapSize = 100;
float resolution = options_.grid_options_2d().resolution(); float resolution = options_.grid_options_2d().resolution();
return absl::make_unique<ProbabilityGrid>( switch (options_.grid_options_2d().grid_type()) {
MapLimits(resolution, case proto::GridOptions2D::PROBABILITY_GRID:
origin.cast<double>() + 0.5 * kInitialSubmapSize * resolution * return absl::make_unique<ProbabilityGrid>(
Eigen::Vector2d::Ones(), MapLimits(resolution,
CellLimits(kInitialSubmapSize, kInitialSubmapSize)), origin.cast<double>() + 0.5 * kInitialSubmapSize *
&conversion_tables_); resolution *
Eigen::Vector2d::Ones(),
CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
&conversion_tables_);
case proto::GridOptions2D::TSDF:
return absl::make_unique<TSDF2D>(
MapLimits(resolution,
origin.cast<double>() + 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) { void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) {

View File

@ -47,7 +47,7 @@ class Submap2D : public Submap {
explicit Submap2D(const proto::Submap2D& proto, explicit Submap2D(const proto::Submap2D& proto,
ValueConversionTables* conversion_tables); 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 UpdateFromProto(const proto::Submap& proto) override;
void ToResponseProto(const transform::Rigid3d& global_submap_pose, void ToResponseProto(const transform::Rigid3d& global_submap_pose,

View File

@ -30,15 +30,12 @@ TSDF2D::TSDF2D(const MapLimits& limits, float truncation_distance,
truncation_distance, max_weight, conversion_tables_)), truncation_distance, max_weight, conversion_tables_)),
weight_cells_( weight_cells_(
limits.cell_limits().num_x_cells * limits.cell_limits().num_y_cells, limits.cell_limits().num_x_cells * limits.cell_limits().num_y_cells,
value_converter_->getUnknownWeightValue()) { value_converter_->getUnknownWeightValue()) {}
*mutable_grid_type() = GridType::TSDF;
}
TSDF2D::TSDF2D(const proto::Grid2D& proto, TSDF2D::TSDF2D(const proto::Grid2D& proto,
ValueConversionTables* conversion_tables) ValueConversionTables* conversion_tables)
: Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) { : Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) {
CHECK(proto.has_tsdf_2d()); CHECK(proto.has_tsdf_2d());
*mutable_grid_type() = GridType::TSDF;
value_converter_ = absl::make_unique<TSDValueConverter>( value_converter_ = absl::make_unique<TSDValueConverter>(
proto.tsdf_2d().truncation_distance(), proto.tsdf_2d().max_weight(), proto.tsdf_2d().truncation_distance(), proto.tsdf_2d().max_weight(),
conversion_tables_); conversion_tables_);
@ -70,6 +67,8 @@ void TSDF2D::SetCell(const Eigen::Array2i& cell_index, float tsd,
*weight_cell = value_converter_->WeightToValue(weight); *weight_cell = value_converter_->WeightToValue(weight);
} }
GridType TSDF2D::GetGridType() const { return GridType::TSDF; }
float TSDF2D::GetTSD(const Eigen::Array2i& cell_index) const { float TSDF2D::GetTSD(const Eigen::Array2i& cell_index) const {
if (limits().Contains(cell_index)) { if (limits().Contains(cell_index)) {
return value_converter_->ValueToTSD( return value_converter_->ValueToTSD(

View File

@ -37,15 +37,16 @@ class TSDF2D : public Grid2D {
void SetCell(const Eigen::Array2i& cell_index, const float tsd, void SetCell(const Eigen::Array2i& cell_index, const float tsd,
const float weight); const float weight);
GridType GetGridType() const override;
float GetTSD(const Eigen::Array2i& cell_index) const; float GetTSD(const Eigen::Array2i& cell_index) const;
float GetWeight(const Eigen::Array2i& cell_index) const; float GetWeight(const Eigen::Array2i& cell_index) const;
std::pair<float, float> GetTSDAndWeight( std::pair<float, float> GetTSDAndWeight(
const Eigen::Array2i& cell_index) const; 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; proto::Grid2D ToProto() const override;
virtual std::unique_ptr<Grid2D> ComputeCroppedGrid() const override; std::unique_ptr<Grid2D> ComputeCroppedGrid() const override;
virtual bool DrawToSubmapTexture( bool DrawToSubmapTexture(
proto::SubmapQuery::Response::SubmapTexture* const texture, proto::SubmapQuery::Response::SubmapTexture* const texture,
transform::Rigid3d local_pose) const override; transform::Rigid3d local_pose) const override;
bool CellIsUpdated(const Eigen::Array2i& cell_index) const; bool CellIsUpdated(const Eigen::Array2i& cell_index) const;

View File

@ -48,7 +48,7 @@ TEST(TSDF2DTest, ProtoConstructor) {
ValueConversionTables conversion_tables; ValueConversionTables conversion_tables;
TSDF2D grid(proto, &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()); EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString());
} }
@ -56,7 +56,7 @@ TEST(TSDF2DTest, ConstructorGridType) {
ValueConversionTables conversion_tables; ValueConversionTables conversion_tables;
TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), 1.0f, TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), 1.0f,
10.0f, &conversion_tables); 10.0f, &conversion_tables);
EXPECT_EQ(tsdf.grid_type(), GridType::TSDF); EXPECT_EQ(tsdf.GetGridType(), GridType::TSDF);
} }
TEST(TSDF2DTest, ToProto) { TEST(TSDF2DTest, ToProto) {

View File

@ -26,6 +26,7 @@
#include "cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h" #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/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/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 "cartographer/transform/transform.h"
#include "ceres/ceres.h" #include "ceres/ceres.h"
#include "glog/logging.h" #include "glog/logging.h"
@ -70,12 +71,23 @@ void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
initial_pose_estimate.rotation().angle()}; initial_pose_estimate.rotation().angle()};
ceres::Problem problem; ceres::Problem problem;
CHECK_GT(options_.occupied_space_weight(), 0.); CHECK_GT(options_.occupied_space_weight(), 0.);
problem.AddResidualBlock( if (grid.GetGridType() == GridType::PROBABILITY_GRID) {
CreateOccupiedSpaceCostFunction2D( problem.AddResidualBlock(
options_.occupied_space_weight() / CreateOccupiedSpaceCostFunction2D(
std::sqrt(static_cast<double>(point_cloud.size())), options_.occupied_space_weight() /
point_cloud, grid), std::sqrt(static_cast<double>(point_cloud.size())),
nullptr /* loss function */, ceres_pose_estimate); 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<double>(point_cloud.size())),
point_cloud, static_cast<const TSDF2D&>(grid)),
nullptr /* loss function */, ceres_pose_estimate);
} else {
LOG(FATAL) << "Unknown GridType.";
}
CHECK_GT(options_.translation_weight(), 0.); CHECK_GT(options_.translation_weight(), 0.);
problem.AddResidualBlock( problem.AddResidualBlock(
TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction( TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(

View File

@ -18,6 +18,7 @@
#include "cartographer/common/config.h" #include "cartographer/common/config.h"
#include "cartographer/io/proto_stream.h" #include "cartographer/io/proto_stream.h"
#include "cartographer/mapping/2d/grid_2d.h"
#include "cartographer/mapping/internal/testing/test_helpers.h" #include "cartographer/mapping/internal/testing/test_helpers.h"
#include "gmock/gmock.h" #include "gmock/gmock.h"
#include "gtest/gtest.h" #include "gtest/gtest.h"
@ -34,7 +35,8 @@ constexpr double kDuration = 4.; // Seconds.
constexpr double kTimeStep = 0.1; // Seconds. constexpr double kTimeStep = 0.1; // Seconds.
constexpr double kTravelDistance = 1.2; // Meters. constexpr double kTravelDistance = 1.2; // Meters.
class MapBuilderTest : public ::testing::Test { template <class T>
class MapBuilderTestBase : public T {
protected: protected:
void SetUp() override { void SetUp() override {
// Global SLAM optimization is not executed. // Global SLAM optimization is not executed.
@ -70,6 +72,25 @@ class MapBuilderTest : public ::testing::Test {
map_builder_options_.set_use_trajectory_builder_3d(true); 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() { void SetOptionsEnableGlobalOptimization() {
map_builder_options_.mutable_pose_graph_options() map_builder_options_.mutable_pose_graph_options()
->set_optimize_every_n_nodes(3); ->set_optimize_every_n_nodes(3);
@ -111,7 +132,26 @@ class MapBuilderTest : public ::testing::Test {
std::vector<::cartographer::transform::Rigid3d> local_slam_result_poses_; 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<GridType>> {};
class MapBuilderTestByGridTypeAndDimensions
: public MapBuilderTestBase<
::testing::TestWithParam<std::pair<GridType, int /* dimensions */>>> {
};
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(); BuildMapBuilder();
int trajectory_id = map_builder_->AddTrajectoryBuilder( int trajectory_id = map_builder_->AddTrajectoryBuilder(
{kRangeSensorId}, trajectory_builder_options_, {kRangeSensorId}, trajectory_builder_options_,
@ -124,21 +164,8 @@ TEST_F(MapBuilderTest, TrajectoryAddFinish2D) {
EXPECT_TRUE(map_builder_->pose_graph()->IsTrajectoryFinished(trajectory_id)); EXPECT_TRUE(map_builder_->pose_graph()->IsTrajectoryFinished(trajectory_id));
} }
TEST_F(MapBuilderTest, TrajectoryAddFinish3D) { TEST_P(MapBuilderTestByGridType, LocalSlam2D) {
SetOptionsTo3D(); if (GetParam() == GridType::TSDF) SetOptionsToTSDF2D();
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) {
BuildMapBuilder(); BuildMapBuilder();
int trajectory_id = map_builder_->AddTrajectoryBuilder( int trajectory_id = map_builder_->AddTrajectoryBuilder(
{kRangeSensorId}, trajectory_builder_options_, {kRangeSensorId}, trajectory_builder_options_,
@ -187,7 +214,8 @@ TEST_F(MapBuilderTest, LocalSlam3D) {
0.1 * kTravelDistance); 0.1 * kTravelDistance);
} }
TEST_F(MapBuilderTest, GlobalSlam2D) { TEST_P(MapBuilderTestByGridType, GlobalSlam2D) {
if (GetParam() == GridType::TSDF) SetOptionsToTSDF2D();
SetOptionsEnableGlobalOptimization(); SetOptionsEnableGlobalOptimization();
BuildMapBuilder(); BuildMapBuilder();
int trajectory_id = map_builder_->AddTrajectoryBuilder( int trajectory_id = map_builder_->AddTrajectoryBuilder(
@ -268,7 +296,8 @@ TEST_F(MapBuilderTest, GlobalSlam3D) {
0.1 * kTravelDistance); 0.1 * kTravelDistance);
} }
TEST_F(MapBuilderTest, DeleteFinishedTrajectory2D) { TEST_P(MapBuilderTestByGridType, DeleteFinishedTrajectory2D) {
if (GetParam() == GridType::TSDF) SetOptionsToTSDF2D();
SetOptionsEnableGlobalOptimization(); SetOptionsEnableGlobalOptimization();
BuildMapBuilder(); BuildMapBuilder();
int trajectory_id = CreateTrajectoryWithFakeData(); int trajectory_id = CreateTrajectoryWithFakeData();
@ -309,60 +338,57 @@ TEST_F(MapBuilderTest, DeleteFinishedTrajectory2D) {
0); 0);
} }
TEST_F(MapBuilderTest, SaveLoadState) { TEST_P(MapBuilderTestByGridTypeAndDimensions, SaveLoadState) {
for (int dimensions : {2, 3}) { if (GetParam().second == 3) SetOptionsTo3D();
if (dimensions == 3) { if (GetParam().first == GridType::TSDF) SetOptionsToTSDF2D();
SetOptionsTo3D(); trajectory_builder_options_.mutable_trajectory_builder_2d_options()
} ->set_use_imu_data(true);
trajectory_builder_options_.mutable_trajectory_builder_2d_options() BuildMapBuilder();
->set_use_imu_data(true); int trajectory_id = map_builder_->AddTrajectoryBuilder(
BuildMapBuilder(); {kRangeSensorId, kIMUSensorId}, trajectory_builder_options_,
int trajectory_id = map_builder_->AddTrajectoryBuilder( GetLocalSlamResultCallback());
{kRangeSensorId, kIMUSensorId}, trajectory_builder_options_, TrajectoryBuilderInterface* trajectory_builder =
GetLocalSlamResultCallback()); map_builder_->GetTrajectoryBuilder(trajectory_id);
TrajectoryBuilderInterface* trajectory_builder = const auto measurements = testing::GenerateFakeRangeMeasurements(
map_builder_->GetTrajectoryBuilder(trajectory_id); kTravelDistance, kDuration, kTimeStep);
const auto measurements = testing::GenerateFakeRangeMeasurements( for (const auto& measurement : measurements) {
kTravelDistance, kDuration, kTimeStep); trajectory_builder->AddSensorData(kRangeSensorId.id, measurement);
for (const auto& measurement : measurements) { trajectory_builder->AddSensorData(
trajectory_builder->AddSensorData(kRangeSensorId.id, measurement); kIMUSensorId.id,
trajectory_builder->AddSensorData( sensor::ImuData{measurement.time, Eigen::Vector3d(0., 0., 9.8),
kIMUSensorId.id, Eigen::Vector3d::Zero()});
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));
} }
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(); BuildMapBuilder();
int temp_trajectory_id = CreateTrajectoryWithFakeData(); int temp_trajectory_id = CreateTrajectoryWithFakeData();
map_builder_->pose_graph()->RunFinalOptimization(); map_builder_->pose_graph()->RunFinalOptimization();

View File

@ -20,6 +20,7 @@ message GridOptions2D {
enum GridType { enum GridType {
INVALID_GRID = 0; INVALID_GRID = 0;
PROBABILITY_GRID = 1; PROBABILITY_GRID = 1;
TSDF = 2;
} }
GridType grid_type = 1; GridType grid_type = 1;

View File

@ -62,7 +62,7 @@ class Submap {
: local_pose_(local_submap_pose) {} : local_pose_(local_submap_pose) {}
virtual ~Submap() {} 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; virtual void UpdateFromProto(const proto::Submap& proto) = 0;
// Fills data into the 'response'. // Fills data into the 'response'.

View File

@ -82,10 +82,10 @@ TRAJECTORY_BUILDER_2D = {
num_normal_samples = 4, num_normal_samples = 4,
sample_radius = 0.5, 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_range_exponent = 0,
update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0, update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0.5,
update_weight_distance_cell_to_hit_kernel_bandwith = 0, update_weight_distance_cell_to_hit_kernel_bandwith = 0.5,
}, },
}, },
}, },