Change CastRays to directly take the ProbabilityGrid. (#386)

PAIR=wohe
master
Holger Rapp 2017-07-05 13:39:44 +02:00 committed by GitHub
parent 611f244d7a
commit a313448bf0
4 changed files with 37 additions and 40 deletions

View File

@ -53,19 +53,11 @@ RangeDataInserter::RangeDataInserter(
void RangeDataInserter::Insert(const sensor::RangeData& range_data,
ProbabilityGrid* const probability_grid) const {
CHECK_NOTNULL(probability_grid)->StartUpdate();
// By not starting a new update after hits are inserted, we give hits priority
// (i.e. no hits will be ignored because of a miss in the same cell).
CastRays(range_data, probability_grid->limits(),
[this, &probability_grid](const Eigen::Array2i& hit) {
probability_grid->ApplyLookupTable(hit, hit_table_);
},
[this, &probability_grid](const Eigen::Array2i& miss) {
if (options_.insert_free_space()) {
probability_grid->ApplyLookupTable(miss, miss_table_);
}
});
CHECK_NOTNULL(probability_grid)->StartUpdate();
CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(),
probability_grid);
}
} // namespace mapping_2d

View File

@ -45,9 +45,6 @@ class RangeDataInserter {
void Insert(const sensor::RangeData& range_data,
ProbabilityGrid* probability_grid) const;
const std::vector<uint16>& hit_table() const { return hit_table_; }
const std::vector<uint16>& miss_table() const { return miss_table_; }
private:
const proto::RangeDataInserterOptions options_;
const std::vector<uint16> hit_table_;

View File

@ -28,10 +28,11 @@ constexpr int kSubpixelScale = 1000;
// and 'end' are coordinates at subpixel precision. We compute all pixels in
// which some part of the line segment connecting 'begin' and 'end' lies.
void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
const std::function<void(const Eigen::Array2i&)>& visitor) {
const std::vector<uint16>& miss_table,
ProbabilityGrid* const probability_grid) {
// For simplicity, we order 'begin' and 'end' by their x coordinate.
if (begin.x() > end.x()) {
CastRay(end, begin, visitor);
CastRay(end, begin, miss_table, probability_grid);
return;
}
@ -46,7 +47,7 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
std::min(begin.y(), end.y()) / kSubpixelScale);
const int end_y = std::max(begin.y(), end.y()) / kSubpixelScale;
for (; current.y() <= end_y; ++current.y()) {
visitor(current);
probability_grid->ApplyLookupTable(current, miss_table);
}
return;
}
@ -86,11 +87,11 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
sub_y += dy * first_pixel;
if (dy > 0) {
while (true) {
visitor(current);
probability_grid->ApplyLookupTable(current, miss_table);
while (sub_y > denominator) {
sub_y -= denominator;
++current.y();
visitor(current);
probability_grid->ApplyLookupTable(current, miss_table);
}
++current.x();
if (sub_y == denominator) {
@ -105,11 +106,11 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
}
// Move from the pixel border on the right to 'end'.
sub_y += dy * last_pixel;
visitor(current);
probability_grid->ApplyLookupTable(current, miss_table);
while (sub_y > denominator) {
sub_y -= denominator;
++current.y();
visitor(current);
probability_grid->ApplyLookupTable(current, miss_table);
}
CHECK_NE(sub_y, denominator);
CHECK_EQ(current.y(), end.y() / kSubpixelScale);
@ -118,11 +119,11 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
// Same for lines non-ascending in y coordinates.
while (true) {
visitor(current);
probability_grid->ApplyLookupTable(current, miss_table);
while (sub_y < 0) {
sub_y += denominator;
--current.y();
visitor(current);
probability_grid->ApplyLookupTable(current, miss_table);
}
++current.x();
if (sub_y == 0) {
@ -135,11 +136,11 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
sub_y += dy * 2 * kSubpixelScale;
}
sub_y += dy * last_pixel;
visitor(current);
probability_grid->ApplyLookupTable(current, miss_table);
while (sub_y < 0) {
sub_y += denominator;
--current.y();
visitor(current);
probability_grid->ApplyLookupTable(current, miss_table);
}
CHECK_NE(sub_y, 0);
CHECK_EQ(current.y(), end.y() / kSubpixelScale);
@ -147,9 +148,12 @@ void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
} // namespace
void CastRays(const sensor::RangeData& range_data, const MapLimits& limits,
const std::function<void(const Eigen::Array2i&)>& hit_visitor,
const std::function<void(const Eigen::Array2i&)>& miss_visitor) {
void CastRays(const sensor::RangeData& range_data,
const std::vector<uint16>& hit_table,
const std::vector<uint16>& miss_table,
const bool insert_free_space,
ProbabilityGrid* const probability_grid) {
const MapLimits& limits = probability_grid->limits();
const double superscaled_resolution = limits.resolution() / kSubpixelScale;
const MapLimits superscaled_limits(
superscaled_resolution, limits.max(),
@ -158,19 +162,22 @@ void CastRays(const sensor::RangeData& range_data, const MapLimits& limits,
const Eigen::Array2i begin =
superscaled_limits.GetXYIndexOfCellContainingPoint(range_data.origin.x(),
range_data.origin.y());
// Compute and add the end points.
std::vector<Eigen::Array2i> ends;
ends.reserve(range_data.returns.size());
for (const Eigen::Vector3f& hit : range_data.returns) {
ends.push_back(
superscaled_limits.GetXYIndexOfCellContainingPoint(hit.x(), hit.y()));
hit_visitor(ends.back() / kSubpixelScale);
probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
}
if (!insert_free_space) {
return;
}
// Now add the misses.
for (const Eigen::Array2i& end : ends) {
CastRay(begin, end, miss_visitor);
CastRay(begin, end, miss_table, probability_grid);
}
// Finally, compute and add empty rays based on misses in the scan.
@ -178,7 +185,7 @@ void CastRays(const sensor::RangeData& range_data, const MapLimits& limits,
CastRay(begin,
superscaled_limits.GetXYIndexOfCellContainingPoint(
missing_echo.x(), missing_echo.y()),
miss_visitor);
miss_table, probability_grid);
}
}

View File

@ -17,10 +17,10 @@
#ifndef CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_
#define CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_
#include <functional>
#include <vector>
#include "cartographer/mapping_2d/map_limits.h"
#include "cartographer/mapping_2d/xy_index.h"
#include "cartographer/common/port.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/transform.h"
@ -28,11 +28,12 @@
namespace cartographer {
namespace mapping_2d {
// For each ray in 'range_data', calls 'hit_visitor' and 'miss_visitor' on the
// appropriate cells. Hits are handled before misses.
void CastRays(const sensor::RangeData& range_data, const MapLimits& limits,
const std::function<void(const Eigen::Array2i&)>& hit_visitor,
const std::function<void(const Eigen::Array2i&)>& miss_visitor);
// For each ray in 'range_data', inserts hits and misses into
// 'probability_grid'. Hits are handled before misses.
void CastRays(const sensor::RangeData& range_data,
const std::vector<uint16>& hit_table,
const std::vector<uint16>& miss_table, bool insert_free_space,
ProbabilityGrid* probability_grid);
} // namespace mapping_2d
} // namespace cartographer