Integrate TSDF components into SLAM pipeline. (#1361)
[RFC=0019](https://github.com/googlecartographer/rfcs/blob/master/text/0019-probability-grid-and-submap2d-restructuring.md)master
parent
12e11856af
commit
77886cb53b
|
@ -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();
|
||||||
|
|
|
@ -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)),
|
||||||
|
|
|
@ -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_;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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() {
|
||||||
|
switch (options_.range_data_inserter_options().range_data_inserter_type()) {
|
||||||
|
case proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D:
|
||||||
return absl::make_unique<ProbabilityGridRangeDataInserter2D>(
|
return absl::make_unique<ProbabilityGridRangeDataInserter2D>(
|
||||||
options_.range_data_inserter_options()
|
options_.range_data_inserter_options()
|
||||||
.probability_grid_range_data_inserter_options_2d());
|
.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();
|
||||||
|
switch (options_.grid_options_2d().grid_type()) {
|
||||||
|
case proto::GridOptions2D::PROBABILITY_GRID:
|
||||||
return absl::make_unique<ProbabilityGrid>(
|
return absl::make_unique<ProbabilityGrid>(
|
||||||
MapLimits(resolution,
|
MapLimits(resolution,
|
||||||
origin.cast<double>() + 0.5 * kInitialSubmapSize * resolution *
|
origin.cast<double>() + 0.5 * kInitialSubmapSize *
|
||||||
|
resolution *
|
||||||
Eigen::Vector2d::Ones(),
|
Eigen::Vector2d::Ones(),
|
||||||
CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
|
CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
|
||||||
&conversion_tables_);
|
&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) {
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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(
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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.);
|
||||||
|
if (grid.GetGridType() == GridType::PROBABILITY_GRID) {
|
||||||
problem.AddResidualBlock(
|
problem.AddResidualBlock(
|
||||||
CreateOccupiedSpaceCostFunction2D(
|
CreateOccupiedSpaceCostFunction2D(
|
||||||
options_.occupied_space_weight() /
|
options_.occupied_space_weight() /
|
||||||
std::sqrt(static_cast<double>(point_cloud.size())),
|
std::sqrt(static_cast<double>(point_cloud.size())),
|
||||||
point_cloud, grid),
|
point_cloud, grid),
|
||||||
nullptr /* loss function */, ceres_pose_estimate);
|
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(
|
||||||
|
|
|
@ -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,11 +338,9 @@ 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()
|
trajectory_builder_options_.mutable_trajectory_builder_2d_options()
|
||||||
->set_use_imu_data(true);
|
->set_use_imu_data(true);
|
||||||
BuildMapBuilder();
|
BuildMapBuilder();
|
||||||
|
@ -351,18 +378,17 @@ TEST_F(MapBuilderTest, SaveLoadState) {
|
||||||
auto trajectory_remapping =
|
auto trajectory_remapping =
|
||||||
map_builder_->LoadState(&reader, false /* load_frozen_state */);
|
map_builder_->LoadState(&reader, false /* load_frozen_state */);
|
||||||
map_builder_->pose_graph()->RunFinalOptimization();
|
map_builder_->pose_graph()->RunFinalOptimization();
|
||||||
EXPECT_EQ(num_constraints,
|
EXPECT_EQ(num_constraints, map_builder_->pose_graph()->constraints().size());
|
||||||
map_builder_->pose_graph()->constraints().size());
|
|
||||||
ASSERT_EQ(trajectory_remapping.size(), 1);
|
ASSERT_EQ(trajectory_remapping.size(), 1);
|
||||||
int new_trajectory_id = trajectory_remapping.begin()->second;
|
int new_trajectory_id = trajectory_remapping.begin()->second;
|
||||||
EXPECT_EQ(
|
EXPECT_EQ(
|
||||||
num_nodes,
|
num_nodes,
|
||||||
map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero(
|
map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero(
|
||||||
new_trajectory_id));
|
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();
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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'.
|
||||||
|
|
|
@ -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,
|
||||||
},
|
},
|
||||||
},
|
},
|
||||||
},
|
},
|
||||||
|
|
Loading…
Reference in New Issue