parent
9134a8967d
commit
817dc884d5
|
@ -80,10 +80,10 @@ void Submaps::AddProbabilityGridToResponse(
|
||||||
response->set_height(limits.num_y_cells);
|
response->set_height(limits.num_y_cells);
|
||||||
const double resolution = probability_grid.limits().resolution();
|
const double resolution = probability_grid.limits().resolution();
|
||||||
response->set_resolution(resolution);
|
response->set_resolution(resolution);
|
||||||
const double max_x = probability_grid.limits().edge_limits().max().x() -
|
const double max_x =
|
||||||
resolution * offset.y();
|
probability_grid.limits().max().x() - resolution * offset.y();
|
||||||
const double max_y = probability_grid.limits().edge_limits().max().y() -
|
const double max_y =
|
||||||
resolution * offset.x();
|
probability_grid.limits().max().y() - resolution * offset.x();
|
||||||
*response->mutable_slice_pose() = transform::ToProto(
|
*response->mutable_slice_pose() = transform::ToProto(
|
||||||
local_submap_pose.inverse() *
|
local_submap_pose.inverse() *
|
||||||
transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
|
transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
|
||||||
|
|
|
@ -144,6 +144,7 @@ google_library(mapping_2d_submaps
|
||||||
DEPENDS
|
DEPENDS
|
||||||
common_lua_parameter_dictionary
|
common_lua_parameter_dictionary
|
||||||
common_make_unique
|
common_make_unique
|
||||||
|
common_port
|
||||||
mapping_2d_laser_fan_inserter
|
mapping_2d_laser_fan_inserter
|
||||||
mapping_2d_map_limits
|
mapping_2d_map_limits
|
||||||
mapping_2d_probability_grid
|
mapping_2d_probability_grid
|
||||||
|
|
|
@ -32,12 +32,7 @@ class LaserFanInserterTest : public ::testing::Test {
|
||||||
protected:
|
protected:
|
||||||
LaserFanInserterTest()
|
LaserFanInserterTest()
|
||||||
: probability_grid_(
|
: probability_grid_(
|
||||||
MapLimits(1., Eigen::AlignedBox2d(Eigen::Vector2d(-4., 0.),
|
MapLimits(1., Eigen::Vector2d(1., 5.), CellLimits(5, 5))) {
|
||||||
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);
|
|
||||||
|
|
||||||
auto parameter_dictionary = common::MakeDictionary(
|
auto parameter_dictionary = common::MakeDictionary(
|
||||||
"return { "
|
"return { "
|
||||||
"insert_free_space = true, "
|
"insert_free_space = true, "
|
||||||
|
@ -68,12 +63,8 @@ class LaserFanInserterTest : public ::testing::Test {
|
||||||
TEST_F(LaserFanInserterTest, InsertPointCloud) {
|
TEST_F(LaserFanInserterTest, InsertPointCloud) {
|
||||||
InsertPointCloud();
|
InsertPointCloud();
|
||||||
|
|
||||||
const Eigen::AlignedBox2d& edge_limits =
|
EXPECT_NEAR(1., probability_grid_.limits().max().x(), 1e-9);
|
||||||
probability_grid_.limits().edge_limits();
|
EXPECT_NEAR(5., probability_grid_.limits().max().y(), 1e-9);
|
||||||
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);
|
|
||||||
|
|
||||||
const CellLimits& cell_limits = probability_grid_.limits().cell_limits();
|
const CellLimits& cell_limits = probability_grid_.limits().cell_limits();
|
||||||
EXPECT_EQ(5, cell_limits.num_x_cells);
|
EXPECT_EQ(5, cell_limits.num_x_cells);
|
||||||
|
|
|
@ -38,29 +38,21 @@ namespace mapping_2d {
|
||||||
// performance reasons.
|
// performance reasons.
|
||||||
class MapLimits {
|
class MapLimits {
|
||||||
public:
|
public:
|
||||||
MapLimits(const double resolution, const Eigen::AlignedBox2d& edge_limits)
|
MapLimits(const double resolution, const Eigen::Vector2d& max,
|
||||||
: resolution_(resolution) {
|
|
||||||
SetLimits(edge_limits);
|
|
||||||
}
|
|
||||||
|
|
||||||
MapLimits(const double resolution, const double max_x, const double max_y,
|
|
||||||
const CellLimits& cell_limits)
|
const CellLimits& cell_limits)
|
||||||
: resolution_(resolution) {
|
: resolution_(resolution), max_(max), cell_limits_(cell_limits) {
|
||||||
SetLimits(max_x, max_y, 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
|
// Returns the cell size in meters. All cells are square and the resolution is
|
||||||
// the length of one side.
|
// the length of one side.
|
||||||
double resolution() const { return resolution_; }
|
double resolution() const { return resolution_; }
|
||||||
|
|
||||||
// Returns the limits of the grid from edge to edge in meters.
|
// Returns the corner of the limits, i.e., all pixels have positions with
|
||||||
const Eigen::AlignedBox2d& edge_limits() const { return edge_limits_; }
|
// smaller coordinates.
|
||||||
|
const Eigen::Vector2d& max() const { return max_; }
|
||||||
// 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 limits of the grid in number of cells.
|
// Returns the limits of the grid in number of cells.
|
||||||
const CellLimits& cell_limits() const { return cell_limits_; }
|
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
|
// and contains (centered_max_x, centered_max_y). We need to flip and
|
||||||
// rotate.
|
// rotate.
|
||||||
return Eigen::Array2i(
|
return Eigen::Array2i(
|
||||||
common::RoundToInt((centered_limits_.max().y() - y) / resolution_),
|
common::RoundToInt((max_.y() - y) / resolution_ - 0.5),
|
||||||
common::RoundToInt((centered_limits_.max().x() - x) / resolution_));
|
common::RoundToInt((max_.x() - x) / resolution_ - 0.5));
|
||||||
}
|
}
|
||||||
|
|
||||||
// Returns true of the ProbabilityGrid contains 'xy_index'.
|
// Returns true of the ProbabilityGrid contains 'xy_index'.
|
||||||
|
@ -97,7 +89,9 @@ class MapLimits {
|
||||||
const float kPadding = 3.f * resolution;
|
const float kPadding = 3.f * resolution;
|
||||||
bounding_box.min() -= kPadding * Eigen::Vector2f::Ones();
|
bounding_box.min() -= kPadding * Eigen::Vector2f::Ones();
|
||||||
bounding_box.max() += 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(
|
static Eigen::AlignedBox2f ComputeMapBoundingBox(
|
||||||
|
@ -128,49 +122,8 @@ class MapLimits {
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
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_;
|
double resolution_;
|
||||||
Eigen::AlignedBox2d edge_limits_;
|
Eigen::Vector2d max_;
|
||||||
Eigen::AlignedBox2d centered_limits_;
|
|
||||||
CellLimits cell_limits_;
|
CellLimits cell_limits_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -22,43 +22,18 @@ namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping_2d {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
TEST(MapLimits, SetEdgeLimits) {
|
TEST(MapLimits, ConstructAndGet) {
|
||||||
const MapLimits limits(2., Eigen::AlignedBox2d(Eigen::Vector2d(-0.5, 1.5),
|
const MapLimits limits(42., Eigen::Vector2d(3., 0.), CellLimits(2, 3));
|
||||||
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));
|
|
||||||
|
|
||||||
const CellLimits& cell_limits = limits.cell_limits();
|
const CellLimits& cell_limits = limits.cell_limits();
|
||||||
EXPECT_EQ(2, cell_limits.num_x_cells);
|
EXPECT_EQ(2, cell_limits.num_x_cells);
|
||||||
EXPECT_EQ(3, cell_limits.num_y_cells);
|
EXPECT_EQ(3, cell_limits.num_y_cells);
|
||||||
|
|
||||||
const Eigen::AlignedBox2d& edge_limits = limits.edge_limits();
|
const Eigen::Vector2d& max = limits.max();
|
||||||
EXPECT_EQ(-6., edge_limits.min().x());
|
EXPECT_EQ(3., max.x());
|
||||||
EXPECT_EQ(-6., edge_limits.min().y());
|
EXPECT_EQ(0., max.y());
|
||||||
EXPECT_EQ(3., edge_limits.max().x());
|
|
||||||
EXPECT_EQ(0., edge_limits.max().y());
|
|
||||||
}
|
|
||||||
|
|
||||||
TEST(MapLimits, GetCenteredLimits) {
|
EXPECT_EQ(42., limits.resolution());
|
||||||
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());
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
|
@ -130,8 +130,8 @@ class ProbabilityGrid {
|
||||||
const int y_offset = limits_.cell_limits().num_y_cells / 2;
|
const int y_offset = limits_.cell_limits().num_y_cells / 2;
|
||||||
const MapLimits new_limits(
|
const MapLimits new_limits(
|
||||||
limits_.resolution(),
|
limits_.resolution(),
|
||||||
limits_.centered_limits().max().x() + y_offset * limits_.resolution(),
|
limits_.max() +
|
||||||
limits_.centered_limits().max().y() + x_offset * limits_.resolution(),
|
limits_.resolution() * Eigen::Vector2d(y_offset, x_offset),
|
||||||
CellLimits(2 * limits_.cell_limits().num_x_cells,
|
CellLimits(2 * limits_.cell_limits().num_x_cells,
|
||||||
2 * limits_.cell_limits().num_y_cells));
|
2 * limits_.cell_limits().num_y_cells));
|
||||||
const int stride = new_limits.cell_limits().num_x_cells;
|
const int stride = new_limits.cell_limits().num_x_cells;
|
||||||
|
|
|
@ -25,7 +25,8 @@ namespace mapping_2d {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
TEST(ProbabilityGridTest, ApplyOdds) {
|
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();
|
const MapLimits& limits = probability_grid.limits();
|
||||||
|
|
||||||
EXPECT_TRUE(limits.Contains(Eigen::Array2i(0, 0)));
|
EXPECT_TRUE(limits.Contains(Eigen::Array2i(0, 0)));
|
||||||
|
@ -77,15 +78,11 @@ TEST(ProbabilityGridTest, ApplyOdds) {
|
||||||
|
|
||||||
TEST(ProbabilityGridTest, GetProbability) {
|
TEST(ProbabilityGridTest, GetProbability) {
|
||||||
ProbabilityGrid probability_grid(
|
ProbabilityGrid probability_grid(
|
||||||
MapLimits(1., Eigen::AlignedBox2d(Eigen::Vector2d(-0.5, 0.5),
|
MapLimits(1., Eigen::Vector2d(1., 2.), CellLimits(2, 2)));
|
||||||
Eigen::Vector2d(0.5, 1.5))));
|
|
||||||
|
|
||||||
const MapLimits& limits = probability_grid.limits();
|
const MapLimits& limits = probability_grid.limits();
|
||||||
const Eigen::AlignedBox2d& edge_limits = limits.edge_limits();
|
EXPECT_EQ(1., limits.max().x());
|
||||||
EXPECT_EQ(-1., edge_limits.min().x());
|
EXPECT_EQ(2., limits.max().y());
|
||||||
EXPECT_EQ(0., edge_limits.min().y());
|
|
||||||
EXPECT_EQ(1., edge_limits.max().x());
|
|
||||||
EXPECT_EQ(2., edge_limits.max().y());
|
|
||||||
|
|
||||||
const CellLimits& cell_limits = limits.cell_limits();
|
const CellLimits& cell_limits = limits.cell_limits();
|
||||||
ASSERT_EQ(2, cell_limits.num_x_cells);
|
ASSERT_EQ(2, cell_limits.num_x_cells);
|
||||||
|
@ -107,7 +104,8 @@ TEST(ProbabilityGridTest, GetProbability) {
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(ProbabilityGridTest, GetXYIndexOfCellContainingPoint) {
|
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 MapLimits& limits = probability_grid.limits();
|
||||||
const CellLimits& cell_limits = limits.cell_limits();
|
const CellLimits& cell_limits = limits.cell_limits();
|
||||||
|
@ -150,7 +148,7 @@ TEST(ProbabilityGridTest, CorrectCropping) {
|
||||||
std::uniform_real_distribution<float> value_distribution(
|
std::uniform_real_distribution<float> value_distribution(
|
||||||
mapping::kMinProbability, mapping::kMaxProbability);
|
mapping::kMinProbability, mapping::kMaxProbability);
|
||||||
ProbabilityGrid probability_grid(
|
ProbabilityGrid probability_grid(
|
||||||
MapLimits(0.05, 10., 10., CellLimits(400, 400)));
|
MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400)));
|
||||||
probability_grid.StartUpdate();
|
probability_grid.StartUpdate();
|
||||||
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(
|
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(
|
||||||
Eigen::Array2i(100, 100), Eigen::Array2i(299, 299))) {
|
Eigen::Array2i(100, 100), Eigen::Array2i(299, 299))) {
|
||||||
|
|
|
@ -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&)>& hit_visitor,
|
||||||
const std::function<void(const Eigen::Array2i&)>& miss_visitor) {
|
const std::function<void(const Eigen::Array2i&)>& miss_visitor) {
|
||||||
const double superscaled_resolution = limits.resolution() / kSubpixelScale;
|
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(
|
const MapLimits superscaled_limits(
|
||||||
superscaled_resolution, superscaled_centered_max.x(),
|
superscaled_resolution, limits.max(),
|
||||||
superscaled_centered_max.y(),
|
|
||||||
CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
|
CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
|
||||||
limits.cell_limits().num_y_cells * kSubpixelScale));
|
limits.cell_limits().num_y_cells * kSubpixelScale));
|
||||||
const Eigen::Array2i begin =
|
const Eigen::Array2i begin =
|
||||||
|
|
|
@ -35,8 +35,7 @@ class CeresScanMatcherTest : public ::testing::Test {
|
||||||
protected:
|
protected:
|
||||||
CeresScanMatcherTest()
|
CeresScanMatcherTest()
|
||||||
: probability_grid_(
|
: probability_grid_(
|
||||||
MapLimits(1., Eigen::AlignedBox2d(Eigen::Vector2d(-10., -10.),
|
MapLimits(1., Eigen::Vector2d(10., 10.), CellLimits(20, 20))) {
|
||||||
Eigen::Vector2d(10., 10.)))) {
|
|
||||||
probability_grid_.SetProbability(
|
probability_grid_.SetProbability(
|
||||||
probability_grid_.limits().GetXYIndexOfCellContainingPoint(-3.5, 2.5),
|
probability_grid_.limits().GetXYIndexOfCellContainingPoint(-3.5, 2.5),
|
||||||
mapping::kMaxProbability);
|
mapping::kMaxProbability);
|
||||||
|
|
|
@ -79,9 +79,8 @@ TEST(DiscretizeScans, DiscretizeScans) {
|
||||||
point_cloud.emplace_back(-0.125, 0.125);
|
point_cloud.emplace_back(-0.125, 0.125);
|
||||||
point_cloud.emplace_back(-0.125, 0.075);
|
point_cloud.emplace_back(-0.125, 0.075);
|
||||||
point_cloud.emplace_back(-0.125, 0.025);
|
point_cloud.emplace_back(-0.125, 0.025);
|
||||||
const MapLimits map_limits(
|
const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25),
|
||||||
0.05, Eigen::AlignedBox2d(Eigen::Vector2d(-0.225, -0.025),
|
CellLimits(6, 6));
|
||||||
Eigen::Vector2d(0.025, 0.225)));
|
|
||||||
const std::vector<sensor::PointCloud2D> scans =
|
const std::vector<sensor::PointCloud2D> scans =
|
||||||
GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.));
|
GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.));
|
||||||
const std::vector<DiscreteScan> discrete_scans =
|
const std::vector<DiscreteScan> discrete_scans =
|
||||||
|
|
|
@ -232,8 +232,11 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
|
||||||
1e6 * limits_.resolution(), // Linear search window, 1e6 cells/direction.
|
1e6 * limits_.resolution(), // Linear search window, 1e6 cells/direction.
|
||||||
M_PI, // Angular search window, 180 degrees in both directions.
|
M_PI, // Angular search window, 180 degrees in both directions.
|
||||||
point_cloud, limits_.resolution());
|
point_cloud, limits_.resolution());
|
||||||
const transform::Rigid2d center =
|
const transform::Rigid2d center = transform::Rigid2d::Translation(
|
||||||
transform::Rigid2d::Translation(limits_.edge_limits().center());
|
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,
|
return MatchWithSearchParameters(search_parameters, center, point_cloud,
|
||||||
min_score, score, pose_estimate, covariance);
|
min_score, score, pose_estimate, covariance);
|
||||||
}
|
}
|
||||||
|
|
|
@ -40,7 +40,7 @@ TEST(PrecomputationGridTest, CorrectValues) {
|
||||||
std::mt19937 prng(42);
|
std::mt19937 prng(42);
|
||||||
std::uniform_int_distribution<int> distribution(0, 255);
|
std::uniform_int_distribution<int> distribution(0, 255);
|
||||||
ProbabilityGrid probability_grid(
|
ProbabilityGrid probability_grid(
|
||||||
MapLimits(0.05, 5., 5., CellLimits(250, 250)));
|
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(250, 250)));
|
||||||
probability_grid.StartUpdate();
|
probability_grid.StartUpdate();
|
||||||
for (const Eigen::Array2i& xy_index :
|
for (const Eigen::Array2i& xy_index :
|
||||||
XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) {
|
XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) {
|
||||||
|
@ -73,7 +73,8 @@ TEST(PrecomputationGridTest, CorrectValues) {
|
||||||
TEST(PrecomputationGridTest, TinyProbabilityGrid) {
|
TEST(PrecomputationGridTest, TinyProbabilityGrid) {
|
||||||
std::mt19937 prng(42);
|
std::mt19937 prng(42);
|
||||||
std::uniform_int_distribution<int> distribution(0, 255);
|
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();
|
probability_grid.StartUpdate();
|
||||||
for (const Eigen::Array2i& xy_index :
|
for (const Eigen::Array2i& xy_index :
|
||||||
XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
|
XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
|
||||||
|
@ -147,7 +148,7 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
|
||||||
0.5 * distribution(prng));
|
0.5 * distribution(prng));
|
||||||
|
|
||||||
ProbabilityGrid probability_grid(
|
ProbabilityGrid probability_grid(
|
||||||
MapLimits(0.05, 5., 5., CellLimits(200, 200)));
|
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
|
||||||
probability_grid.StartUpdate();
|
probability_grid.StartUpdate();
|
||||||
laser_fan_inserter.Insert(sensor::LaserFan{expected_pose.translation(),
|
laser_fan_inserter.Insert(sensor::LaserFan{expected_pose.translation(),
|
||||||
sensor::TransformPointCloud2D(
|
sensor::TransformPointCloud2D(
|
||||||
|
@ -198,7 +199,7 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
|
||||||
perturbation.inverse();
|
perturbation.inverse();
|
||||||
|
|
||||||
ProbabilityGrid probability_grid(
|
ProbabilityGrid probability_grid(
|
||||||
MapLimits(0.05, 5., 5., CellLimits(200, 200)));
|
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
|
||||||
probability_grid.StartUpdate();
|
probability_grid.StartUpdate();
|
||||||
laser_fan_inserter.Insert(
|
laser_fan_inserter.Insert(
|
||||||
sensor::LaserFan{
|
sensor::LaserFan{
|
||||||
|
|
|
@ -62,11 +62,10 @@ class OccupiedSpaceCostFunctor {
|
||||||
const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].x())),
|
const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].x())),
|
||||||
(T(point_cloud_[i].y())), T(1.));
|
(T(point_cloud_[i].y())), T(1.));
|
||||||
const Eigen::Matrix<T, 3, 1> world = transform * point;
|
const Eigen::Matrix<T, 3, 1> world = transform * point;
|
||||||
interpolator.Evaluate((limits.centered_limits().max().x() - world[0]) /
|
interpolator.Evaluate(
|
||||||
limits.resolution() +
|
(limits.max().x() - world[0]) / limits.resolution() - 0.5 +
|
||||||
T(kPadding),
|
T(kPadding),
|
||||||
(limits.centered_limits().max().y() - world[1]) /
|
(limits.max().y() - world[1]) / limits.resolution() - 0.5 +
|
||||||
limits.resolution() +
|
|
||||||
T(kPadding),
|
T(kPadding),
|
||||||
&residual[i]);
|
&residual[i]);
|
||||||
residual[i] = scaling_factor_ * (1. - residual[i]);
|
residual[i] = scaling_factor_ * (1. - residual[i]);
|
||||||
|
|
|
@ -37,9 +37,8 @@ namespace {
|
||||||
class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
|
class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
|
||||||
protected:
|
protected:
|
||||||
RealTimeCorrelativeScanMatcherTest()
|
RealTimeCorrelativeScanMatcherTest()
|
||||||
: probability_grid_(MapLimits(
|
: probability_grid_(
|
||||||
0.05, Eigen::AlignedBox2d(Eigen::Vector2d(-0.225, -0.025),
|
MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6))) {
|
||||||
Eigen::Vector2d(0.025, 0.225)))) {
|
|
||||||
{
|
{
|
||||||
auto parameter_dictionary = common::MakeDictionary(
|
auto parameter_dictionary = common::MakeDictionary(
|
||||||
"return { "
|
"return { "
|
||||||
|
|
|
@ -24,6 +24,7 @@
|
||||||
|
|
||||||
#include "Eigen/Geometry"
|
#include "Eigen/Geometry"
|
||||||
#include "cartographer/common/make_unique.h"
|
#include "cartographer/common/make_unique.h"
|
||||||
|
#include "cartographer/common/port.h"
|
||||||
#include "glog/logging.h"
|
#include "glog/logging.h"
|
||||||
#include "webp/encode.h"
|
#include "webp/encode.h"
|
||||||
|
|
||||||
|
@ -70,11 +71,10 @@ ProbabilityGrid ComputeCroppedProbabilityGrid(
|
||||||
CellLimits limits;
|
CellLimits limits;
|
||||||
probability_grid.ComputeCroppedLimits(&offset, &limits);
|
probability_grid.ComputeCroppedLimits(&offset, &limits);
|
||||||
const double resolution = probability_grid.limits().resolution();
|
const double resolution = probability_grid.limits().resolution();
|
||||||
const double max_x = probability_grid.limits().centered_limits().max().x() -
|
const Eigen::Vector2d max =
|
||||||
resolution * offset.y();
|
probability_grid.limits().max() -
|
||||||
const double max_y = probability_grid.limits().centered_limits().max().y() -
|
resolution * Eigen::Vector2d(offset.y(), offset.x());
|
||||||
resolution * offset.x();
|
ProbabilityGrid cropped_grid(MapLimits(resolution, max, limits));
|
||||||
ProbabilityGrid cropped_grid(MapLimits(resolution, max_x, max_y, limits));
|
|
||||||
cropped_grid.StartUpdate();
|
cropped_grid.StartUpdate();
|
||||||
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) {
|
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) {
|
||||||
if (probability_grid.IsKnown(xy_index + offset)) {
|
if (probability_grid.IsKnown(xy_index + offset)) {
|
||||||
|
@ -164,13 +164,14 @@ void Submaps::AddSubmap(const Eigen::Vector2f& origin) {
|
||||||
if (size() > 1) {
|
if (size() > 1) {
|
||||||
FinishSubmap(size() - 2);
|
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>(
|
submaps_.push_back(common::make_unique<Submap>(
|
||||||
MapLimits(options_.resolution(),
|
MapLimits(options_.resolution(),
|
||||||
Eigen::AlignedBox2d(
|
|
||||||
origin.cast<double>() -
|
|
||||||
options_.half_length() * Eigen::Vector2d::Ones(),
|
|
||||||
origin.cast<double>() +
|
origin.cast<double>() +
|
||||||
options_.half_length() * Eigen::Vector2d::Ones())),
|
options_.half_length() * Eigen::Vector2d::Ones(),
|
||||||
|
CellLimits(num_cells_per_dimension, num_cells_per_dimension)),
|
||||||
origin, num_laser_fans_));
|
origin, num_laser_fans_));
|
||||||
LOG(INFO) << "Added submap " << size();
|
LOG(INFO) << "Added submap " << size();
|
||||||
num_laser_fans_in_last_submap_ = 0;
|
num_laser_fans_in_last_submap_ = 0;
|
||||||
|
|
Loading…
Reference in New Issue