From a08037dc47d544d697261940727b934fefd8dfb9 Mon Sep 17 00:00:00 2001 From: Kevin Daun Date: Fri, 13 Jul 2018 13:47:09 +0200 Subject: [PATCH] Add TSDF interpolator. (#1274) Based on [interpolated_grid.h](https://github.com/googlecartographer/cartographer/blob/master/cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h). --- .../2d/scan_matching/interpolated_tsdf_2d.h | 136 ++++++++++++++++++ .../interpolated_tsdf_2d_test.cc | 127 ++++++++++++++++ 2 files changed, 263 insertions(+) create mode 100644 cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h create mode 100644 cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d_test.cc diff --git a/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h b/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h new file mode 100644 index 0000000..f2a5b86 --- /dev/null +++ b/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h @@ -0,0 +1,136 @@ +/* + * 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_INTERNAL_2D_SCAN_MATCHING_TSDF_INTERPOLATED_TSDF_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_INTERPOLATED_TSDF_2D_H_ + +#include + +#include "cartographer/mapping/2d/tsdf_2d.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// Interpolates between TSDF2D pixels with bilinear interpolation. +// +// This class is templated to work with Ceres autodiff. +class InterpolatedTSDF2D { + public: + explicit InterpolatedTSDF2D(const TSDF2D& grid) : tsdf_(grid) {} + + InterpolatedTSDF2D(const InterpolatedTSDF2D&) = delete; + InterpolatedTSDF2D& operator=(const InterpolatedTSDF2D&) = delete; + + // Returns the interpolated correspondence cost at (x,y). Cells with at least + // one 'unknown' interpolation point result in "MaxCorrespondenceCost()" with + // zero gradient. + template + T GetCorrespondenceCost(const T& x, const T& y) const { + float x1, y1, x2, y2; + ComputeInterpolationDataPoints(x, y, &x1, &y1, &x2, &y2); + + const Eigen::Array2i index1 = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x1, y1)); + + const float w11 = tsdf_.GetWeight(index1); + const float w12 = tsdf_.GetWeight(index1 + Eigen::Array2i(-1, 0)); + const float w21 = tsdf_.GetWeight(index1 + Eigen::Array2i(0, -1)); + const float w22 = tsdf_.GetWeight(index1 + Eigen::Array2i(-1, -1)); + if (w11 == 0.0 || w12 == 0.0 || w21 == 0.0 || w22 == 0.0) { + return T(tsdf_.GetMaxCorrespondenceCost()); + } + + const float q11 = tsdf_.GetCorrespondenceCost(index1); + const float q12 = + tsdf_.GetCorrespondenceCost(index1 + Eigen::Array2i(-1, 0)); + const float q21 = + tsdf_.GetCorrespondenceCost(index1 + Eigen::Array2i(0, -1)); + const float q22 = + tsdf_.GetCorrespondenceCost(index1 + Eigen::Array2i(-1, -1)); + + return InterpolateBilinear(x, y, x1, y1, x2, y2, q11, q12, q21, q22); + } + + // Returns the interpolated weight at (x,y). + template + T GetWeight(const T& x, const T& y) const { + float x1, y1, x2, y2; + ComputeInterpolationDataPoints(x, y, &x1, &y1, &x2, &y2); + + const Eigen::Array2i index1 = + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x1, y1)); + const float q11 = tsdf_.GetWeight(index1); + const float q12 = tsdf_.GetWeight(index1 + Eigen::Array2i(-1, 0)); + const float q21 = tsdf_.GetWeight(index1 + Eigen::Array2i(0, -1)); + const float q22 = tsdf_.GetWeight(index1 + Eigen::Array2i(-1, -1)); + + return InterpolateBilinear(x, y, x1, y1, x2, y2, q11, q12, q21, q22); + } + + private: + template + void ComputeInterpolationDataPoints(const T& x, const T& y, float* x1, + float* y1, float* x2, float* y2) const { + const Eigen::Vector2f lower = CenterOfLowerPixel(x, y); + *x1 = lower.x(); + *y1 = lower.y(); + *x2 = lower.x() + static_cast(tsdf_.limits().resolution()); + *y2 = lower.y() + static_cast(tsdf_.limits().resolution()); + } + + template + T InterpolateBilinear(const T& x, const T& y, float x1, float y1, float x2, + float y2, float q11, float q12, float q21, + float q22) const { + const T normalized_x = (x - x1) / (x2 - x1); + const T normalized_y = (y - y1) / (y2 - y1); + const T q1 = (q12 - q11) * normalized_y + q11; + const T q2 = (q22 - q21) * normalized_y + q21; + return (q2 - q1) * normalized_x + q1; + } + + // Center of the next lower pixel, i.e., not necessarily the pixel containing + // (x, y). For each dimension, the largest pixel index so that the + // corresponding center is at most the given coordinate. + Eigen::Vector2f CenterOfLowerPixel(double x, double y) const { + // Center of the cell containing (x, y). + Eigen::Vector2f center = tsdf_.limits().GetCellCenter( + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y))); + // Move to the next lower pixel center. + if (center.x() > x) { + center.x() -= tsdf_.limits().resolution(); + } + if (center.y() > y) { + center.y() -= tsdf_.limits().resolution(); + } + return center; + } + + // Uses the scalar part of a Ceres Jet. + template + Eigen::Vector2f CenterOfLowerPixel(const T& jet_x, const T& jet_y) const { + return CenterOfLowerPixel(jet_x.a, jet_y.a); + } + + const TSDF2D& tsdf_; +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_INTERPOLATED_TSDF_2D_H_ \ No newline at end of file diff --git a/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d_test.cc new file mode 100644 index 0000000..c1f4fa6 --- /dev/null +++ b/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d_test.cc @@ -0,0 +1,127 @@ +/* + * 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/internal/2d/scan_matching/interpolated_tsdf_2d.h" + +#include "Eigen/Core" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +class InterpolatedTSDF2DTest : public ::testing::Test { + protected: + InterpolatedTSDF2DTest() + : tsdf_(MapLimits(1., Eigen::Vector2d(5.5, 5.5), CellLimits(10, 10)), + 1.0f, 10.0f, &conversion_tables_), + interpolated_tsdf_(tsdf_) {} + + float GetUninterpolatedCorrespondenceCost(float x, float y) const { + return tsdf_.GetCorrespondenceCost( + tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y))); + } + + float GetUninterpolatedWeight(float x, float y) const { + return tsdf_.GetWeight(tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y))); + } + + ValueConversionTables conversion_tables_; + TSDF2D tsdf_; + InterpolatedTSDF2D interpolated_tsdf_; +}; + +TEST_F(InterpolatedTSDF2DTest, InterpolatesGridPoints) { + std::vector inner_points = { + Eigen::Vector2f(1.f, 1.f), Eigen::Vector2f(2.f, 1.f), + Eigen::Vector2f(1.f, 2.f), Eigen::Vector2f(2.f, 2.f)}; + for (const Eigen::Vector2f& point : inner_points) { + tsdf_.SetCell(tsdf_.limits().GetCellIndex(point), 0.1f, 1.0f); + } + // Insert surrounding points. + for (size_t x = 0; x < 4; ++x) { + tsdf_.SetCell(tsdf_.limits().GetCellIndex( + Eigen::Vector2f(static_cast(x), 0.f)), + 0.1f, 1.0f); + tsdf_.SetCell(tsdf_.limits().GetCellIndex( + Eigen::Vector2f(static_cast(x), 3.f)), + 0.1f, 1.0f); + } + for (size_t y = 1; y < 3; ++y) { + tsdf_.SetCell(tsdf_.limits().GetCellIndex( + Eigen::Vector2f(0.f, static_cast(y))), + 0.1f, 1.0f); + tsdf_.SetCell(tsdf_.limits().GetCellIndex( + Eigen::Vector2f(3.f, static_cast(y))), + 0.1f, 1.0f); + } + for (const Eigen::Vector2f& point : inner_points) { + EXPECT_NEAR( + GetUninterpolatedCorrespondenceCost(point[0], point[1]), + interpolated_tsdf_.GetCorrespondenceCost(static_cast(point[0]), + static_cast(point[1])), + 1e-4); + EXPECT_NEAR(GetUninterpolatedWeight(point[0], point[1]), + interpolated_tsdf_.GetWeight(static_cast(point[0]), + static_cast(point[1])), + 1e-4); + } + // Check unknown cell. + EXPECT_NEAR(tsdf_.GetMaxCorrespondenceCost(), + interpolated_tsdf_.GetCorrespondenceCost(3.0, 2.0), 1e-4); + EXPECT_NEAR(GetUninterpolatedWeight(3.f, 2.f), + interpolated_tsdf_.GetWeight(3.0, 2.0), 1e-4); +} + +TEST_F(InterpolatedTSDF2DTest, InterpolatesWithinCell) { + const float tsd_00 = 0.1f; + const float tsd_01 = 0.2f; + const float tsd_10 = 0.3f; + const float tsd_11 = 0.4f; + const float w_00 = 1.f; + const float w_01 = 2.f; + const float w_10 = 3.f; + const float w_11 = 4.f; + + tsdf_.SetCell(tsdf_.limits().GetCellIndex(Eigen::Vector2f(0.f, 0.f)), tsd_00, + w_00); + tsdf_.SetCell(tsdf_.limits().GetCellIndex(Eigen::Vector2f(0.f, 1.f)), tsd_01, + w_01); + tsdf_.SetCell(tsdf_.limits().GetCellIndex(Eigen::Vector2f(1.f, 0.f)), tsd_10, + w_10); + tsdf_.SetCell(tsdf_.limits().GetCellIndex(Eigen::Vector2f(1.f, 1.f)), tsd_11, + w_11); + + const double kSampleStep = tsdf_.limits().resolution() / 100.; + for (double x = 0. + kSampleStep; x < 1.; x += tsdf_.limits().resolution()) { + for (double y = 0. + kSampleStep; y < 1.; + y += tsdf_.limits().resolution()) { + const float tsd_expected = (x * tsd_10 + (1.f - x) * tsd_00) * (1.f - y) + + (x * tsd_11 + (1.f - x) * tsd_01) * y; + EXPECT_NEAR(interpolated_tsdf_.GetCorrespondenceCost(x, y), tsd_expected, + 1e-3); + const float w_expected = (x * w_10 + (1.f - x) * w_00) * (1.f - y) + + (x * w_11 + (1.f - x) * w_01) * y; + EXPECT_NEAR(interpolated_tsdf_.GetWeight(x, y), w_expected, 1e-3); + } + } +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer