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

Clean up and simplify MapLimits. (#12) 

MapLimits no longer keeps redundant data.
master
Wolfgang Hess 2016-08-25 15:40:05 +02:00 committed by GitHub
parent 9134a8967d
commit 817dc884d5
15 changed files with 71 additions and 156 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cartographer/mapping/submaps.cc
View File

@ -80,10 +80,10 @@ void Submaps::AddProbabilityGridToResponse(
response->set_height(limits.num_y_cells);
const double resolution = probability_grid.limits().resolution();
response->set_resolution(resolution);
const double max_x = probability_grid.limits().edge_limits().max().x() -
resolution * offset.y();
const double max_y = probability_grid.limits().edge_limits().max().y() -
resolution * offset.x();
const double max_x =
probability_grid.limits().max().x() - resolution * offset.y();
const double max_y =
probability_grid.limits().max().y() - resolution * offset.x();
*response->mutable_slice_pose() = transform::ToProto(
local_submap_pose.inverse() *
transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));

1
cartographer/mapping_2d/CMakeLists.txt
View File

@ -144,6 +144,7 @@ google_library(mapping_2d_submaps
DEPENDS
common_lua_parameter_dictionary
common_make_unique
common_port
mapping_2d_laser_fan_inserter
mapping_2d_map_limits
mapping_2d_probability_grid

15
cartographer/mapping_2d/laser_fan_inserter_test.cc
View File

@ -32,12 +32,7 @@ class LaserFanInserterTest : public ::testing::Test {
protected:
LaserFanInserterTest()
: probability_grid_(
MapLimits(1., Eigen::AlignedBox2d(Eigen::Vector2d(-4., 0.),
Eigen::Vector2d(0., 4.)))) {
const CellLimits& cell_limits = probability_grid_.limits().cell_limits();
CHECK_EQ(5, cell_limits.num_x_cells);
CHECK_EQ(5, cell_limits.num_y_cells);
MapLimits(1., Eigen::Vector2d(1., 5.), CellLimits(5, 5))) {
auto parameter_dictionary = common::MakeDictionary(
"return { "
"insert_free_space = true, "
@ -68,12 +63,8 @@ class LaserFanInserterTest : public ::testing::Test {
TEST_F(LaserFanInserterTest, InsertPointCloud) {
InsertPointCloud();
const Eigen::AlignedBox2d& edge_limits =
probability_grid_.limits().edge_limits();
EXPECT_NEAR(-4., edge_limits.min().x(), 1e-9);
EXPECT_NEAR(0., edge_limits.min().y(), 1e-9);
EXPECT_NEAR(1., edge_limits.max().x(), 1e-9);
EXPECT_NEAR(5., edge_limits.max().y(), 1e-9);
EXPECT_NEAR(1., probability_grid_.limits().max().x(), 1e-9);
EXPECT_NEAR(5., probability_grid_.limits().max().y(), 1e-9);
const CellLimits& cell_limits = probability_grid_.limits().cell_limits();
EXPECT_EQ(5, cell_limits.num_x_cells);

75
cartographer/mapping_2d/map_limits.h
View File

@ -38,29 +38,21 @@ namespace mapping_2d {
// performance reasons.
class MapLimits {
public:
MapLimits(const double resolution, const Eigen::AlignedBox2d& edge_limits)
: resolution_(resolution) {
SetLimits(edge_limits);
}
MapLimits(const double resolution, const double max_x, const double max_y,
MapLimits(const double resolution, const Eigen::Vector2d& max,
const CellLimits& cell_limits)
: resolution_(resolution) {
SetLimits(max_x, max_y, cell_limits);
: resolution_(resolution), max_(max), cell_limits_(cell_limits) {
CHECK_GT(resolution_, 0.);
CHECK_GT(cell_limits.num_x_cells, 0.);
CHECK_GT(cell_limits.num_y_cells, 0.);
}
// Returns the cell size in meters. All cells are square and the resolution is
// the length of one side.
double resolution() const { return resolution_; }
// Returns the limits of the grid from edge to edge in meters.
const Eigen::AlignedBox2d& edge_limits() const { return edge_limits_; }
// Returns the limits of the grid between the centers of the edge cells in
// meters.
const Eigen::AlignedBox2d& centered_limits() const {
return centered_limits_;
}
// Returns the corner of the limits, i.e., all pixels have positions with
// smaller coordinates.
const Eigen::Vector2d& max() const { return max_; }
// Returns the limits of the grid in number of cells.
const CellLimits& cell_limits() const { return cell_limits_; }
@ -74,8 +66,8 @@ class MapLimits {
// and contains (centered_max_x, centered_max_y). We need to flip and
// rotate.
return Eigen::Array2i(
common::RoundToInt((centered_limits_.max().y() - y) / resolution_),
common::RoundToInt((centered_limits_.max().x() - x) / resolution_));
common::RoundToInt((max_.y() - y) / resolution_ - 0.5),
common::RoundToInt((max_.x() - x) / resolution_ - 0.5));
}
// Returns true of the ProbabilityGrid contains 'xy_index'.
@ -97,7 +89,9 @@ class MapLimits {
const float kPadding = 3.f * resolution;
bounding_box.min() -= kPadding * Eigen::Vector2f::Ones();
bounding_box.max() += kPadding * Eigen::Vector2f::Ones();
return MapLimits(resolution, bounding_box.cast<double>());
return MapLimits(resolution, bounding_box.max().cast<double>(),
CellLimits(common::RoundToInt(bounding_box.sizes().y()),
common::RoundToInt(bounding_box.sizes().x())));
}
static Eigen::AlignedBox2f ComputeMapBoundingBox(
@ -128,49 +122,8 @@ class MapLimits {
}
private:
// Returns the center of the resolution interval containing 'x'.
double Center(const double x) {
return (std::floor(x / resolution_) + 0.5) * resolution_;
}
// Sets the limits of the grid to the specified bounding box in meters.
void SetLimits(const Eigen::AlignedBox2d& limits) {
CHECK(!limits.isEmpty());
const int num_x_cells = common::RoundToInt((Center(limits.max().y()) -
Center(limits.min().y())) /
resolution_) +
1;
const int num_y_cells = common::RoundToInt((Center(limits.max().x()) -
Center(limits.min().x())) /
resolution_) +
1;
SetLimits(limits.max().x(), limits.max().y(),
CellLimits(num_x_cells, num_y_cells));
}
void SetLimits(double max_x, double max_y, const CellLimits& limits) {
CHECK_GT(resolution_, 0.);
CHECK_GT(limits.num_x_cells, 0.);
CHECK_GT(limits.num_y_cells, 0.);
cell_limits_ = limits;
centered_limits_.max().x() = Center(max_x);
centered_limits_.max().y() = Center(max_y);
centered_limits_.min().x() = centered_limits_.max().x() -
resolution_ * (cell_limits_.num_y_cells - 1);
centered_limits_.min().y() = centered_limits_.max().y() -
resolution_ * (cell_limits_.num_x_cells - 1);
const double half_resolution = resolution_ / 2.;
edge_limits_.min().x() = centered_limits_.min().x() - half_resolution;
edge_limits_.min().y() = centered_limits_.min().y() - half_resolution;
edge_limits_.max().x() = centered_limits_.max().x() + half_resolution;
edge_limits_.max().y() = centered_limits_.max().y() + half_resolution;
}
double resolution_;
Eigen::AlignedBox2d edge_limits_;
Eigen::AlignedBox2d centered_limits_;
Eigen::Vector2d max_;
CellLimits cell_limits_;
};

37
cartographer/mapping_2d/map_limits_test.cc
View File

@ -22,43 +22,18 @@ namespace cartographer {
namespace mapping_2d {
namespace {
TEST(MapLimits, SetEdgeLimits) {
const MapLimits limits(2., Eigen::AlignedBox2d(Eigen::Vector2d(-0.5, 1.5),
Eigen::Vector2d(0.5, 5.5)));
const Eigen::AlignedBox2d& edge_limits = limits.edge_limits();
EXPECT_EQ(-2., edge_limits.min().x());
EXPECT_EQ(0., edge_limits.min().y());
EXPECT_EQ(2., edge_limits.max().x());
EXPECT_EQ(6., edge_limits.max().y());
const CellLimits& cell_limits = limits.cell_limits();
EXPECT_EQ(3, cell_limits.num_x_cells);
EXPECT_EQ(2, cell_limits.num_y_cells);
}
TEST(MapLimits, SetCellLimits) {
const MapLimits limits(3., 1.5, -0.5, CellLimits(2, 3));
TEST(MapLimits, ConstructAndGet) {
const MapLimits limits(42., Eigen::Vector2d(3., 0.), CellLimits(2, 3));
const CellLimits& cell_limits = limits.cell_limits();
EXPECT_EQ(2, cell_limits.num_x_cells);
EXPECT_EQ(3, cell_limits.num_y_cells);
const Eigen::AlignedBox2d& edge_limits = limits.edge_limits();
EXPECT_EQ(-6., edge_limits.min().x());
EXPECT_EQ(-6., edge_limits.min().y());
EXPECT_EQ(3., edge_limits.max().x());
EXPECT_EQ(0., edge_limits.max().y());
}
const Eigen::Vector2d& max = limits.max();
EXPECT_EQ(3., max.x());
EXPECT_EQ(0., max.y());
TEST(MapLimits, GetCenteredLimits) {
const MapLimits limits(2., -0.5, 1.5, CellLimits(3, 2));
const Eigen::AlignedBox2d& centered_limits = limits.centered_limits();
EXPECT_EQ(-3, centered_limits.min().x());
EXPECT_EQ(-3., centered_limits.min().y());
EXPECT_EQ(-1., centered_limits.max().x());
EXPECT_EQ(1., centered_limits.max().y());
EXPECT_EQ(42., limits.resolution());
}
} // namespace

4
cartographer/mapping_2d/probability_grid.h
View File

@ -130,8 +130,8 @@ class ProbabilityGrid {
const int y_offset = limits_.cell_limits().num_y_cells / 2;
const MapLimits new_limits(
limits_.resolution(),
limits_.centered_limits().max().x() + y_offset * limits_.resolution(),
limits_.centered_limits().max().y() + x_offset * limits_.resolution(),
limits_.max() +
limits_.resolution() * Eigen::Vector2d(y_offset, x_offset),
CellLimits(2 * limits_.cell_limits().num_x_cells,
2 * limits_.cell_limits().num_y_cells));
const int stride = new_limits.cell_limits().num_x_cells;

18
cartographer/mapping_2d/probability_grid_test.cc
View File

@ -25,7 +25,8 @@ namespace mapping_2d {
namespace {
TEST(ProbabilityGridTest, ApplyOdds) {
ProbabilityGrid probability_grid(MapLimits(1., 0.5, 0.5, CellLimits(2, 2)));
ProbabilityGrid probability_grid(
MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)));
const MapLimits& limits = probability_grid.limits();
EXPECT_TRUE(limits.Contains(Eigen::Array2i(0, 0)));
@ -77,15 +78,11 @@ TEST(ProbabilityGridTest, ApplyOdds) {
TEST(ProbabilityGridTest, GetProbability) {
ProbabilityGrid probability_grid(
MapLimits(1., Eigen::AlignedBox2d(Eigen::Vector2d(-0.5, 0.5),
Eigen::Vector2d(0.5, 1.5))));
MapLimits(1., Eigen::Vector2d(1., 2.), CellLimits(2, 2)));
const MapLimits& limits = probability_grid.limits();
const Eigen::AlignedBox2d& edge_limits = limits.edge_limits();
EXPECT_EQ(-1., edge_limits.min().x());
EXPECT_EQ(0., edge_limits.min().y());
EXPECT_EQ(1., edge_limits.max().x());
EXPECT_EQ(2., edge_limits.max().y());
EXPECT_EQ(1., limits.max().x());
EXPECT_EQ(2., limits.max().y());
const CellLimits& cell_limits = limits.cell_limits();
ASSERT_EQ(2, cell_limits.num_x_cells);
@ -107,7 +104,8 @@ TEST(ProbabilityGridTest, GetProbability) {
}
TEST(ProbabilityGridTest, GetXYIndexOfCellContainingPoint) {
ProbabilityGrid probability_grid(MapLimits(2., 7, 13, CellLimits(14, 8)));
ProbabilityGrid probability_grid(
MapLimits(2., Eigen::Vector2d(8., 14.), CellLimits(14, 8)));
const MapLimits& limits = probability_grid.limits();
const CellLimits& cell_limits = limits.cell_limits();
@ -150,7 +148,7 @@ TEST(ProbabilityGridTest, CorrectCropping) {
std::uniform_real_distribution<float> value_distribution(
mapping::kMinProbability, mapping::kMaxProbability);
ProbabilityGrid probability_grid(
MapLimits(0.05, 10., 10., CellLimits(400, 400)));
MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400)));
probability_grid.StartUpdate();
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(
Eigen::Array2i(100, 100), Eigen::Array2i(299, 299))) {

6
cartographer/mapping_2d/ray_casting.cc
View File

@ -151,12 +151,8 @@ void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
const std::function<void(const Eigen::Array2i&)>& hit_visitor,
const std::function<void(const Eigen::Array2i&)>& miss_visitor) {
const double superscaled_resolution = limits.resolution() / kSubpixelScale;
const Eigen::Vector2d superscaled_centered_max =
limits.edge_limits().max() -
superscaled_resolution / 2. * Eigen::Vector2d::Ones();
const MapLimits superscaled_limits(
superscaled_resolution, superscaled_centered_max.x(),
superscaled_centered_max.y(),
superscaled_resolution, limits.max(),
CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
limits.cell_limits().num_y_cells * kSubpixelScale));
const Eigen::Array2i begin =

3
cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc
View File

@ -35,8 +35,7 @@ class CeresScanMatcherTest : public ::testing::Test {
protected:
CeresScanMatcherTest()
: probability_grid_(
MapLimits(1., Eigen::AlignedBox2d(Eigen::Vector2d(-10., -10.),
Eigen::Vector2d(10., 10.)))) {
MapLimits(1., Eigen::Vector2d(10., 10.), CellLimits(20, 20))) {
probability_grid_.SetProbability(
probability_grid_.limits().GetXYIndexOfCellContainingPoint(-3.5, 2.5),
mapping::kMaxProbability);

5
cartographer/mapping_2d/scan_matching/correlative_scan_matcher_test.cc
View File

@ -79,9 +79,8 @@ TEST(DiscretizeScans, DiscretizeScans) {
point_cloud.emplace_back(-0.125, 0.125);
point_cloud.emplace_back(-0.125, 0.075);
point_cloud.emplace_back(-0.125, 0.025);
const MapLimits map_limits(
0.05, Eigen::AlignedBox2d(Eigen::Vector2d(-0.225, -0.025),
Eigen::Vector2d(0.025, 0.225)));
const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25),
CellLimits(6, 6));
const std::vector<sensor::PointCloud2D> scans =
GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.));
const std::vector<DiscreteScan> discrete_scans =

7
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc
View File

@ -232,8 +232,11 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
1e6 * limits_.resolution(), // Linear search window, 1e6 cells/direction.
M_PI, // Angular search window, 180 degrees in both directions.
point_cloud, limits_.resolution());
const transform::Rigid2d center =
transform::Rigid2d::Translation(limits_.edge_limits().center());
const transform::Rigid2d center = transform::Rigid2d::Translation(
limits_.max() -
0.5 * limits_.resolution() *
Eigen::Vector2d(limits_.cell_limits().num_y_cells,
limits_.cell_limits().num_x_cells));
return MatchWithSearchParameters(search_parameters, center, point_cloud,
min_score, score, pose_estimate, covariance);
}

9
cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc
View File

@ -40,7 +40,7 @@ TEST(PrecomputationGridTest, CorrectValues) {
std::mt19937 prng(42);
std::uniform_int_distribution<int> distribution(0, 255);
ProbabilityGrid probability_grid(
MapLimits(0.05, 5., 5., CellLimits(250, 250)));
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(250, 250)));
probability_grid.StartUpdate();
for (const Eigen::Array2i& xy_index :
XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) {
@ -73,7 +73,8 @@ TEST(PrecomputationGridTest, CorrectValues) {
TEST(PrecomputationGridTest, TinyProbabilityGrid) {
std::mt19937 prng(42);
std::uniform_int_distribution<int> distribution(0, 255);
ProbabilityGrid probability_grid(MapLimits(0.05, 0.1, 0.1, CellLimits(4, 4)));
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4)));
probability_grid.StartUpdate();
for (const Eigen::Array2i& xy_index :
XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
@ -147,7 +148,7 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
0.5 * distribution(prng));
ProbabilityGrid probability_grid(
MapLimits(0.05, 5., 5., CellLimits(200, 200)));
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
probability_grid.StartUpdate();
laser_fan_inserter.Insert(sensor::LaserFan{expected_pose.translation(),
sensor::TransformPointCloud2D(
@ -198,7 +199,7 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
perturbation.inverse();
ProbabilityGrid probability_grid(
MapLimits(0.05, 5., 5., CellLimits(200, 200)));
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
probability_grid.StartUpdate();
laser_fan_inserter.Insert(
sensor::LaserFan{

13
cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h
View File

@ -62,13 +62,12 @@ class OccupiedSpaceCostFunctor {
const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].x())),
(T(point_cloud_[i].y())), T(1.));
const Eigen::Matrix<T, 3, 1> world = transform * point;
interpolator.Evaluate((limits.centered_limits().max().x() - world[0]) /
limits.resolution() +
T(kPadding),
(limits.centered_limits().max().y() - world[1]) /
limits.resolution() +
T(kPadding),
&residual[i]);
interpolator.Evaluate(
(limits.max().x() - world[0]) / limits.resolution() - 0.5 +
T(kPadding),
(limits.max().y() - world[1]) / limits.resolution() - 0.5 +
T(kPadding),
&residual[i]);
residual[i] = scaling_factor_ * (1. - residual[i]);
}
return true;

5
cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc
View File

@ -37,9 +37,8 @@ namespace {
class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
protected:
RealTimeCorrelativeScanMatcherTest()
: probability_grid_(MapLimits(
0.05, Eigen::AlignedBox2d(Eigen::Vector2d(-0.225, -0.025),
Eigen::Vector2d(0.025, 0.225)))) {
: probability_grid_(
MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6))) {
{
auto parameter_dictionary = common::MakeDictionary(
"return { "

21
cartographer/mapping_2d/submaps.cc
View File

@ -24,6 +24,7 @@
#include "Eigen/Geometry"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/port.h"
#include "glog/logging.h"
#include "webp/encode.h"
@ -70,11 +71,10 @@ ProbabilityGrid ComputeCroppedProbabilityGrid(
CellLimits limits;
probability_grid.ComputeCroppedLimits(&offset, &limits);
const double resolution = probability_grid.limits().resolution();
const double max_x = probability_grid.limits().centered_limits().max().x() -
resolution * offset.y();
const double max_y = probability_grid.limits().centered_limits().max().y() -
resolution * offset.x();
ProbabilityGrid cropped_grid(MapLimits(resolution, max_x, max_y, limits));
const Eigen::Vector2d max =
probability_grid.limits().max() -
resolution * Eigen::Vector2d(offset.y(), offset.x());
ProbabilityGrid cropped_grid(MapLimits(resolution, max, limits));
cropped_grid.StartUpdate();
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) {
if (probability_grid.IsKnown(xy_index + offset)) {
@ -164,13 +164,14 @@ void Submaps::AddSubmap(const Eigen::Vector2f& origin) {
if (size() > 1) {
FinishSubmap(size() - 2);
}
const int num_cells_per_dimension =
common::RoundToInt(2. * options_.half_length() / options_.resolution()) +
1;
submaps_.push_back(common::make_unique<Submap>(
MapLimits(options_.resolution(),
Eigen::AlignedBox2d(
origin.cast<double>() -
options_.half_length() * Eigen::Vector2d::Ones(),
origin.cast<double>() +
options_.half_length() * Eigen::Vector2d::Ones())),
origin.cast<double>() +
options_.half_length() * Eigen::Vector2d::Ones(),
CellLimits(num_cells_per_dimension, num_cells_per_dimension)),
origin, num_laser_fans_));
LOG(INFO) << "Added submap " << size();
num_laser_fans_in_last_submap_ = 0;