From 7dfe4042782bb088ac525d18a4991e15292a2b92 Mon Sep 17 00:00:00 2001 From: Kevin Daun Date: Mon, 9 Jul 2018 13:46:46 +0200 Subject: [PATCH] Add TSDF RangeDataInserter (#1236) Adds TSDF RangeDataInserter, tests and integrates the configuration files. --- cartographer/mapping/2d/map_limits.h | 6 + ...probability_grid_range_data_inserter_2d.cc | 1 + cartographer/mapping/2d/submap_2d_test.cc | 13 + cartographer/mapping/2d/tsdf_2d.cc | 13 +- cartographer/mapping/2d/tsdf_2d.h | 1 + .../mapping/2d/tsdf_range_data_inserter_2d.cc | 234 ++++++++++++ .../mapping/2d/tsdf_range_data_inserter_2d.h | 60 +++ .../2d/tsdf_range_data_inserter_2d_test.cc | 348 ++++++++++++++++++ .../mapping/internal/2d/pose_graph_2d_test.cc | 15 +- .../tsdf_range_data_inserter_options_2d.proto | 47 +++ .../proto/range_data_inserter_options.proto | 3 + .../mapping/range_data_inserter_interface.cc | 5 + configuration_files/trajectory_builder_2d.lua | 13 + 13 files changed, 752 insertions(+), 7 deletions(-) create mode 100644 cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc create mode 100644 cartographer/mapping/2d/tsdf_range_data_inserter_2d.h create mode 100644 cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc create mode 100644 cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.proto diff --git a/cartographer/mapping/2d/map_limits.h b/cartographer/mapping/2d/map_limits.h index 3d731b9..241fc57 100644 --- a/cartographer/mapping/2d/map_limits.h +++ b/cartographer/mapping/2d/map_limits.h @@ -75,6 +75,12 @@ class MapLimits { common::RoundToInt((max_.x() - point.x()) / resolution_ - 0.5)); } + // Returns the center of the cell at 'cell_index'. + Eigen::Vector2f GetCellCenter(const Eigen::Array2i cell_index) const { + return {max_.x() - resolution() * (cell_index[1] + 0.5), + max_.y() - resolution() * (cell_index[0] + 0.5)}; + } + // Returns true if the ProbabilityGrid contains 'cell_index'. bool Contains(const Eigen::Array2i& cell_index) const { return (Eigen::Array2i(0, 0) <= cell_index).all() && diff --git a/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc b/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc index a4934f1..3581f5c 100644 --- a/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc +++ b/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc @@ -35,6 +35,7 @@ constexpr int kSubpixelScale = 1000; void GrowAsNeeded(const sensor::RangeData& range_data, ProbabilityGrid* const probability_grid) { Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>()); + // Padding around bounding box to avoid numerical issues at cell boundaries. constexpr float kPadding = 1e-6f; for (const Eigen::Vector3f& hit : range_data.returns) { bounding_box.extend(hit.head<2>()); diff --git a/cartographer/mapping/2d/submap_2d_test.cc b/cartographer/mapping/2d/submap_2d_test.cc index 62266e9..d410560 100644 --- a/cartographer/mapping/2d/submap_2d_test.cc +++ b/cartographer/mapping/2d/submap_2d_test.cc @@ -50,6 +50,19 @@ TEST(Submap2DTest, TheRightNumberOfRangeDataAreInserted) { "hit_probability = 0.53, " "miss_probability = 0.495, " "}," + "tsdf_range_data_inserter = { " + "truncation_distance = 2.0," + "maximum_weight = 10.," + "update_free_space = false," + "normal_estimation_options = {" + "num_normal_samples = 2," + "sample_radius = 10.," + "}," + "project_sdf_distance_to_scan_normal = false," + "update_weight_range_exponent = 0," + "update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0," + "update_weight_distance_cell_to_hit_kernel_bandwith = 0," + "}," "}," "}"); ActiveSubmaps2D submaps{CreateSubmapsOptions2D(parameter_dictionary.get())}; diff --git a/cartographer/mapping/2d/tsdf_2d.cc b/cartographer/mapping/2d/tsdf_2d.cc index b82d6f6..f74ab6a 100644 --- a/cartographer/mapping/2d/tsdf_2d.cc +++ b/cartographer/mapping/2d/tsdf_2d.cc @@ -16,10 +16,6 @@ #include "cartographer/mapping/2d/tsdf_2d.h" -#include - -#include "cartographer/common/make_unique.h" - namespace cartographer { namespace mapping { @@ -43,11 +39,16 @@ TSDF2D::TSDF2D(const proto::Grid2D& proto) : Grid2D(proto) { } } +bool TSDF2D::CellIsUpdated(const Eigen::Array2i& cell_index) const { + const int flat_index = ToFlatIndex(cell_index); + uint16 tsdf_cell = correspondence_cost_cells()[flat_index]; + return tsdf_cell >= value_converter_->getUpdateMarker(); +} + 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; } @@ -55,8 +56,8 @@ void TSDF2D::SetCell(const Eigen::Array2i& cell_index, float tsd, mutable_known_cells_box()->extend(cell_index.matrix()); *tsdf_cell = value_converter_->TSDToValue(tsd) + value_converter_->getUpdateMarker(); + uint16* weight_cell = &weight_cells_[flat_index]; *weight_cell = value_converter_->WeightToValue(weight); - return; } float TSDF2D::GetTSD(const Eigen::Array2i& cell_index) const { diff --git a/cartographer/mapping/2d/tsdf_2d.h b/cartographer/mapping/2d/tsdf_2d.h index 17a6fd2..a4985a0 100644 --- a/cartographer/mapping/2d/tsdf_2d.h +++ b/cartographer/mapping/2d/tsdf_2d.h @@ -46,6 +46,7 @@ class TSDF2D : public Grid2D { virtual bool DrawToSubmapTexture( proto::SubmapQuery::Response::SubmapTexture* const texture, transform::Rigid3d local_pose) const override; + bool CellIsUpdated(const Eigen::Array2i& cell_index) const; private: std::unique_ptr value_converter_; diff --git a/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc b/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc new file mode 100644 index 0000000..d6cd894 --- /dev/null +++ b/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc @@ -0,0 +1,234 @@ +/* + * 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_range_data_inserter_2d.h" + +#include "cartographer/mapping/internal/2d/normal_estimation_2d.h" +#include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h" + +namespace cartographer { +namespace mapping { +namespace { + +// Factor for subpixel accuracy of start and end point for ray casts. +constexpr int kSubpixelScale = 1000; +// Minimum distance between range observation and origin. Otherwise, range +// observations are discarded. +constexpr float kMinRangeMeters = 1e-6f; +constexpr float kSqrtTwoPi = std::sqrt(2.0 * M_PI); + +void GrowAsNeeded(const sensor::RangeData& range_data, + const float truncation_distance, TSDF2D* const tsdf) { + Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>()); + for (const Eigen::Vector3f& hit : range_data.returns) { + const Eigen::Vector3f direction = (hit - range_data.origin).normalized(); + const Eigen::Vector3f end_position = hit + truncation_distance * direction; + bounding_box.extend(end_position.head<2>()); + } + // Padding around bounding box to avoid numerical issues at cell boundaries. + constexpr float kPadding = 1e-6f; + tsdf->GrowLimits(bounding_box.min() - kPadding * Eigen::Vector2f::Ones()); + tsdf->GrowLimits(bounding_box.max() + kPadding * Eigen::Vector2f::Ones()); +} + +float GaussianKernel(const float x, const float sigma) { + return 1.0 / (kSqrtTwoPi * sigma) * std::exp(-0.5 * x * x / (sigma * sigma)); +} + +std::pair SuperscaleRay( + const Eigen::Vector2f& begin, const Eigen::Vector2f& end, + TSDF2D* const tsdf) { + const MapLimits& limits = tsdf->limits(); + const double superscaled_resolution = limits.resolution() / kSubpixelScale; + const MapLimits superscaled_limits( + superscaled_resolution, limits.max(), + CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale, + limits.cell_limits().num_y_cells * kSubpixelScale)); + + const Eigen::Array2i superscaled_begin = + superscaled_limits.GetCellIndex(begin); + const Eigen::Array2i superscaled_end = superscaled_limits.GetCellIndex(end); + return std::make_pair(superscaled_begin, superscaled_end); +} + +struct RangeDataSorter { + RangeDataSorter(Eigen::Vector3f origin) { origin_ = origin.head<2>(); } + bool operator()(Eigen::Vector3f lhs, Eigen::Vector3f rhs) { + const Eigen::Vector2f delta_lhs = (lhs.head<2>() - origin_).normalized(); + const Eigen::Vector2f delta_rhs = (lhs.head<2>() - origin_).normalized(); + if ((delta_lhs[1] < 0.f) != (delta_rhs[1] < 0.f)) { + return delta_lhs[1] < 0.f; + } else if (delta_lhs[1] < 0.f) { + return delta_lhs[0] < delta_rhs[0]; + } else { + return delta_lhs[0] > delta_rhs[0]; + } + } + + private: + Eigen::Vector2f origin_; +}; + +float ComputeRangeWeightFactor(float range, int exponent) { + float weight = 0.f; + if (std::abs(range) > kMinRangeMeters) { + weight = 1.f / (std::pow(range, exponent)); + } + return weight; +} +} // namespace + +proto::TSDFRangeDataInserterOptions2D CreateTSDFRangeDataInserterOptions2D( + common::LuaParameterDictionary* parameter_dictionary) { + proto::TSDFRangeDataInserterOptions2D options; + options.set_truncation_distance( + parameter_dictionary->GetDouble("truncation_distance")); + options.set_maximum_weight(parameter_dictionary->GetDouble("maximum_weight")); + options.set_update_free_space( + parameter_dictionary->GetBool("update_free_space")); + *options + .mutable_normal_estimation_options() = CreateNormalEstimationOptions2D( + parameter_dictionary->GetDictionary("normal_estimation_options").get()); + options.set_project_sdf_distance_to_scan_normal( + parameter_dictionary->GetBool("project_sdf_distance_to_scan_normal")); + options.set_update_weight_range_exponent( + parameter_dictionary->GetInt("update_weight_range_exponent")); + options.set_update_weight_angle_scan_normal_to_ray_kernel_bandwith( + parameter_dictionary->GetDouble( + "update_weight_angle_scan_normal_to_ray_kernel_bandwith")); + options.set_update_weight_distance_cell_to_hit_kernel_bandwith( + parameter_dictionary->GetDouble( + "update_weight_distance_cell_to_hit_kernel_bandwith")); + return options; +} + +TSDFRangeDataInserter2D::TSDFRangeDataInserter2D( + const proto::TSDFRangeDataInserterOptions2D& options) + : options_(options) {} + +// Casts a ray from origin towards hit for each hit in range data. +// If 'options.update_free_space' is 'true', all cells along the ray +// until 'truncation_distance' behind hit are updated. Otherwise, only the cells +// within 'truncation_distance' around hit are updated. +void TSDFRangeDataInserter2D::Insert(const sensor::RangeData& range_data, + GridInterface* grid) const { + const float truncation_distance = + static_cast(options_.truncation_distance()); + TSDF2D* tsdf = static_cast(grid); + GrowAsNeeded(range_data, truncation_distance, tsdf); + + // Compute normals if needed. + bool scale_update_weight_angle_scan_normal_to_ray = + options_.update_weight_angle_scan_normal_to_ray_kernel_bandwith() != 0.f; + sensor::RangeData sorted_range_data = range_data; + std::vector normals; + if (options_.project_sdf_distance_to_scan_normal() || + scale_update_weight_angle_scan_normal_to_ray) { + std::sort(sorted_range_data.returns.begin(), + sorted_range_data.returns.end(), + RangeDataSorter(sorted_range_data.origin)); + normals = EstimateNormals(sorted_range_data, + options_.normal_estimation_options()); + } + + const Eigen::Vector2f origin = sorted_range_data.origin.head<2>(); + for (size_t hit_index = 0; hit_index < sorted_range_data.returns.size(); + ++hit_index) { + const Eigen::Vector2f hit = sorted_range_data.returns[hit_index].head<2>(); + const float normal = normals.empty() + ? std::numeric_limits::quiet_NaN() + : normals[hit_index]; + InsertHit(options_, hit, origin, normal, tsdf); + } + tsdf->FinishUpdate(); +} + +void TSDFRangeDataInserter2D::InsertHit( + const proto::TSDFRangeDataInserterOptions2D& options, + const Eigen::Vector2f& hit, const Eigen::Vector2f& origin, float normal, + TSDF2D* tsdf) const { + const Eigen::Vector2f ray = hit - origin; + const float range = ray.norm(); + const float truncation_distance = + static_cast(options_.truncation_distance()); + if (range < truncation_distance) return; + const float truncation_ratio = truncation_distance / range; + const Eigen::Vector2f ray_begin = + options_.update_free_space() ? origin + : origin + (1.0f - truncation_ratio) * ray; + const Eigen::Vector2f ray_end = origin + (1.0f + truncation_ratio) * ray; + std::pair superscaled_ray = + SuperscaleRay(ray_begin, ray_end, tsdf); + std::vector ray_mask = RayToPixelMask( + superscaled_ray.first, superscaled_ray.second, kSubpixelScale); + + // Precompute weight factors. + float weight_factor_angle_ray_normal = 1.f; + if (options_.update_weight_angle_scan_normal_to_ray_kernel_bandwith() != + 0.f) { + const Eigen::Vector2f negative_ray = -ray; + float angle_ray_normal = + common::NormalizeAngleDifference(normal - common::atan2(negative_ray)); + weight_factor_angle_ray_normal = GaussianKernel( + angle_ray_normal, + options_.update_weight_angle_scan_normal_to_ray_kernel_bandwith()); + } + float weight_factor_range = 1.f; + if (options_.update_weight_range_exponent() != 0) { + weight_factor_range = ComputeRangeWeightFactor( + range, options_.update_weight_range_exponent()); + } + + // Update Cells. + for (const Eigen::Array2i& cell_index : ray_mask) { + if (tsdf->CellIsUpdated(cell_index)) continue; + Eigen::Vector2f cell_center = tsdf->limits().GetCellCenter(cell_index); + float distance_cell_to_origin = (cell_center - origin).norm(); + float update_tsd = range - distance_cell_to_origin; + if (options_.project_sdf_distance_to_scan_normal()) { + float normal_orientation = normal; + update_tsd = (cell_center - hit) + .dot(Eigen::Vector2f{std::cos(normal_orientation), + std::sin(normal_orientation)}); + } + update_tsd = + common::Clamp(update_tsd, -truncation_distance, truncation_distance); + float update_weight = weight_factor_range * weight_factor_angle_ray_normal; + if (options_.update_weight_distance_cell_to_hit_kernel_bandwith() != 0.f) { + update_weight *= GaussianKernel( + update_tsd, + options_.update_weight_distance_cell_to_hit_kernel_bandwith()); + } + UpdateCell(cell_index, update_tsd, update_weight, tsdf); + } +} + +void TSDFRangeDataInserter2D::UpdateCell(const Eigen::Array2i& cell, + float update_sdf, float update_weight, + TSDF2D* tsdf) const { + if (update_weight == 0.f) return; + const std::pair tsd_and_weight = tsdf->GetTSDAndWeight(cell); + float updated_weight = tsd_and_weight.second + update_weight; + float updated_sdf = (tsd_and_weight.first * tsd_and_weight.second + + update_sdf * update_weight) / + updated_weight; + updated_weight = + std::min(updated_weight, static_cast(options_.maximum_weight())); + tsdf->SetCell(cell, updated_sdf, updated_weight); +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/2d/tsdf_range_data_inserter_2d.h b/cartographer/mapping/2d/tsdf_range_data_inserter_2d.h new file mode 100644 index 0000000..61a77a2 --- /dev/null +++ b/cartographer/mapping/2d/tsdf_range_data_inserter_2d.h @@ -0,0 +1,60 @@ +/* + * 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_RANGE_DATA_INSERTER_2D_H_ +#define CARTOGRAPHER_MAPPING_2D_TSDF_RANGE_DATA_INSERTER_2D_H_ + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping/2d/tsdf_2d.h" +#include "cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.pb.h" +#include "cartographer/mapping/range_data_inserter_interface.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/range_data.h" + +namespace cartographer { +namespace mapping { + +proto::TSDFRangeDataInserterOptions2D CreateTSDFRangeDataInserterOptions2D( + common::LuaParameterDictionary* parameter_dictionary); + +class TSDFRangeDataInserter2D : public RangeDataInserterInterface { + public: + explicit TSDFRangeDataInserter2D( + const proto::TSDFRangeDataInserterOptions2D& options); + + TSDFRangeDataInserter2D(const TSDFRangeDataInserter2D&) = delete; + TSDFRangeDataInserter2D& operator=(const TSDFRangeDataInserter2D&) = delete; + + // Casts a ray from origin towards hit for each hit in range data. + // If 'options.update_free_space' is 'true', all cells along the ray + // until 'truncation_distance' behind hit are updated. Otherwise, only the + // cells within 'truncation_distance' around hit are updated. + virtual void Insert(const sensor::RangeData& range_data, + GridInterface* grid) const override; + + private: + void InsertHit(const proto::TSDFRangeDataInserterOptions2D& options, + const Eigen::Vector2f& hit, const Eigen::Vector2f& origin, + float normal, TSDF2D* tsdf) const; + void UpdateCell(const Eigen::Array2i& cell, float update_sdf, + float update_weight, TSDF2D* tsdf) const; + const proto::TSDFRangeDataInserterOptions2D options_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_TSDF_RANGE_DATA_INSERTER_2D_H_ \ No newline at end of file diff --git a/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc b/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc new file mode 100644 index 0000000..0a9f120 --- /dev/null +++ b/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc @@ -0,0 +1,348 @@ +/* + * 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_range_data_inserter_2d.h" + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "gmock/gmock.h" + +namespace cartographer { +namespace mapping { +namespace { + +class RangeDataInserterTest2DTSDF : public ::testing::Test { + protected: + RangeDataInserterTest2DTSDF() + : tsdf_(MapLimits(1., Eigen::Vector2d(0., 7.), CellLimits(8, 1)), 2.0, + 10.0) { + auto parameter_dictionary = common::MakeDictionary( + "return { " + "truncation_distance = 2.0," + "maximum_weight = 10.," + "update_free_space = false," + "normal_estimation_options = {" + "num_normal_samples = 2," + "sample_radius = 10.," + "}," + "project_sdf_distance_to_scan_normal = false," + "update_weight_range_exponent = 0," + "update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0," + "update_weight_distance_cell_to_hit_kernel_bandwith = 0," + "}"); + options_ = CreateTSDFRangeDataInserterOptions2D(parameter_dictionary.get()); + range_data_inserter_ = + common::make_unique(options_); + } + + void InsertPoint() { + sensor::RangeData range_data; + range_data.returns.emplace_back(-0.5f, 3.5f, 0.f); + range_data.origin.x() = -0.5f; + range_data.origin.y() = -0.5f; + range_data_inserter_->Insert(range_data, &tsdf_); + tsdf_.FinishUpdate(); + } + + proto::TSDFRangeDataInserterOptions2D options_; + TSDF2D tsdf_; + std::unique_ptr range_data_inserter_; +}; + +class MockCellProperties { + public: + MockCellProperties(const Eigen::Array2i& cell_index, const TSDF2D& tsdf) + : is_known_(tsdf.IsKnown(cell_index)), + tsd_(tsdf.GetTSD(cell_index)), + weight_(tsdf.GetWeight(cell_index)){}; + + bool is_known_; + float tsd_; + float weight_; +}; + +::std::ostream& operator<<(::std::ostream& os, const MockCellProperties& bar) { + return os << std::to_string(bar.is_known_) + "\t" + std::to_string(bar.tsd_) + + "\t" + std::to_string(bar.weight_); +} + +MATCHER_P3(EqualCellProperties, expected_is_known, expected_tsd, + expected_weight, + std::string("Expected ") + std::to_string(expected_is_known) + "\t" + + std::to_string(expected_tsd) + "\t" + + std::to_string(expected_weight)) { + bool result = expected_is_known == arg.is_known_; + result = result && (std::abs(expected_tsd - arg.tsd_ < 1e-4)); + result = result && (std::abs(expected_weight - arg.weight_ < 1e-2)); + return result; +} + +TEST_F(RangeDataInserterTest2DTSDF, InsertPoint) { + InsertPoint(); + const float truncation_distance = + static_cast(options_.truncation_distance()); + const float maximum_weight = static_cast(options_.maximum_weight()); + + for (float y = 1.5; y < 6.; ++y) { + // Cell intersects with ray. + float x = -0.5f; + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float expected_tsdf = + std::max(std::min(3.5f - y, truncation_distance), -truncation_distance); + float expected_weight = 1.f; + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, expected_weight)); + // Cells don't intersect with ray. + x = 0.5f; + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + expected_tsdf = -truncation_distance; + expected_weight = 0.; + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(false, expected_tsdf, expected_weight)); + x = 1.5f; + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(false, expected_tsdf, expected_weight)); + } + + // Cells don't intersect with ray. + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(0.5, 6.5)); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(false, -truncation_distance, 0.)); + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(-0.5, -1.5)); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(false, -truncation_distance, 0.)); + for (int i = 0; i < 1000; ++i) { + InsertPoint(); + } + for (float y = 1.5; y < 6.; ++y) { + // Cell intersects with ray. + float x = -0.5f; + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float expected_tsdf = + std::max(std::min(3.5f - y, truncation_distance), -truncation_distance); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, maximum_weight)); + } +} + +TEST_F(RangeDataInserterTest2DTSDF, InsertPointWithFreeSpaceUpdate) { + options_.set_update_free_space(true); + range_data_inserter_ = common::make_unique(options_); + InsertPoint(); + const float truncation_distance = + static_cast(options_.truncation_distance()); + const float maximum_weight = static_cast(options_.maximum_weight()); + + for (float y = -0.5; y < 6.; ++y) { + // Cells intersect with ray. + float x = -0.5f; + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float expected_tsdf = + std::max(std::min(3.5f - y, truncation_distance), -truncation_distance); + float expected_weight = 1.f; + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, expected_weight)); + // Cells don't intersect with ray. + x = 0.5f; + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + expected_tsdf = -truncation_distance; + expected_weight = 0.; + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(false, expected_tsdf, expected_weight)); + x = 1.5f; + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(false, expected_tsdf, expected_weight)); + } + + // Cells don't intersect with the ray. + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(-0.5, 6.5)); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(false, -truncation_distance, 0.)); + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(-0.5, -1.5)); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(false, -truncation_distance, 0.)); + for (int i = 0; i < 1000; ++i) { + InsertPoint(); + } + for (float y = -0.5; y < 6.; ++y) { + // Cell intersects with ray. + float x = -0.5f; + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float expected_tsdf = + std::max(std::min(3.5f - y, truncation_distance), -truncation_distance); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, maximum_weight)); + } +} + +TEST_F(RangeDataInserterTest2DTSDF, InsertPointLinearWeight) { + options_.set_update_weight_range_exponent(1); + range_data_inserter_ = common::make_unique(options_); + InsertPoint(); + const float truncation_distance = + static_cast(options_.truncation_distance()); + for (float y = 1.5; y < 6.; ++y) { + // Cell intersects with ray. + float x = -0.5f; + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float expected_tsdf = + std::max(std::min(3.5f - y, truncation_distance), -truncation_distance); + float expected_weight = 1.f / 4.f; + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, expected_weight)); + } +} + +TEST_F(RangeDataInserterTest2DTSDF, InsertPointQuadraticWeight) { + options_.set_update_weight_range_exponent(2); + range_data_inserter_ = common::make_unique(options_); + InsertPoint(); + const float truncation_distance = + static_cast(options_.truncation_distance()); + for (float y = 1.5; y < 6.; ++y) { + // Cell intersects with ray. + float x = -0.5f; + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float expected_tsdf = + std::max(std::min(3.5f - y, truncation_distance), -truncation_distance); + float expected_weight = 1.f / std::pow(4.f, 2); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, expected_weight)); + } +} + +TEST_F(RangeDataInserterTest2DTSDF, + InsertSmallAnglePointWithoutNormalProjection) { + sensor::RangeData range_data; + range_data.returns.emplace_back(-0.5f, 3.5f, 0.f); + range_data.returns.emplace_back(5.5f, 3.5f, 0.f); + range_data.returns.emplace_back(10.5f, 3.5f, 0.f); + range_data.origin.x() = -0.5f; + range_data.origin.y() = -0.5f; + range_data_inserter_->Insert(range_data, &tsdf_); + tsdf_.FinishUpdate(); + float x = 4.5f; + float y = 2.5f; + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + Eigen::Vector2f ray = + Eigen::Vector2f(-0.5f, -0.5f) - Eigen::Vector2f(5.5f, 3.5f); + float ray_length = ray.norm(); + Eigen::Vector2f origin_to_cell = + Eigen::Vector2f(x, y) - Eigen::Vector2f(-0.5f, -0.5f); + float distance_origin_to_cell = origin_to_cell.norm(); + float expected_tsdf = ray_length - distance_origin_to_cell; + float expected_weight = 1.f; + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, expected_weight)); +} + +TEST_F(RangeDataInserterTest2DTSDF, InsertSmallAnglePointWitNormalProjection) { + options_.set_project_sdf_distance_to_scan_normal(true); + range_data_inserter_ = common::make_unique(options_); + sensor::RangeData range_data; + range_data.returns.emplace_back(-0.5f, 3.5f, 0.f); + range_data.returns.emplace_back(5.5f, 3.5f, 0.f); + range_data.origin.x() = -0.5f; + range_data.origin.y() = -0.5f; + range_data_inserter_->Insert(range_data, &tsdf_); + tsdf_.FinishUpdate(); + float x = 4.5f; + float y = 2.5f; + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float expected_tsdf = 1.f; + float expected_weight = 1.f; + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, expected_weight)); + x = 6.5f; + y = 4.5f; + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + expected_tsdf = -1.f; + expected_weight = 1.f; + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, expected_weight)); +} + +TEST_F(RangeDataInserterTest2DTSDF, + InsertPointsWithAngleScanNormalToRayWeight) { + float bandwith = 10.f; + options_.set_update_weight_angle_scan_normal_to_ray_kernel_bandwith(bandwith); + range_data_inserter_ = common::make_unique(options_); + sensor::RangeData range_data; + range_data.returns.emplace_back(-0.5f, 3.5f, 0.f); + range_data.returns.emplace_back(5.5f, 3.5f, 0.f); + range_data.origin.x() = -0.5f; + range_data.origin.y() = -0.5f; + range_data_inserter_->Insert(range_data, &tsdf_); + tsdf_.FinishUpdate(); + float x = -0.5f; + float y = 3.5f; + // Ray is perpendicular to surface. + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float expected_weight = 1.f / (std::sqrt(2 * M_PI) * bandwith); + EXPECT_NEAR(expected_weight, tsdf_.GetWeight(cell_index), 1e-3); + x = 6.5f; + y = 4.5f; + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + EXPECT_NEAR(expected_weight, tsdf_.GetWeight(cell_index), 1e-3); + // Ray is inclined relative to surface. + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float angle = std::atan(7.f / 5.f); + expected_weight = 1.f / (std::sqrt(2 * M_PI) * bandwith) * + std::exp(angle * angle / (2 * std::pow(bandwith, 2))); + EXPECT_NEAR(expected_weight, tsdf_.GetWeight(cell_index), 1e-3); + x = 6.5f; + y = 4.5f; + cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + EXPECT_NEAR(expected_weight, tsdf_.GetWeight(cell_index), 1e-3); +} + +TEST_F(RangeDataInserterTest2DTSDF, InsertPointsWithDistanceCellToHit) { + float bandwith = 10.f; + options_.set_update_weight_distance_cell_to_hit_kernel_bandwith(bandwith); + range_data_inserter_ = common::make_unique(options_); + InsertPoint(); + const float truncation_distance = + static_cast(options_.truncation_distance()); + for (float y = 1.5; y < 6.; ++y) { + float x = -0.5f; + Eigen::Array2i cell_index = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y)); + float expected_tsdf = + std::max(std::min(3.5f - y, truncation_distance), -truncation_distance); + float expected_weight = + 1.f / (std::sqrt(2 * M_PI) * bandwith) * + std::exp(std::pow(expected_tsdf, 2) / (2 * std::pow(bandwith, 2))); + EXPECT_THAT(MockCellProperties(cell_index, tsdf_), + EqualCellProperties(true, expected_tsdf, expected_weight)); + } +} + +} // namespace +} // namespace mapping +} // namespace cartographer \ No newline at end of file diff --git a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc index ad009be..547742b 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc @@ -62,8 +62,21 @@ class PoseGraph2DTest : public ::testing::Test { hit_probability = 0.53, miss_probability = 0.495, }, + tsdf_range_data_inserter = { + truncation_distance = 0.3, + maximum_weight = 10., + update_free_space = false, + normal_estimation_options = { + num_normal_samples = 4, + sample_radius = 0.5, + }, + project_sdf_distance_to_scan_normal = false, + update_weight_range_exponent = 0, + update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0, + update_weight_distance_cell_to_hit_kernel_bandwith = 0, }, - })text"); + }, + })text"); active_submaps_ = common::make_unique( mapping::CreateSubmapsOptions2D(parameter_dictionary.get())); } diff --git a/cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.proto b/cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.proto new file mode 100644 index 0000000..a674ca3 --- /dev/null +++ b/cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.proto @@ -0,0 +1,47 @@ +// 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; + +import "cartographer/mapping/proto/2d/normal_estimation_options_2d.proto"; + +message TSDFRangeDataInserterOptions2D { + // Distance to the surface within the signed distance function is evaluated. + double truncation_distance = 1; + // Maximum weight that can be stored in a cell. + double maximum_weight = 2; + + // Enables updating cells between the sensor origin and the range observation + // as free space. + bool update_free_space = 3; + + NormalEstimationOptions2D normal_estimation_options = 4; + + // Project the distance between the updated cell und the range observation to + // the estimated scan normal. + bool project_sdf_distance_to_scan_normal = 5; + + // Update weight is scaled with 1/distance(origin,hit)^range_exponent. + int32 update_weight_range_exponent = 6; + + // Kernel bandwith of the weight factor based on the angle between. + // scan normal and ray + double update_weight_angle_scan_normal_to_ray_kernel_bandwith = 7; + + // Kernel bandwith of the weight factor based on the distance between + // cell and scan observation. + double update_weight_distance_cell_to_hit_kernel_bandwith = 8; +} diff --git a/cartographer/mapping/proto/range_data_inserter_options.proto b/cartographer/mapping/proto/range_data_inserter_options.proto index f0be593..7b0d5b1 100644 --- a/cartographer/mapping/proto/range_data_inserter_options.proto +++ b/cartographer/mapping/proto/range_data_inserter_options.proto @@ -15,6 +15,7 @@ syntax = "proto3"; import "cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.proto"; +import "cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.proto"; package cartographer.mapping.proto; @@ -22,9 +23,11 @@ message RangeDataInserterOptions { enum RangeDataInserterType { INVALID_INSERTER = 0; PROBABILITY_GRID_INSERTER_2D = 1; + TSDF_INSERTER_2D = 2; } RangeDataInserterType range_data_inserter_type = 1; ProbabilityGridRangeDataInserterOptions2D probability_grid_range_data_inserter_options_2d = 2; + TSDFRangeDataInserterOptions2D tsdf_range_data_inserter_options_2d = 3; } diff --git a/cartographer/mapping/range_data_inserter_interface.cc b/cartographer/mapping/range_data_inserter_interface.cc index 10c3c0b..cf01396 100644 --- a/cartographer/mapping/range_data_inserter_interface.cc +++ b/cartographer/mapping/range_data_inserter_interface.cc @@ -16,6 +16,7 @@ #include "cartographer/mapping/range_data_inserter_interface.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" +#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h" namespace cartographer { namespace mapping { @@ -37,6 +38,10 @@ proto::RangeDataInserterOptions CreateRangeDataInserterOptions( parameter_dictionary ->GetDictionary("probability_grid_range_data_inserter") .get()); + *options.mutable_tsdf_range_data_inserter_options_2d() = + CreateTSDFRangeDataInserterOptions2D( + parameter_dictionary->GetDictionary("tsdf_range_data_inserter") + .get()); return options; } } // namespace mapping diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index 4bb55c6..1009a9c 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/configuration_files/trajectory_builder_2d.lua @@ -74,6 +74,19 @@ TRAJECTORY_BUILDER_2D = { hit_probability = 0.55, miss_probability = 0.49, }, + tsdf_range_data_inserter = { + truncation_distance = 0.3, + maximum_weight = 10., + update_free_space = false, + normal_estimation_options = { + num_normal_samples = 4, + sample_radius = 0.5, + }, + project_sdf_distance_to_scan_normal = false, + update_weight_range_exponent = 0, + update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0, + update_weight_distance_cell_to_hit_kernel_bandwith = 0, + }, }, }, }