From 15ecb88a92cca38b5303d8c6a94beb1fcb8fb838 Mon Sep 17 00:00:00 2001 From: Kevin Daun Date: Mon, 25 Jun 2018 11:40:12 +0200 Subject: [PATCH] Add TSDF2D Grid (#1209) - Adds TSDF2D Grid - Adds tests for TSDF2D Grid - Introduces Grid2D::GrowLimits(...) for multiple grids to reduce code duplication between TSDF2D and Grid2D --- cartographer/mapping/2d/grid_2d.cc | 30 ++-- cartographer/mapping/2d/grid_2d.h | 8 +- cartographer/mapping/2d/tsdf_2d.cc | 173 ++++++++++++++++++++ cartographer/mapping/2d/tsdf_2d.h | 58 +++++++ cartographer/mapping/2d/tsdf_2d_test.cc | 155 ++++++++++++++++++ cartographer/mapping/proto/2d/grid_2d.proto | 2 + cartographer/mapping/proto/2d/tsdf_2d.proto | 23 +++ 7 files changed, 436 insertions(+), 13 deletions(-) create mode 100644 cartographer/mapping/2d/tsdf_2d.cc create mode 100644 cartographer/mapping/2d/tsdf_2d.h create mode 100644 cartographer/mapping/2d/tsdf_2d_test.cc create mode 100644 cartographer/mapping/proto/2d/tsdf_2d.proto diff --git a/cartographer/mapping/2d/grid_2d.cc b/cartographer/mapping/2d/grid_2d.cc index 43221df..9672a59 100644 --- a/cartographer/mapping/2d/grid_2d.cc +++ b/cartographer/mapping/2d/grid_2d.cc @@ -114,6 +114,13 @@ void Grid2D::ComputeCroppedLimits(Eigen::Array2i* const offset, // these coordinates going forward. This method must be called immediately // after 'FinishUpdate', before any calls to 'ApplyLookupTable'. void Grid2D::GrowLimits(const Eigen::Vector2f& point) { + GrowLimits(point, {mutable_correspondence_cost_cells()}, + {kUnknownCorrespondenceValue}); +} + +void Grid2D::GrowLimits(const Eigen::Vector2f& point, + const std::vector*>& grids, + const std::vector& grids_unknown_cell_values) { CHECK(update_indices_.empty()); while (!limits_.Contains(limits_.GetCellIndex(point))) { const int x_offset = limits_.cell_limits().num_x_cells / 2; @@ -128,15 +135,18 @@ void Grid2D::GrowLimits(const Eigen::Vector2f& point) { const int offset = x_offset + stride * y_offset; const int new_size = new_limits.cell_limits().num_x_cells * new_limits.cell_limits().num_y_cells; - std::vector new_cells(new_size, kUnknownCorrespondenceValue); - for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) { - for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) { - new_cells[offset + j + i * stride] = - correspondence_cost_cells_[j + - i * limits_.cell_limits().num_x_cells]; + + for (size_t grid_index = 0; grid_index < grids.size(); ++grid_index) { + std::vector new_cells(new_size, + grids_unknown_cell_values[grid_index]); + for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) { + for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) { + new_cells[offset + j + i * stride] = + (*grids[grid_index])[j + i * limits_.cell_limits().num_x_cells]; + } } + *grids[grid_index] = new_cells; } - correspondence_cost_cells_ = new_cells; limits_ = new_limits; if (!known_cells_box_.isEmpty()) { known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset)); @@ -147,10 +157,8 @@ void Grid2D::GrowLimits(const Eigen::Vector2f& point) { proto::Grid2D Grid2D::ToProto() const { proto::Grid2D result; *result.mutable_limits() = mapping::ToProto(limits_); - result.mutable_cells()->Reserve(correspondence_cost_cells_.size()); - for (const auto& cell : correspondence_cost_cells_) { - result.mutable_cells()->Add(cell); - } + *result.mutable_cells() = {correspondence_cost_cells_.begin(), + correspondence_cost_cells_.end()}; CHECK(update_indices().empty()) << "Serializing a grid during an update is " "not supported. Finish the update first."; if (!known_cells_box().isEmpty()) { diff --git a/cartographer/mapping/2d/grid_2d.h b/cartographer/mapping/2d/grid_2d.h index 72bbc63..c7506e4 100644 --- a/cartographer/mapping/2d/grid_2d.h +++ b/cartographer/mapping/2d/grid_2d.h @@ -33,8 +33,8 @@ proto::GridOptions2D CreateGridOptions2D( class Grid2D : public GridInterface { public: - explicit Grid2D(const MapLimits& limits, float min_correspondence_cost, - float max_correspondence_cost); + Grid2D(const MapLimits& limits, float min_correspondence_cost, + float max_correspondence_cost); explicit Grid2D(const proto::Grid2D& proto); // Returns the limits of this Grid2D. @@ -73,6 +73,10 @@ class Grid2D : public GridInterface { transform::Rigid3d local_pose) const = 0; protected: + void GrowLimits(const Eigen::Vector2f& point, + const std::vector*>& grids, + const std::vector& grids_unknown_cell_values); + const std::vector& correspondence_cost_cells() const { return correspondence_cost_cells_; } diff --git a/cartographer/mapping/2d/tsdf_2d.cc b/cartographer/mapping/2d/tsdf_2d.cc new file mode 100644 index 0000000..b82d6f6 --- /dev/null +++ b/cartographer/mapping/2d/tsdf_2d.cc @@ -0,0 +1,173 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/2d/tsdf_2d.h" + +#include + +#include "cartographer/common/make_unique.h" + +namespace cartographer { +namespace mapping { + +TSDF2D::TSDF2D(const MapLimits& limits, float truncation_distance, + float max_weight) + : Grid2D(limits, -truncation_distance, truncation_distance), + value_converter_(common::make_unique( + truncation_distance, max_weight)), + weight_cells_( + limits.cell_limits().num_x_cells * limits.cell_limits().num_y_cells, + value_converter_->getUnknownWeightValue()) {} + +TSDF2D::TSDF2D(const proto::Grid2D& proto) : Grid2D(proto) { + CHECK(proto.has_tsdf_2d()); + value_converter_ = common::make_unique( + proto.tsdf_2d().truncation_distance(), proto.tsdf_2d().max_weight()); + weight_cells_.reserve(proto.tsdf_2d().weight_cells_size()); + for (const auto& cell : proto.tsdf_2d().weight_cells()) { + CHECK_LE(cell, std::numeric_limits::max()); + weight_cells_.push_back(cell); + } +} + +void TSDF2D::SetCell(const Eigen::Array2i& cell_index, float tsd, + float weight) { + const int flat_index = ToFlatIndex(cell_index); + uint16* tsdf_cell = &(*mutable_correspondence_cost_cells())[flat_index]; + uint16* weight_cell = &weight_cells_[flat_index]; + if (*tsdf_cell >= value_converter_->getUpdateMarker()) { + return; + } + mutable_update_indices()->push_back(flat_index); + mutable_known_cells_box()->extend(cell_index.matrix()); + *tsdf_cell = + value_converter_->TSDToValue(tsd) + value_converter_->getUpdateMarker(); + *weight_cell = value_converter_->WeightToValue(weight); + return; +} + +float TSDF2D::GetTSD(const Eigen::Array2i& cell_index) const { + if (limits().Contains(cell_index)) { + return value_converter_->ValueToTSD( + correspondence_cost_cells()[ToFlatIndex(cell_index)]); + } + return value_converter_->getMinTSD(); +} + +float TSDF2D::GetWeight(const Eigen::Array2i& cell_index) const { + if (limits().Contains(cell_index)) { + return value_converter_->ValueToWeight( + weight_cells_[ToFlatIndex(cell_index)]); + } + return value_converter_->getMinWeight(); +} + +std::pair TSDF2D::GetTSDAndWeight( + const Eigen::Array2i& cell_index) const { + if (limits().Contains(cell_index)) { + int flat_index = ToFlatIndex(cell_index); + return std::make_pair( + value_converter_->ValueToTSD(correspondence_cost_cells()[flat_index]), + value_converter_->ValueToWeight(weight_cells_[flat_index])); + } + return std::make_pair(value_converter_->getMinTSD(), + value_converter_->getMinWeight()); +} + +void TSDF2D::GrowLimits(const Eigen::Vector2f& point) { + Grid2D::GrowLimits(point, + {mutable_correspondence_cost_cells(), &weight_cells_}, + {value_converter_->getUnknownTSDValue(), + value_converter_->getUnknownWeightValue()}); +} + +proto::Grid2D TSDF2D::ToProto() const { + proto::Grid2D result; + result = Grid2D::ToProto(); + *result.mutable_tsdf_2d()->mutable_weight_cells() = {weight_cells_.begin(), + weight_cells_.end()}; + result.mutable_tsdf_2d()->set_truncation_distance( + value_converter_->getMaxTSD()); + result.mutable_tsdf_2d()->set_max_weight(value_converter_->getMaxWeight()); + return result; +} + +std::unique_ptr TSDF2D::ComputeCroppedGrid() const { + Eigen::Array2i offset; + CellLimits cell_limits; + ComputeCroppedLimits(&offset, &cell_limits); + const double resolution = limits().resolution(); + const Eigen::Vector2d max = + limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x()); + std::unique_ptr cropped_grid = common::make_unique( + MapLimits(resolution, max, cell_limits), value_converter_->getMaxTSD(), + value_converter_->getMaxWeight()); + for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) { + if (!IsKnown(xy_index + offset)) continue; + cropped_grid->SetCell(xy_index, GetTSD(xy_index + offset), + GetWeight(xy_index + offset)); + } + return std::move(cropped_grid); +} + +bool TSDF2D::DrawToSubmapTexture( + proto::SubmapQuery::Response::SubmapTexture* const texture, + transform::Rigid3d local_pose) const { + Eigen::Array2i offset; + CellLimits cell_limits; + ComputeCroppedLimits(&offset, &cell_limits); + + std::string cells; + for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) { + if (!IsKnown(xy_index + offset)) { + cells.push_back(0); // value + cells.push_back(0); // alpha + } + // We would like to add 'delta' but this is not possible using a value and + // alpha. We use premultiplied alpha, so when 'delta' is positive we can + // add it by setting 'alpha' to zero. If it is negative, we set 'value' to + // zero, and use 'alpha' to subtract. This is only correct when the pixel + // is currently white, so walls will look too gray. This should be hard to + // detect visually for the user, though. + float normalized_tsdf = std::abs(GetTSD(xy_index + offset)); + normalized_tsdf = + std::pow(normalized_tsdf / value_converter_->getMaxTSD(), 0.5f); + float normalized_weight = + GetWeight(xy_index + offset) / value_converter_->getMaxWeight(); + const int delta = static_cast( + std::round(normalized_weight * (normalized_tsdf * 255. - 128.))); + const uint8 alpha = delta > 0 ? 0 : -delta; + const uint8 value = delta > 0 ? delta : 0; + cells.push_back(value); + cells.push_back((value || alpha) ? alpha : 1); + } + + common::FastGzipString(cells, texture->mutable_cells()); + texture->set_width(cell_limits.num_x_cells); + texture->set_height(cell_limits.num_y_cells); + const double resolution = limits().resolution(); + texture->set_resolution(resolution); + const double max_x = limits().max().x() - resolution * offset.y(); + const double max_y = limits().max().y() - resolution * offset.x(); + *texture->mutable_slice_pose() = transform::ToProto( + local_pose.inverse() * + transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.))); + + return true; +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/2d/tsdf_2d.h b/cartographer/mapping/2d/tsdf_2d.h new file mode 100644 index 0000000..35c88c1 --- /dev/null +++ b/cartographer/mapping/2d/tsdf_2d.h @@ -0,0 +1,58 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_2D_TSDF_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_TSDF_2D_H_ + +#include + +#include "cartographer/common/port.h" +#include "cartographer/mapping/2d/grid_2d.h" +#include "cartographer/mapping/2d/map_limits.h" +#include "cartographer/mapping/2d/xy_index.h" +#include "cartographer/mapping/internal/tsd_value_converter.h" +namespace cartographer { +namespace mapping { + +// Represents a 2D grid of truncated signed distances and weights. +class TSDF2D : public Grid2D { + public: + TSDF2D(const MapLimits& limits, float truncation_distance, float max_weight); + explicit TSDF2D(const proto::Grid2D& proto); + + void SetCell(const Eigen::Array2i& cell_index, const float tsd, + const float weight); + float GetTSD(const Eigen::Array2i& cell_index) const; + float GetWeight(const Eigen::Array2i& cell_index) const; + std::pair GetTSDAndWeight( + const Eigen::Array2i& cell_index) const; + + virtual void GrowLimits(const Eigen::Vector2f& point) override; + proto::Grid2D ToProto() const; + virtual std::unique_ptr ComputeCroppedGrid() const override; + virtual bool DrawToSubmapTexture( + proto::SubmapQuery::Response::SubmapTexture* const texture, + transform::Rigid3d local_pose) const override; + + private: + std::unique_ptr value_converter_; + std::vector weight_cells_; // Highest bit is update marker. +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_TSDF_2D_H_ diff --git a/cartographer/mapping/2d/tsdf_2d_test.cc b/cartographer/mapping/2d/tsdf_2d_test.cc new file mode 100644 index 0000000..f1b3420 --- /dev/null +++ b/cartographer/mapping/2d/tsdf_2d_test.cc @@ -0,0 +1,155 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/2d/tsdf_2d.h" + +#include + +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +using Eigen::Array2i; +using Eigen::Vector2f; + +TEST(TSDF2DTest, ProtoConstructor) { + proto::Grid2D proto; + const MapLimits limits(1., {2., 3.}, CellLimits(4., 5.)); + *proto.mutable_limits() = ToProto(limits); + proto.mutable_tsdf_2d()->set_max_weight(10.0); + proto.mutable_tsdf_2d()->set_truncation_distance(1.0); + for (int i = 6; i < 12; ++i) { + proto.mutable_cells()->Add(static_cast(i)); + proto.mutable_tsdf_2d()->mutable_weight_cells()->Add( + static_cast(i)); + } + proto.mutable_known_cells_box()->set_max_x(19); + proto.mutable_known_cells_box()->set_max_y(20); + proto.mutable_known_cells_box()->set_min_x(21); + proto.mutable_known_cells_box()->set_min_y(22); + proto.set_max_correspondence_cost(1.0); + proto.set_min_correspondence_cost(-1.0); + + TSDF2D grid(proto); + EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString()); +} + +TEST(TSDF2DTest, ToProto) { + TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), 1.0f, + 10.0f); + + const auto proto = tsdf.ToProto(); + EXPECT_EQ(ToProto(tsdf.limits()).DebugString(), proto.limits().DebugString()); +} + +TEST(TSDF2DTest, GetCellIndex) { + TSDF2D tsdf(MapLimits(2., Eigen::Vector2d(8., 14.), CellLimits(14, 8)), 1.f, + 10.f); + + const MapLimits& limits = tsdf.limits(); + const CellLimits& cell_limits = limits.cell_limits(); + ASSERT_EQ(14, cell_limits.num_x_cells); + ASSERT_EQ(8, cell_limits.num_y_cells); + EXPECT_TRUE( + (Array2i(0, 0) == limits.GetCellIndex(Vector2f(7.f, 13.f))).all()); + EXPECT_TRUE( + (Array2i(13, 0) == limits.GetCellIndex(Vector2f(7.f, -13.f))).all()); + EXPECT_TRUE( + (Array2i(0, 7) == limits.GetCellIndex(Vector2f(-7.f, 13.f))).all()); + EXPECT_TRUE( + (Array2i(13, 7) == limits.GetCellIndex(Vector2f(-7.f, -13.f))).all()); + + // Check around the origin. + EXPECT_TRUE( + (Array2i(6, 3) == limits.GetCellIndex(Vector2f(0.5f, 0.5f))).all()); + EXPECT_TRUE( + (Array2i(6, 3) == limits.GetCellIndex(Vector2f(1.5f, 1.5f))).all()); + EXPECT_TRUE( + (Array2i(7, 3) == limits.GetCellIndex(Vector2f(0.5f, -0.5f))).all()); + EXPECT_TRUE( + (Array2i(6, 4) == limits.GetCellIndex(Vector2f(-0.5f, 0.5f))).all()); + EXPECT_TRUE( + (Array2i(7, 4) == limits.GetCellIndex(Vector2f(-0.5f, -0.5f))).all()); +} + +TEST(TSDF2DTest, WriteRead) { + const float truncation_distance = 1.f; + const float max_weight = 10.f; + TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 2.), CellLimits(2, 2)), + truncation_distance, max_weight); + + const MapLimits& limits = tsdf.limits(); + EXPECT_EQ(1., limits.max().x()); + EXPECT_EQ(2., limits.max().y()); + + const CellLimits& cell_limits = limits.cell_limits(); + ASSERT_EQ(2, cell_limits.num_x_cells); + ASSERT_EQ(2, cell_limits.num_y_cells); + + std::mt19937 rng(42); + std::uniform_real_distribution tsd_distribution(-truncation_distance, + truncation_distance); + std::uniform_real_distribution weight_distribution(0.f, max_weight); + for (size_t i = 0; i < 1; ++i) { + const float tsd = tsd_distribution(rng); + const float weight = weight_distribution(rng); + tsdf.SetCell(limits.GetCellIndex(Vector2f(-0.5f, 0.5f)), tsd, weight); + EXPECT_NEAR( + tsdf.GetTSDAndWeight(limits.GetCellIndex(Vector2f(-0.5f, 0.5f))).first, + tsd, 2.f * truncation_distance / 32768.f); + EXPECT_NEAR( + tsdf.GetTSDAndWeight(limits.GetCellIndex(Vector2f(-0.5f, 0.5f))).second, + weight, max_weight / 32768.f); + EXPECT_NEAR(tsdf.GetTSD(limits.GetCellIndex(Vector2f(-0.5f, 0.5f))), tsd, + 2.f * truncation_distance / 32768.f); + EXPECT_NEAR(tsdf.GetWeight(limits.GetCellIndex(Vector2f(-0.5f, 0.5f))), + weight, max_weight / 32768.f); + } + for (const Array2i& xy_index : {limits.GetCellIndex(Vector2f(-0.5f, 1.5f)), + limits.GetCellIndex(Vector2f(0.5f, 0.5f)), + limits.GetCellIndex(Vector2f(0.5f, 1.5f))}) { + EXPECT_TRUE(limits.Contains(xy_index)); + EXPECT_FALSE(tsdf.IsKnown(xy_index)); + } +} + +TEST(TSDF2DTest, CorrectCropping) { + // Create a TSDF with random values. + const float truncation_distance = 1.f; + const float max_weight = 10.f; + std::mt19937 rng(42); + std::uniform_real_distribution tsdf_distribution(-truncation_distance, + truncation_distance); + std::uniform_real_distribution weight_distribution(0.f, max_weight); + TSDF2D tsdf(MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400)), + truncation_distance, max_weight); + for (const Array2i& xy_index : + XYIndexRangeIterator(Array2i(100, 100), Array2i(299, 299))) { + tsdf.SetCell(xy_index, tsdf_distribution(rng), weight_distribution(rng)); + } + Array2i offset; + CellLimits limits; + tsdf.ComputeCroppedLimits(&offset, &limits); + EXPECT_TRUE((offset == Array2i(100, 100)).all()); + EXPECT_EQ(limits.num_x_cells, 200); + EXPECT_EQ(limits.num_y_cells, 200); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/proto/2d/grid_2d.proto b/cartographer/mapping/proto/2d/grid_2d.proto index 152c719..70b246d 100644 --- a/cartographer/mapping/proto/2d/grid_2d.proto +++ b/cartographer/mapping/proto/2d/grid_2d.proto @@ -18,6 +18,7 @@ package cartographer.mapping.proto; import "cartographer/mapping/proto/2d/map_limits.proto"; import "cartographer/mapping/proto/2d/probability_grid.proto"; +import "cartographer/mapping/proto/2d/tsdf_2d.proto"; message Grid2D { message CellBox { @@ -34,6 +35,7 @@ message Grid2D { CellBox known_cells_box = 3; oneof grid { ProbabilityGrid probability_grid_2d = 4; + TSDF2D tsdf_2d = 5; } float min_correspondence_cost = 6; float max_correspondence_cost = 7; diff --git a/cartographer/mapping/proto/2d/tsdf_2d.proto b/cartographer/mapping/proto/2d/tsdf_2d.proto new file mode 100644 index 0000000..bfbb44d --- /dev/null +++ b/cartographer/mapping/proto/2d/tsdf_2d.proto @@ -0,0 +1,23 @@ +// Copyright 2018 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +message TSDF2D { + float truncation_distance = 1; + float max_weight = 2; + repeated int32 weight_cells = 3; +}