Add IntensityHybridGrid. (#1739)
Adds a new structure IntensityHybridGrid, similar to HybridGrid but which stores intensities instead of probabilities. InterpolatedGrid is adapted to handle both types of HybridGrids. Signed-off-by: Wolfgang Hess <whess@lyft.com>master
parent
01cb6b1d6f
commit
da779339fa
|
@ -544,6 +544,32 @@ class HybridGrid : public HybridGridBase<uint16> {
|
|||
std::vector<ValueType*> update_indices_;
|
||||
};
|
||||
|
||||
struct AverageIntensityData {
|
||||
float sum = 0.f;
|
||||
int count = 0;
|
||||
};
|
||||
|
||||
class IntensityHybridGrid : public HybridGridBase<AverageIntensityData> {
|
||||
public:
|
||||
explicit IntensityHybridGrid(const float resolution)
|
||||
: HybridGridBase<AverageIntensityData>(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
|
||||
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -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 HybridGridType>
|
||||
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<HybridGridType>&) = delete;
|
||||
InterpolatedGrid& operator=(const InterpolatedGrid<HybridGridType>&) = 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 <typename T>
|
||||
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<IntensityHybridGrid>;
|
||||
using InterpolatedProbabilityGrid = InterpolatedGrid<HybridGrid>;
|
||||
|
||||
} // namespace scan_matching
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
|
|
@ -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)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -73,7 +73,7 @@ class OccupiedSpaceCostFunction3D {
|
|||
const Eigen::Matrix<T, 3, 1> world =
|
||||
transform * point_cloud_[i].position.cast<T>();
|
||||
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
|
||||
|
|
Loading…
Reference in New Issue