12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

99
cartographer/cloud/internal/client_server_test.cc
View File

@ -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 T>
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<std::mutex> 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();

2
cartographer/mapping/2d/grid_2d.cc
View File

@ -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)),

9
cartographer/mapping/2d/grid_2d.h
View File

@ -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<int>* 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<uint16> correspondence_cost_cells_;
float min_correspondence_cost_;
float max_correspondence_cost_;

9
cartographer/mapping/2d/probability_grid.cc
View File

@ -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;

8
cartographer/mapping/2d/probability_grid.h
View File

@ -50,12 +50,14 @@ class ProbabilityGrid : public Grid2D {
bool ApplyLookupTable(const Eigen::Array2i& cell_index,
const std::vector<uint16>& 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<Grid2D> ComputeCroppedGrid() const override;
virtual bool DrawToSubmapTexture(
proto::Grid2D ToProto() const override;
std::unique_ptr<Grid2D> ComputeCroppedGrid() const override;
bool DrawToSubmapTexture(
proto::SubmapQuery::Response::SubmapTexture* const texture,
transform::Rigid3d local_pose) const override;

4
cartographer/mapping/2d/probability_grid_test.cc
View File

@ -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) {

81
cartographer/mapping/2d/submap_2d.cc
View File

@ -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<ProbabilityGrid>(proto.grid(), conversion_tables_);
if (proto.grid().has_probability_grid_2d()) {
grid_ =
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_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<ProbabilityGrid>(submap_2d.grid(),
conversion_tables_);
if (proto.submap_2d().grid().has_probability_grid_2d()) {
grid_ = absl::make_unique<ProbabilityGrid>(proto.submap_2d().grid(),
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>
ActiveSubmaps2D::CreateRangeDataInserter() {
return absl::make_unique<ProbabilityGridRangeDataInserter2D>(
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<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(
const Eigen::Vector2f& origin) {
constexpr int kInitialSubmapSize = 100;
float resolution = options_.grid_options_2d().resolution();
return absl::make_unique<ProbabilityGrid>(
MapLimits(resolution,
origin.cast<double>() + 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<ProbabilityGrid>(
MapLimits(resolution,
origin.cast<double>() + 0.5 * kInitialSubmapSize *
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) {

2
cartographer/mapping/2d/submap_2d.h
View File

@ -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,

7
cartographer/mapping/2d/tsdf_2d.cc
View File

@ -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<TSDValueConverter>(
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(

7
cartographer/mapping/2d/tsdf_2d.h
View File

@ -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<float, float> 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<Grid2D> ComputeCroppedGrid() const override;
virtual bool DrawToSubmapTexture(
std::unique_ptr<Grid2D> 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;

4
cartographer/mapping/2d/tsdf_2d_test.cc
View File

@ -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) {

24
cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc
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/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<double>(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<double>(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<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.);
problem.AddResidualBlock(
TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(

166
cartographer/mapping/map_builder_test.cc
View File

@ -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 T>
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<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();
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();

1
cartographer/mapping/proto/2d/grid_2d_options.proto
View File

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

2
cartographer/mapping/submaps.h
View File

@ -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'.

6
configuration_files/trajectory_builder_2d.lua
View File

@ -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,
},
},
},