12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
cartographer/mapping_2d/range_data_inserter.cc
View File

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

3
cartographer/mapping_2d/range_data_inserter.h
View File

@ -45,9 +45,6 @@ class RangeDataInserter {
void Insert(const sensor::RangeData& range_data, void Insert(const sensor::RangeData& range_data,
ProbabilityGrid* probability_grid) const; 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: private:
const proto::RangeDataInserterOptions options_; const proto::RangeDataInserterOptions options_;
const std::vector<uint16> hit_table_; const std::vector<uint16> hit_table_;

43
cartographer/mapping_2d/ray_casting.cc
View File

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

17
cartographer/mapping_2d/ray_casting.h
View File

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