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

Replace ProbabilityGrid in Submap2D by Grid2D (#1097)

master
Kevin Daun 2018-04-20 22:58:46 +02:00 committed by Christoph Schütte
parent e2623991da
commit d29153a744
13 changed files with 112 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cartographer/mapping/2d/grid_2d.h
View File

@ -21,6 +21,7 @@
#include "cartographer/mapping/2d/map_limits.h"
#include "cartographer/mapping/2d/proto/grid_2d.pb.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h"
namespace cartographer {
namespace mapping {
@ -58,8 +59,14 @@ class Grid2D {
// after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
void GrowLimits(const Eigen::Vector2f& point);
virtual std::unique_ptr<Grid2D> ComputeCroppedGrid() const = 0;
virtual proto::Grid2D ToProto() const;
virtual bool DrawToSubmapTexture(
proto::SubmapQuery::Response::SubmapTexture* const texture,
transform::Rigid3d local_pose) const = 0;
protected:
const std::vector<uint16>& correspondence_cost_cells() const {
return correspondence_cost_cells_;

62
cartographer/mapping/2d/probability_grid.cc
View File

@ -17,7 +17,9 @@
#include <limits>
#include "cartographer/common/make_unique.h"
#include "cartographer/mapping/probability_values.h"
#include "cartographer/mapping/submaps.h"
namespace cartographer {
namespace mapping {
@ -76,5 +78,65 @@ proto::Grid2D ProbabilityGrid::ToProto() const {
return result;
}
std::unique_ptr<Grid2D> ProbabilityGrid::ComputeCroppedGrid() const {
Eigen::Array2i offset;
CellLimits cell_limits;
ComputeCroppedLimits(&offset, &cell_limits);
const double resolution = limits().resolution();
const Eigen::Vector2d max =
limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x());
std::unique_ptr<ProbabilityGrid> cropped_grid =
common::make_unique<ProbabilityGrid>(
MapLimits(resolution, max, cell_limits));
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
if (!IsKnown(xy_index + offset)) continue;
cropped_grid->SetProbability(xy_index, GetProbability(xy_index + offset));
}
return std::unique_ptr<Grid2D>(cropped_grid.release());
}
bool ProbabilityGrid::DrawToSubmapTexture(
proto::SubmapQuery::Response::SubmapTexture* const texture,
transform::Rigid3d local_pose) const {
Eigen::Array2i offset;
CellLimits cell_limits;
ComputeCroppedLimits(&offset, &cell_limits);
std::string cells;
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
if (!IsKnown(xy_index + offset)) {
cells.push_back(0 /* unknown log odds value */);
cells.push_back(0 /* alpha */);
continue;
}
// We would like to add 'delta' but this is not possible using a value and
// alpha. We use premultiplied alpha, so when 'delta' is positive we can
// add it by setting 'alpha' to zero. If it is negative, we set 'value' to
// zero, and use 'alpha' to subtract. This is only correct when the pixel
// is currently white, so walls will look too gray. This should be hard to
// detect visually for the user, though.
const int delta =
128 - ProbabilityToLogOddsInteger(GetProbability(xy_index + offset));
const uint8 alpha = delta > 0 ? 0 : -delta;
const uint8 value = delta > 0 ? delta : 0;
cells.push_back(value);
cells.push_back((value || alpha) ? alpha : 1);
}
common::FastGzipString(cells, texture->mutable_cells());
texture->set_width(cell_limits.num_x_cells);
texture->set_height(cell_limits.num_y_cells);
const double resolution = limits().resolution();
texture->set_resolution(resolution);
const double max_x = limits().max().x() - resolution * offset.y();
const double max_y = limits().max().y() - resolution * offset.x();
*texture->mutable_slice_pose() = transform::ToProto(
local_pose.inverse() *
transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
return true;
}
} // namespace mapping
} // namespace cartographer

4
cartographer/mapping/2d/probability_grid.h
View File

@ -52,6 +52,10 @@ class ProbabilityGrid : public Grid2D {
float GetProbability(const Eigen::Array2i& cell_index) const;
virtual proto::Grid2D ToProto() const override;
virtual std::unique_ptr<Grid2D> ComputeCroppedGrid() const override;
virtual bool DrawToSubmapTexture(
proto::SubmapQuery::Response::SubmapTexture* const texture,
transform::Rigid3d local_pose) const override;
};
} // namespace mapping

9
cartographer/mapping/2d/range_data_inserter_2d.cc
View File

@ -52,14 +52,13 @@ RangeDataInserter2D::RangeDataInserter2D(
miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
Odds(options.miss_probability()))) {}
void RangeDataInserter2D::Insert(
const sensor::RangeData& range_data,
ProbabilityGrid* const probability_grid) const {
void RangeDataInserter2D::Insert(const sensor::RangeData& range_data,
Grid2D* const grid) const {
// By not finishing the 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, hit_table_, miss_table_, options_.insert_free_space(),
CHECK_NOTNULL(probability_grid));
probability_grid->FinishUpdate();
CHECK_NOTNULL(static_cast<ProbabilityGrid*>(grid)));
grid->FinishUpdate();
}
} // namespace mapping

