diff --git a/cartographer/mapping/3d/hybrid_grid.h b/cartographer/mapping/3d/hybrid_grid.h index 10d2a59..709f279 100644 --- a/cartographer/mapping/3d/hybrid_grid.h +++ b/cartographer/mapping/3d/hybrid_grid.h @@ -544,6 +544,32 @@ class HybridGrid : public HybridGridBase { std::vector update_indices_; }; +struct AverageIntensityData { + float sum = 0.f; + int count = 0; +}; + +class IntensityHybridGrid : public HybridGridBase { + public: + explicit IntensityHybridGrid(const float resolution) + : HybridGridBase(resolution) {} + + void AddIntensity(const Eigen::Array3i& index, const float intensity) { + AverageIntensityData* const cell = mutable_value(index); + cell->count += 1; + cell->sum += intensity; + } + + float GetIntensity(const Eigen::Array3i& index) const { + const AverageIntensityData& cell = value(index); + if (cell.count == 0) { + return 0.f; + } else { + return cell.sum / cell.count; + } + } +}; + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/3d/hybrid_grid_test.cc b/cartographer/mapping/3d/hybrid_grid_test.cc index 333b114..f600032 100644 --- a/cartographer/mapping/3d/hybrid_grid_test.cc +++ b/cartographer/mapping/3d/hybrid_grid_test.cc @@ -84,6 +84,23 @@ TEST(HybridGridTest, GetProbability) { } } +TEST(HybridGridTest, GetIntensity) { + IntensityHybridGrid hybrid_grid(1.f); + const Eigen::Array3i cell_index = + hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f)); + const float intensity = 58.0f; + + EXPECT_NEAR(hybrid_grid.GetIntensity(cell_index), 0.0f, 1e-9); + hybrid_grid.AddIntensity(cell_index, intensity); + EXPECT_NEAR(hybrid_grid.GetIntensity(cell_index), intensity, 1e-9); + for (const Eigen::Array3i& index : + {hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 2.f, 1.f)), + hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 1.f, 1.f)), + hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 2.f, 1.f))}) { + EXPECT_NEAR(hybrid_grid.GetIntensity(index), 0.0f, 1e-9); + } +} + MATCHER_P(AllCwiseEqual, index, "") { return (arg == index).all(); } TEST(HybridGridTest, GetCellIndex) { diff --git a/cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h b/cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h index d2b30d7..b38fdbe 100644 --- a/cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h +++ b/cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h @@ -25,22 +25,23 @@ namespace cartographer { namespace mapping { namespace scan_matching { -// Interpolates between HybridGrid probability voxels. We use the tricubic +// Interpolates between HybridGrid voxels. We use the tricubic // interpolation which interpolates the values and has vanishing derivative at // these points. // // This class is templated to work with the autodiff that Ceres provides. // For this reason, it is also important that the interpolation scheme be // continuously differentiable. +template class InterpolatedGrid { public: - explicit InterpolatedGrid(const HybridGrid& hybrid_grid) + explicit InterpolatedGrid(const HybridGridType& hybrid_grid) : hybrid_grid_(hybrid_grid) {} - InterpolatedGrid(const InterpolatedGrid&) = delete; - InterpolatedGrid& operator=(const InterpolatedGrid&) = delete; + InterpolatedGrid(const InterpolatedGrid&) = delete; + InterpolatedGrid& operator=(const InterpolatedGrid&) = delete; - // Returns the interpolated probability at (x, y, z) of the HybridGrid + // Returns the interpolated value at (x, y, z) of the HybridGrid // used to perform the interpolation. // // This is a piecewise, continuously differentiable function. We use the @@ -48,27 +49,27 @@ class InterpolatedGrid { // tensor product volume of piecewise cubic polynomials that interpolate // the values, and have vanishing derivative at the interval boundaries. template - T GetProbability(const T& x, const T& y, const T& z) const { + T GetInterpolatedValue(const T& x, const T& y, const T& z) const { double x1, y1, z1, x2, y2, z2; ComputeInterpolationDataPoints(x, y, z, &x1, &y1, &z1, &x2, &y2, &z2); const Eigen::Array3i index1 = hybrid_grid_.GetCellIndex(Eigen::Vector3f(x1, y1, z1)); - const double q111 = hybrid_grid_.GetProbability(index1); + const double q111 = GetValue(hybrid_grid_, index1); const double q112 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 0, 1)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 0, 1)); const double q121 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 1, 0)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 1, 0)); const double q122 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 1, 1)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 1, 1)); const double q211 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 0, 0)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 0, 0)); const double q212 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 0, 1)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 0, 1)); const double q221 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 1, 0)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 1, 0)); const double q222 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 1, 1)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 1, 1)); const T normalized_x = (x - x1) / (x2 - x1); const T normalized_y = (y - y1) / (y2 - y1); @@ -145,9 +146,22 @@ class InterpolatedGrid { return CenterOfLowerVoxel(jet_x.a, jet_y.a, jet_z.a); } - const HybridGrid& hybrid_grid_; + static float GetValue(const HybridGrid& probability_grid, + const Eigen::Array3i& index) { + return probability_grid.GetProbability(index); + } + + static float GetValue(const IntensityHybridGrid& intensity_grid, + const Eigen::Array3i& index) { + return intensity_grid.GetIntensity(index); + } + + const HybridGridType& hybrid_grid_; }; +using InterpolatedIntensityGrid = InterpolatedGrid; +using InterpolatedProbabilityGrid = InterpolatedGrid; + } // namespace scan_matching } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/internal/3d/scan_matching/interpolated_grid_test.cc b/cartographer/mapping/internal/3d/scan_matching/interpolated_grid_test.cc index f11126d..503b2ec 100644 --- a/cartographer/mapping/internal/3d/scan_matching/interpolated_grid_test.cc +++ b/cartographer/mapping/internal/3d/scan_matching/interpolated_grid_test.cc @@ -44,7 +44,7 @@ class InterpolatedGridTest : public ::testing::Test { } HybridGrid hybrid_grid_; - InterpolatedGrid interpolated_grid_; + InterpolatedProbabilityGrid interpolated_grid_; }; TEST_F(InterpolatedGridTest, InterpolatesGridPoints) { @@ -52,7 +52,7 @@ TEST_F(InterpolatedGridTest, InterpolatesGridPoints) { for (double y = 1.; y < 5.; y += hybrid_grid_.resolution()) { for (double x = -8.; x < -2.; x += hybrid_grid_.resolution()) { EXPECT_NEAR(GetHybridGridProbability(x, y, z), - interpolated_grid_.GetProbability(x, y, z), 1e-6); + interpolated_grid_.GetInterpolatedValue(x, y, z), 1e-6); } } } @@ -73,10 +73,11 @@ TEST_F(InterpolatedGridTest, MonotonicBehaviorBetweenGridPointsInX) { for (double sample = kSampleStep; sample < hybrid_grid_.resolution() - 2 * kSampleStep; sample += kSampleStep) { - EXPECT_LT(0., grid_difference * (interpolated_grid_.GetProbability( - x + sample + kSampleStep, y, z) - - interpolated_grid_.GetProbability( - x + sample, y, z))); + EXPECT_LT(0., + grid_difference * (interpolated_grid_.GetInterpolatedValue( + x + sample + kSampleStep, y, z) - + interpolated_grid_.GetInterpolatedValue( + x + sample, y, z))); } } } diff --git a/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h b/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h index 5e0cb80..ff5b653 100644 --- a/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h +++ b/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h @@ -73,7 +73,7 @@ class OccupiedSpaceCostFunction3D { const Eigen::Matrix world = transform * point_cloud_[i].position.cast(); const T probability = - interpolated_grid_.GetProbability(world[0], world[1], world[2]); + interpolated_grid_.GetInterpolatedValue(world[0], world[1], world[2]); residual[i] = scaling_factor_ * (1. - probability); } return true; @@ -81,7 +81,7 @@ class OccupiedSpaceCostFunction3D { const double scaling_factor_; const sensor::PointCloud& point_cloud_; - const InterpolatedGrid interpolated_grid_; + const InterpolatedProbabilityGrid interpolated_grid_; }; } // namespace scan_matching