3
cartographer/mapping/2d/range_data_inserter_2d.h
View File

@ -43,8 +43,7 @@ class RangeDataInserter2D {
RangeDataInserter2D& operator=(const RangeDataInserter2D&) = delete;
// Inserts 'range_data' into 'probability_grid'.
void Insert(const sensor::RangeData& range_data,
ProbabilityGrid* probability_grid) const;
void Insert(const sensor::RangeData& range_data, Grid2D* grid) const;
private:
const proto::RangeDataInserterOptions2D options_;

80
cartographer/mapping/2d/submap_2d.cc
View File

@ -30,25 +30,6 @@
namespace cartographer {
namespace mapping {
ProbabilityGrid ComputeCroppedProbabilityGrid(
const ProbabilityGrid& probability_grid) {
Eigen::Array2i offset;
CellLimits limits;
probability_grid.ComputeCroppedLimits(&offset, &limits);
const double resolution = probability_grid.limits().resolution();
const Eigen::Vector2d max =
probability_grid.limits().max() -
resolution * Eigen::Vector2d(offset.y(), offset.x());
ProbabilityGrid cropped_grid(MapLimits(resolution, max, limits));
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) {
if (probability_grid.IsKnown(xy_index + offset)) {
cropped_grid.SetProbability(
xy_index, probability_grid.GetProbability(xy_index + offset));
}
}
return cropped_grid;
}
proto::SubmapsOptions2D CreateSubmapsOptions2D(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::SubmapsOptions2D options;
@ -65,13 +46,13 @@ proto::SubmapsOptions2D CreateSubmapsOptions2D(
Submap2D::Submap2D(const MapLimits& limits, const Eigen::Vector2f& origin)
: Submap(transform::Rigid3d::Translation(
Eigen::Vector3d(origin.x(), origin.y(), 0.))),
probability_grid_(common::make_unique<ProbabilityGrid>(limits)) {}
grid_(common::make_unique<ProbabilityGrid>(limits)) {}
Submap2D::Submap2D(const proto::Submap2D& proto)
: Submap(transform::ToRigid3(proto.local_pose())) {
if (proto.has_grid()) {
CHECK(proto.grid().has_probability_grid_2d());
probability_grid_ = common::make_unique<ProbabilityGrid>(proto.grid());
grid_ = common::make_unique<ProbabilityGrid>(proto.grid());
}
set_num_range_data(proto.num_range_data());
set_finished(proto.finished());
@ -84,8 +65,8 @@ void Submap2D::ToProto(proto::Submap* const proto,
submap_2d->set_num_range_data(num_range_data());
submap_2d->set_finished(finished());
if (include_probability_grid_data) {
CHECK(probability_grid_);
*submap_2d->mutable_grid() = probability_grid_->ToProto();
CHECK(grid_);
*submap_2d->mutable_grid() = grid_->ToProto();
}
}
@ -96,71 +77,32 @@ void Submap2D::UpdateFromProto(const proto::Submap& proto) {
set_finished(submap_2d.finished());
if (proto.submap_2d().has_grid()) {
CHECK(proto.submap_2d().grid().has_probability_grid_2d());
probability_grid_ = common::make_unique<ProbabilityGrid>(submap_2d.grid());
grid_ = common::make_unique<ProbabilityGrid>(submap_2d.grid());
}
}
void Submap2D::ToResponseProto(
const transform::Rigid3d&,
proto::SubmapQuery::Response* const response) const {
if (!probability_grid_) return;
if (!grid_) return;
response->set_submap_version(num_range_data());
Eigen::Array2i offset;
CellLimits limits;
probability_grid_->ComputeCroppedLimits(&offset, &limits);
std::string cells;
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) {
if (probability_grid_->IsKnown(xy_index + offset)) {
// We would like to add 'delta' but this is not possible using a value and
// alpha. We use premultiplied alpha, so when 'delta' is positive we can
// add it by setting 'alpha' to zero. If it is negative, we set 'value' to
// zero, and use 'alpha' to subtract. This is only correct when the pixel
// is currently white, so walls will look too gray. This should be hard to
// detect visually for the user, though.
const int delta =
128 - ProbabilityToLogOddsInteger(
probability_grid_->GetProbability(xy_index + offset));
const uint8 alpha = delta > 0 ? 0 : -delta;
const uint8 value = delta > 0 ? delta : 0;
cells.push_back(value);
cells.push_back((value || alpha) ? alpha : 1);
} else {
constexpr uint8 kUnknownLogOdds = 0;
cells.push_back(static_cast<uint8>(kUnknownLogOdds)); // value
cells.push_back(0); // alpha
}
}
proto::SubmapQuery::Response::SubmapTexture* const texture =
response->add_textures();
common::FastGzipString(cells, texture->mutable_cells());
texture->set_width(limits.num_x_cells);
texture->set_height(limits.num_y_cells);
const double resolution = probability_grid_->limits().resolution();
texture->set_resolution(resolution);
const double max_x =
probability_grid_->limits().max().x() - resolution * offset.y();
const double max_y =
probability_grid_->limits().max().y() - resolution * offset.x();
*texture->mutable_slice_pose() = transform::ToProto(
local_pose().inverse() *
transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
grid()->DrawToSubmapTexture(texture, local_pose());
}
void Submap2D::InsertRangeData(const sensor::RangeData& range_data,
const RangeDataInserter2D& range_data_inserter) {
CHECK(probability_grid_);
CHECK(grid_);
CHECK(!finished());
range_data_inserter.Insert(range_data, probability_grid_.get());
range_data_inserter.Insert(range_data, grid_.get());
set_num_range_data(num_range_data() + 1);
}
void Submap2D::Finish() {
CHECK(probability_grid_);
CHECK(grid_);
CHECK(!finished());
*probability_grid_ = ComputeCroppedProbabilityGrid(*probability_grid_);
grid_ = grid_->ComputeCroppedGrid();
set_finished(true);
}

9
cartographer/mapping/2d/submap_2d.h
View File

@ -22,8 +22,8 @@
#include "Eigen/Core"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping/2d/grid_2d.h"
#include "cartographer/mapping/2d/map_limits.h"
#include "cartographer/mapping/2d/probability_grid.h"
#include "cartographer/mapping/2d/proto/submaps_options_2d.pb.h"
#include "cartographer/mapping/2d/range_data_inserter_2d.h"
#include "cartographer/mapping/proto/serialization.pb.h"
@ -36,9 +36,6 @@
namespace cartographer {
namespace mapping {
ProbabilityGrid ComputeCroppedProbabilityGrid(
const ProbabilityGrid& probability_grid);
proto::SubmapsOptions2D CreateSubmapsOptions2D(
common::LuaParameterDictionary* parameter_dictionary);
@ -54,7 +51,7 @@ class Submap2D : public Submap {
void ToResponseProto(const transform::Rigid3d& global_submap_pose,
proto::SubmapQuery::Response* response) const override;
const ProbabilityGrid& probability_grid() const { return *probability_grid_; }
const Grid2D* grid() const { return grid_.get(); }
// Insert 'range_data' into this submap using 'range_data_inserter'. The
// submap must not be finished yet.
@ -63,7 +60,7 @@ class Submap2D : public Submap {
void Finish();
private:
std::unique_ptr<ProbabilityGrid> probability_grid_;
std::unique_ptr<Grid2D> grid_;
};
// Except during initialization when only a single submap exists, there are

12
cartographer/mapping/2d/submap_2d_test.cc
View File

@ -83,12 +83,12 @@ TEST(Submap2DTest, ToFromProto) {
actual.local_pose().rotation(), 1e-6));
EXPECT_EQ(expected.num_range_data(), actual.num_range_data());
EXPECT_EQ(expected.finished(), actual.finished());
EXPECT_NEAR(expected.probability_grid().limits().resolution(),
actual.probability_grid().limits().resolution(), 1e-6);
EXPECT_TRUE(expected.probability_grid().limits().max().isApprox(
actual.probability_grid().limits().max(), 1e-6));
EXPECT_EQ(expected.probability_grid().limits().cell_limits().num_x_cells,
actual.probability_grid().limits().cell_limits().num_x_cells);
EXPECT_NEAR(expected.grid()->limits().resolution(),
actual.grid()->limits().resolution(), 1e-6);
EXPECT_TRUE(expected.grid()->limits().max().isApprox(
actual.grid()->limits().max(), 1e-6));
EXPECT_EQ(expected.grid()->limits().cell_limits().num_x_cells,
actual.grid()->limits().cell_limits().num_x_cells);
}
} // namespace

8
cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
View File

@ -76,9 +76,11 @@ std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
return nullptr;
}
if (options_.use_online_correlative_scan_matching()) {
// todo(kdaun) add CHECK on options to guarantee grid is a probability grid
double score = real_time_correlative_scan_matcher_.Match(
pose_prediction, filtered_gravity_aligned_point_cloud,
matching_submap->probability_grid(), &initial_ceres_pose);
*static_cast<const ProbabilityGrid*>(matching_submap->grid()),
&initial_ceres_pose);
kFastCorrelativeScanMatcherScoreMetric->Observe(score);
}
@ -86,8 +88,8 @@ std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
ceres::Solver::Summary summary;
ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
filtered_gravity_aligned_point_cloud,
matching_submap->probability_grid(),
pose_observation.get(), &summary);
*matching_submap->grid(), pose_observation.get(),
&summary);
if (pose_observation) {
kCeresScanMatcherCostMetric->Observe(summary.final_cost);
double residual_distance =

11
cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc
View File

@ -51,7 +51,7 @@ Eigen::Vector2d GetCornerOfFirstSubmap(
const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data) {
auto submap_2d = std::static_pointer_cast<const Submap2D>(
submap_data.begin()->data.submap);
return submap_2d->probability_grid().limits().max();
return submap_2d->grid()->limits().max();
}
// Iterates over every cell in a submap, transforms the center of the cell to
@ -68,13 +68,12 @@ std::set<SubmapId> AddSubmapsToSubmapCoverageGrid2D(
if (freshness == submap_freshness.end()) continue;
if (!submap.data.submap->finished()) continue;
all_submap_ids.insert(submap.id);
const ProbabilityGrid& probability_grid =
std::static_pointer_cast<const Submap2D>(submap.data.submap)
->probability_grid();
const Grid2D& grid =
*std::static_pointer_cast<const Submap2D>(submap.data.submap)->grid();
// Iterate over every cell in a submap.
Eigen::Array2i offset;
CellLimits cell_limits;
probability_grid.ComputeCroppedLimits(&offset, &cell_limits);
grid.ComputeCroppedLimits(&offset, &cell_limits);
if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) {
LOG(WARNING) << "Empty grid found in submap ID = " << submap.id;
continue;
@ -83,7 +82,7 @@ std::set<SubmapId> AddSubmapsToSubmapCoverageGrid2D(
transform::Project2D(submap.data.pose);
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
const Eigen::Array2i index = xy_index + offset;
if (!probability_grid.IsKnown(index)) continue;
if (!grid.IsKnown(index)) continue;
const transform::Rigid2d center_of_cell_in_global_frame =
projected_submap_pose *
transform::Rigid2d::Translation({index.x() + 0.5, index.y() + 0.5});

4
cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.cc
View File

@ -85,7 +85,7 @@ void ConstraintBuilder2D::MaybeAddConstraint(
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
submap_id, submap->grid(), [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id,
false, /* match_full_submap */
constant_data, initial_relative_pose, constraint);
@ -104,7 +104,7 @@ void ConstraintBuilder2D::MaybeAddGlobalConstraint(
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
submap_id, submap->grid(), [=]() EXCLUDES(mutex_) {
ComputeConstraint(
submap_id, submap, node_id, true, /* match_full_submap */
constant_data, transform::Rigid2d::Identity(), constraint);

2
cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h
View File

@ -43,7 +43,7 @@ class CeresScanMatcher2D {
CeresScanMatcher2D(const CeresScanMatcher2D&) = delete;
CeresScanMatcher2D& operator=(const CeresScanMatcher2D&) = delete;
// Aligns 'point_cloud' within the 'probability_grid' given an
// Aligns 'point_cloud' within the 'grid' given an
// 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
// 'summary'.
void Match(const Eigen::Vector2d& target_translation,

2
cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h
View File

@ -78,7 +78,7 @@ class PrecomputationGrid2D {
private:
uint8 ComputeCellValue(float probability) const;
// Offset of the precomputation grid in relation to the 'probability_grid'
// Offset of the precomputation grid in relation to the 'grid'
// including the additional 'width' - 1 cells.
const Eigen::Array2i offset_;