Separate raycast and grid update (#1216)
Separate raycast and grid update logic. Rename raycast to ray_to_pixel_mask.master
Kevin Daun
Wally B. Feed
parent
d2f3c1ea31
commit
cc9fc75757
|
|
@ -21,12 +21,79 @@
|
|||
#include "Eigen/Core"
|
||||
#include "Eigen/Geometry"
|
||||
#include "cartographer/mapping/2d/xy_index.h"
|
||||
#include "cartographer/mapping/internal/2d/ray_casting.h"
|
||||
#include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h"
|
||||
#include "cartographer/mapping/probability_values.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
namespace {
|
||||
|
||||
// Factor for subpixel accuracy of start and end point for ray casts.
|
||||
constexpr int kSubpixelScale = 1000;
|
||||
|
||||
void GrowAsNeeded(const sensor::RangeData& range_data,
|
||||
ProbabilityGrid* const probability_grid) {
|
||||
Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
|
||||
constexpr float kPadding = 1e-6f;
|
||||
for (const Eigen::Vector3f& hit : range_data.returns) {
|
||||
bounding_box.extend(hit.head<2>());
|
||||
}
|
||||
for (const Eigen::Vector3f& miss : range_data.misses) {
|
||||
bounding_box.extend(miss.head<2>());
|
||||
}
|
||||
probability_grid->GrowLimits(bounding_box.min() -
|
||||
kPadding * Eigen::Vector2f::Ones());
|
||||
probability_grid->GrowLimits(bounding_box.max() +
|
||||
kPadding * Eigen::Vector2f::Ones());
|
||||
}
|
||||
|
||||
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* probability_grid) {
|
||||
GrowAsNeeded(range_data, probability_grid);
|
||||
|
||||
const MapLimits& limits = probability_grid->limits();
|
||||
const double superscaled_resolution = limits.resolution() / kSubpixelScale;
|
||||
const MapLimits superscaled_limits(
|
||||
superscaled_resolution, limits.max(),
|
||||
CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
|
||||
limits.cell_limits().num_y_cells * kSubpixelScale));
|
||||
const Eigen::Array2i begin =
|
||||
superscaled_limits.GetCellIndex(range_data.origin.head<2>());
|
||||
// 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.GetCellIndex(hit.head<2>()));
|
||||
probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
|
||||
}
|
||||
|
||||
if (!insert_free_space) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Now add the misses.
|
||||
for (const Eigen::Array2i& end : ends) {
|
||||
std::vector<Eigen::Array2i> ray =
|
||||
RayToPixelMask(begin, end, kSubpixelScale);
|
||||
for (const Eigen::Array2i& cell_index : ray) {
|
||||
probability_grid->ApplyLookupTable(cell_index, miss_table);
|
||||
}
|
||||
}
|
||||
|
||||
// Finally, compute and add empty rays based on misses in the range data.
|
||||
for (const Eigen::Vector3f& missing_echo : range_data.misses) {
|
||||
std::vector<Eigen::Array2i> ray = RayToPixelMask(
|
||||
begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()),
|
||||
kSubpixelScale);
|
||||
for (const Eigen::Array2i& cell_index : ray) {
|
||||
probability_grid->ApplyLookupTable(cell_index, miss_table);
|
||||
}
|
||||
}
|
||||
}
|
||||
} // namespace
|
||||
|
||||
proto::ProbabilityGridRangeDataInserterOptions2D
|
||||
CreateProbabilityGridRangeDataInserterOptions2D(
|
||||
|
|
|
|||
|
|
@ -1,206 +0,0 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping/internal/2d/ray_casting.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
namespace {
|
||||
|
||||
// Factor for subpixel accuracy of start and end point.
|
||||
constexpr int kSubpixelScale = 1000;
|
||||
|
||||
// We divide each pixel in kSubpixelScale x kSubpixelScale subpixels. 'begin'
|
||||
// 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::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, miss_table, probability_grid);
|
||||
return;
|
||||
}
|
||||
|
||||
CHECK_GE(begin.x(), 0);
|
||||
CHECK_GE(begin.y(), 0);
|
||||
CHECK_GE(end.y(), 0);
|
||||
|
||||
// Special case: We have to draw a vertical line in full pixels, as 'begin'
|
||||
// and 'end' have the same full pixel x coordinate.
|
||||
if (begin.x() / kSubpixelScale == end.x() / kSubpixelScale) {
|
||||
Eigen::Array2i current(begin.x() / kSubpixelScale,
|
||||
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()) {
|
||||
probability_grid->ApplyLookupTable(current, miss_table);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
const int64 dx = end.x() - begin.x();
|
||||
const int64 dy = end.y() - begin.y();
|
||||
const int64 denominator = 2 * kSubpixelScale * dx;
|
||||
|
||||
// The current full pixel coordinates. We begin at 'begin'.
|
||||
Eigen::Array2i current = begin / kSubpixelScale;
|
||||
|
||||
// To represent subpixel centers, we use a factor of 2 * 'kSubpixelScale' in
|
||||
// the denominator.
|
||||
// +-+-+-+ -- 1 = (2 * kSubpixelScale) / (2 * kSubpixelScale)
|
||||
// | | | |
|
||||
// +-+-+-+
|
||||
// | | | |
|
||||
// +-+-+-+ -- top edge of first subpixel = 2 / (2 * kSubpixelScale)
|
||||
// | | | | -- center of first subpixel = 1 / (2 * kSubpixelScale)
|
||||
// +-+-+-+ -- 0 = 0 / (2 * kSubpixelScale)
|
||||
|
||||
// The center of the subpixel part of 'begin.y()' assuming the
|
||||
// 'denominator', i.e., sub_y / denominator is in (0, 1).
|
||||
int64 sub_y = (2 * (begin.y() % kSubpixelScale) + 1) * dx;
|
||||
|
||||
// The distance from the from 'begin' to the right pixel border, to be divided
|
||||
// by 2 * 'kSubpixelScale'.
|
||||
const int first_pixel =
|
||||
2 * kSubpixelScale - 2 * (begin.x() % kSubpixelScale) - 1;
|
||||
// The same from the left pixel border to 'end'.
|
||||
const int last_pixel = 2 * (end.x() % kSubpixelScale) + 1;
|
||||
|
||||
// The full pixel x coordinate of 'end'.
|
||||
const int end_x = std::max(begin.x(), end.x()) / kSubpixelScale;
|
||||
|
||||
// Move from 'begin' to the next pixel border to the right.
|
||||
sub_y += dy * first_pixel;
|
||||
if (dy > 0) {
|
||||
while (true) {
|
||||
probability_grid->ApplyLookupTable(current, miss_table);
|
||||
while (sub_y > denominator) {
|
||||
sub_y -= denominator;
|
||||
++current.y();
|
||||
probability_grid->ApplyLookupTable(current, miss_table);
|
||||
}
|
||||
++current.x();
|
||||
if (sub_y == denominator) {
|
||||
sub_y -= denominator;
|
||||
++current.y();
|
||||
}
|
||||
if (current.x() == end_x) {
|
||||
break;
|
||||
}
|
||||
// Move from one pixel border to the next.
|
||||
sub_y += dy * 2 * kSubpixelScale;
|
||||
}
|
||||
// Move from the pixel border on the right to 'end'.
|
||||
sub_y += dy * last_pixel;
|
||||
probability_grid->ApplyLookupTable(current, miss_table);
|
||||
while (sub_y > denominator) {
|
||||
sub_y -= denominator;
|
||||
++current.y();
|
||||
probability_grid->ApplyLookupTable(current, miss_table);
|
||||
}
|
||||
CHECK_NE(sub_y, denominator);
|
||||
CHECK_EQ(current.y(), end.y() / kSubpixelScale);
|
||||
return;
|
||||
}
|
||||
|
||||
// Same for lines non-ascending in y coordinates.
|
||||
while (true) {
|
||||
probability_grid->ApplyLookupTable(current, miss_table);
|
||||
while (sub_y < 0) {
|
||||
sub_y += denominator;
|
||||
--current.y();
|
||||
probability_grid->ApplyLookupTable(current, miss_table);
|
||||
}
|
||||
++current.x();
|
||||
if (sub_y == 0) {
|
||||
sub_y += denominator;
|
||||
--current.y();
|
||||
}
|
||||
if (current.x() == end_x) {
|
||||
break;
|
||||
}
|
||||
sub_y += dy * 2 * kSubpixelScale;
|
||||
}
|
||||
sub_y += dy * last_pixel;
|
||||
probability_grid->ApplyLookupTable(current, miss_table);
|
||||
while (sub_y < 0) {
|
||||
sub_y += denominator;
|
||||
--current.y();
|
||||
probability_grid->ApplyLookupTable(current, miss_table);
|
||||
}
|
||||
CHECK_NE(sub_y, 0);
|
||||
CHECK_EQ(current.y(), end.y() / kSubpixelScale);
|
||||
}
|
||||
|
||||
void GrowAsNeeded(const sensor::RangeData& range_data,
|
||||
ProbabilityGrid* const probability_grid) {
|
||||
Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
|
||||
constexpr float kPadding = 1e-6f;
|
||||
for (const Eigen::Vector3f& hit : range_data.returns) {
|
||||
bounding_box.extend(hit.head<2>());
|
||||
}
|
||||
for (const Eigen::Vector3f& miss : range_data.misses) {
|
||||
bounding_box.extend(miss.head<2>());
|
||||
}
|
||||
probability_grid->GrowLimits(bounding_box.min() -
|
||||
kPadding * Eigen::Vector2f::Ones());
|
||||
probability_grid->GrowLimits(bounding_box.max() +
|
||||
kPadding * Eigen::Vector2f::Ones());
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
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) {
|
||||
GrowAsNeeded(range_data, probability_grid);
|
||||
|
||||
const MapLimits& limits = probability_grid->limits();
|
||||
const double superscaled_resolution = limits.resolution() / kSubpixelScale;
|
||||
const MapLimits superscaled_limits(
|
||||
superscaled_resolution, limits.max(),
|
||||
CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
|
||||
limits.cell_limits().num_y_cells * kSubpixelScale));
|
||||
const Eigen::Array2i begin =
|
||||
superscaled_limits.GetCellIndex(range_data.origin.head<2>());
|
||||
// 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.GetCellIndex(hit.head<2>()));
|
||||
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_table, probability_grid);
|
||||
}
|
||||
|
||||
// Finally, compute and add empty rays based on misses in the range data.
|
||||
for (const Eigen::Vector3f& missing_echo : range_data.misses) {
|
||||
CastRay(begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()),
|
||||
miss_table, probability_grid);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,159 @@
|
|||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h"
|
||||
|
||||
#include "Eigen/Dense"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
namespace {
|
||||
|
||||
bool isEqual(const Eigen::Array2i& lhs, const Eigen::Array2i& rhs) {
|
||||
return ((lhs - rhs).matrix().lpNorm<1>() == 0);
|
||||
}
|
||||
} // namespace
|
||||
|
||||
// Compute all pixels that contain some part of the line segment connecting
|
||||
// 'scaled_begin' and 'scaled_end'. 'scaled_begin' and 'scaled_end' are scaled
|
||||
// by 'subpixel_scale'. 'scaled_begin' and 'scaled_end' are expected to be
|
||||
// greater than zero. Return values are in pixels and not scaled.
|
||||
std::vector<Eigen::Array2i> RayToPixelMask(const Eigen::Array2i& scaled_begin,
|
||||
const Eigen::Array2i& scaled_end,
|
||||
int subpixel_scale) {
|
||||
// For simplicity, we order 'scaled_begin' and 'scaled_end' by their x
|
||||
// coordinate.
|
||||
if (scaled_begin.x() > scaled_end.x()) {
|
||||
return RayToPixelMask(scaled_end, scaled_begin, subpixel_scale);
|
||||
}
|
||||
|
||||
CHECK_GE(scaled_begin.x(), 0);
|
||||
CHECK_GE(scaled_begin.y(), 0);
|
||||
CHECK_GE(scaled_end.y(), 0);
|
||||
std::vector<Eigen::Array2i> pixel_mask;
|
||||
// Special case: We have to draw a vertical line in full pixels, as
|
||||
// 'scaled_begin' and 'scaled_end' have the same full pixel x coordinate.
|
||||
if (scaled_begin.x() / subpixel_scale == scaled_end.x() / subpixel_scale) {
|
||||
Eigen::Array2i current(
|
||||
scaled_begin.x() / subpixel_scale,
|
||||
std::min(scaled_begin.y(), scaled_end.y()) / subpixel_scale);
|
||||
pixel_mask.push_back(current);
|
||||
const int end_y =
|
||||
std::max(scaled_begin.y(), scaled_end.y()) / subpixel_scale;
|
||||
for (; current.y() <= end_y; ++current.y()) {
|
||||
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
|
||||
}
|
||||
return pixel_mask;
|
||||
}
|
||||
|
||||
const int64 dx = scaled_end.x() - scaled_begin.x();
|
||||
const int64 dy = scaled_end.y() - scaled_begin.y();
|
||||
const int64 denominator = 2 * subpixel_scale * dx;
|
||||
|
||||
// The current full pixel coordinates. We scaled_begin at 'scaled_begin'.
|
||||
Eigen::Array2i current = scaled_begin / subpixel_scale;
|
||||
pixel_mask.push_back(current);
|
||||
|
||||
// To represent subpixel centers, we use a factor of 2 * 'subpixel_scale' in
|
||||
// the denominator.
|
||||
// +-+-+-+ -- 1 = (2 * subpixel_scale) / (2 * subpixel_scale)
|
||||
// | | | |
|
||||
// +-+-+-+
|
||||
// | | | |
|
||||
// +-+-+-+ -- top edge of first subpixel = 2 / (2 * subpixel_scale)
|
||||
// | | | | -- center of first subpixel = 1 / (2 * subpixel_scale)
|
||||
// +-+-+-+ -- 0 = 0 / (2 * subpixel_scale)
|
||||
|
||||
// The center of the subpixel part of 'scaled_begin.y()' assuming the
|
||||
// 'denominator', i.e., sub_y / denominator is in (0, 1).
|
||||
int64 sub_y = (2 * (scaled_begin.y() % subpixel_scale) + 1) * dx;
|
||||
|
||||
// The distance from the from 'scaled_begin' to the right pixel border, to be
|
||||
// divided by 2 * 'subpixel_scale'.
|
||||
const int first_pixel =
|
||||
2 * subpixel_scale - 2 * (scaled_begin.x() % subpixel_scale) - 1;
|
||||
// The same from the left pixel border to 'scaled_end'.
|
||||
const int last_pixel = 2 * (scaled_end.x() % subpixel_scale) + 1;
|
||||
|
||||
// The full pixel x coordinate of 'scaled_end'.
|
||||
const int end_x = std::max(scaled_begin.x(), scaled_end.x()) / subpixel_scale;
|
||||
|
||||
// Move from 'scaled_begin' to the next pixel border to the right.
|
||||
sub_y += dy * first_pixel;
|
||||
if (dy > 0) {
|
||||
while (true) {
|
||||
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
|
||||
while (sub_y > denominator) {
|
||||
sub_y -= denominator;
|
||||
++current.y();
|
||||
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
|
||||
}
|
||||
++current.x();
|
||||
if (sub_y == denominator) {
|
||||
sub_y -= denominator;
|
||||
++current.y();
|
||||
}
|
||||
if (current.x() == end_x) {
|
||||
break;
|
||||
}
|
||||
// Move from one pixel border to the next.
|
||||
sub_y += dy * 2 * subpixel_scale;
|
||||
}
|
||||
// Move from the pixel border on the right to 'scaled_end'.
|
||||
sub_y += dy * last_pixel;
|
||||
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
|
||||
while (sub_y > denominator) {
|
||||
sub_y -= denominator;
|
||||
++current.y();
|
||||
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
|
||||
}
|
||||
CHECK_NE(sub_y, denominator);
|
||||
CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale);
|
||||
return pixel_mask;
|
||||
}
|
||||
|
||||
// Same for lines non-ascending in y coordinates.
|
||||
while (true) {
|
||||
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
|
||||
while (sub_y < 0) {
|
||||
sub_y += denominator;
|
||||
--current.y();
|
||||
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
|
||||
}
|
||||
++current.x();
|
||||
if (sub_y == 0) {
|
||||
sub_y += denominator;
|
||||
--current.y();
|
||||
}
|
||||
if (current.x() == end_x) {
|
||||
break;
|
||||
}
|
||||
sub_y += dy * 2 * subpixel_scale;
|
||||
}
|
||||
sub_y += dy * last_pixel;
|
||||
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
|
||||
while (sub_y < 0) {
|
||||
sub_y += denominator;
|
||||
--current.y();
|
||||
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
|
||||
}
|
||||
CHECK_NE(sub_y, 0);
|
||||
CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale);
|
||||
return pixel_mask;
|
||||
}
|
||||
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
|
@ -14,28 +14,25 @@
|
|||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_CASTING_H_
|
||||
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_CASTING_H_
|
||||
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_TO_PIXEL_MASK_H_
|
||||
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_TO_PIXEL_MASK_H_
|
||||
|
||||
#include <vector>
|
||||
|
||||
#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"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
|
||||
// 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);
|
||||
// Compute all pixels that contain some part of the line segment connecting
|
||||
// 'scaled_begin' and 'scaled_end'. 'scaled_begin' and 'scaled_end' are scaled
|
||||
// by 'subpixel_scale'. 'scaled_begin' and 'scaled_end' are expected to be
|
||||
// greater than zero. Return values are in pixels and not scaled.
|
||||
std::vector<Eigen::Array2i> RayToPixelMask(const Eigen::Array2i& scaled_begin,
|
||||
const Eigen::Array2i& scaled_end,
|
||||
int subpixel_scale);
|
||||
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_CASTING_H_
|
||||
#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_TO_PIXEL_MASK_H_
|
||||
|
|
@ -0,0 +1,207 @@
|
|||
/*
|
||||
* Copyright 2018 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h"
|
||||
|
||||
#include "cartographer/mapping/2d/map_limits.h"
|
||||
#include "gmock/gmock.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping {
|
||||
namespace {
|
||||
|
||||
using ::testing::ElementsAre;
|
||||
|
||||
MATCHER_P(PixelMaskEqual, value, "") {
|
||||
Eigen::Array2i residual = value - arg;
|
||||
return residual.matrix().lpNorm<1>() == 0;
|
||||
}
|
||||
|
||||
TEST(RayToPixelMaskTest, SingleCell) {
|
||||
const Eigen::Array2i& begin = {1, 1};
|
||||
const Eigen::Array2i& end = {1, 1};
|
||||
const int subpixel_scale = 1;
|
||||
std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
|
||||
std::vector<Eigen::Array2i> ray_reference =
|
||||
std::vector<Eigen::Array2i>{begin};
|
||||
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(begin)));
|
||||
}
|
||||
|
||||
TEST(RayToPixelMaskTest, AxisAlignedX) {
|
||||
const Eigen::Array2i& begin = {1, 1};
|
||||
const Eigen::Array2i& end = {3, 1};
|
||||
const int subpixel_scale = 1;
|
||||
std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
|
||||
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
|
||||
PixelMaskEqual(Eigen::Array2i({2, 1})),
|
||||
PixelMaskEqual(Eigen::Array2i({3, 1}))));
|
||||
ray = RayToPixelMask(end, begin, subpixel_scale);
|
||||
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
|
||||
PixelMaskEqual(Eigen::Array2i({2, 1})),
|
||||
PixelMaskEqual(Eigen::Array2i({3, 1}))));
|
||||
}
|
||||
|
||||
TEST(RayToPixelMaskTest, AxisAlignedY) {
|
||||
const Eigen::Array2i& begin = {1, 1};
|
||||
const Eigen::Array2i& end = {1, 3};
|
||||
const int subpixel_scale = 1;
|
||||
std::vector<Eigen::Array2i> ray_reference = std::vector<Eigen::Array2i>{
|
||||
Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({1, 3})};
|
||||
std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
|
||||
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
|
||||
PixelMaskEqual(Eigen::Array2i({1, 2})),
|
||||
PixelMaskEqual(Eigen::Array2i({1, 3}))));
|
||||
ray = RayToPixelMask(end, begin, subpixel_scale);
|
||||
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
|
||||
PixelMaskEqual(Eigen::Array2i({1, 2})),
|
||||
PixelMaskEqual(Eigen::Array2i({1, 3}))));
|
||||
}
|
||||
|
||||
TEST(RayToPixelMaskTest, Diagonal) {
|
||||
Eigen::Array2i begin = {1, 1};
|
||||
Eigen::Array2i end = {3, 3};
|
||||
const int subpixel_scale = 1;
|
||||
std::vector<Eigen::Array2i> ray_reference = std::vector<Eigen::Array2i>{
|
||||
Eigen::Array2i({1, 1}), Eigen::Array2i({2, 2}), Eigen::Array2i({3, 3})};
|
||||
std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
|
||||
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
|
||||
PixelMaskEqual(Eigen::Array2i({2, 2})),
|
||||
PixelMaskEqual(Eigen::Array2i({3, 3}))));
|
||||
ray = RayToPixelMask(end, begin, subpixel_scale);
|
||||
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
|
||||
PixelMaskEqual(Eigen::Array2i({2, 2})),
|
||||
PixelMaskEqual(Eigen::Array2i({3, 3}))));
|
||||
begin = Eigen::Array2i({1, 3});
|
||||
end = Eigen::Array2i({3, 1});
|
||||
ray = RayToPixelMask(begin, end, subpixel_scale);
|
||||
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 3})),
|
||||
PixelMaskEqual(Eigen::Array2i({2, 2})),
|
||||
PixelMaskEqual(Eigen::Array2i({3, 1}))));
|
||||
ray = RayToPixelMask(end, begin, subpixel_scale);
|
||||
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 3})),
|
||||
PixelMaskEqual(Eigen::Array2i({2, 2})),
|
||||
PixelMaskEqual(Eigen::Array2i({3, 1}))));
|
||||
}
|
||||
|
||||
TEST(RayToPixelMaskTest, SteepLine) {
|
||||
Eigen::Array2i begin = {1, 1};
|
||||
Eigen::Array2i end = {2, 5};
|
||||
const int subpixel_scale = 1;
|
||||
std::vector<Eigen::Array2i> ray_reference = std::vector<Eigen::Array2i>{
|
||||
Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({1, 3}),
|
||||
Eigen::Array2i({2, 3}), Eigen::Array2i({2, 4}), Eigen::Array2i({2, 5})};
|
||||
std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
|
||||
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
|
||||
PixelMaskEqual(Eigen::Array2i({1, 2})),
|
||||
PixelMaskEqual(Eigen::Array2i({1, 3})),
|
||||
PixelMaskEqual(Eigen::Array2i({2, 3})),
|
||||
PixelMaskEqual(Eigen::Array2i({2, 4})),
|
||||
PixelMaskEqual(Eigen::Array2i({2, 5}))));
|
||||
begin = {1, 1};
|
||||
end = {2, 4};
|
||||
ray_reference = std::vector<Eigen::Array2i>{
|
||||
Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({2, 3}),
|
||||
Eigen::Array2i({2, 4})};
|
||||
ray = RayToPixelMask(begin, end, subpixel_scale);
|
||||
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
|
||||
PixelMaskEqual(Eigen::Array2i({1, 2})),
|
||||
PixelMaskEqual(Eigen::Array2i({2, 3})),
|
||||
PixelMaskEqual(Eigen::Array2i({2, 4}))));
|
||||
}
|
||||
|
||||
TEST(RayToPixelMaskTest, FlatLine) {
|
||||
Eigen::Array2i begin = {1, 1};
|
||||
Eigen::Array2i end = {5, 2};
|
||||
const int subpixel_scale = 1;
|
||||
std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
|
||||
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
|
||||
PixelMaskEqual(Eigen::Array2i({2, 1})),
|
||||
PixelMaskEqual(Eigen::Array2i({3, 1})),
|
||||
PixelMaskEqual(Eigen::Array2i({3, 2})),
|
||||
PixelMaskEqual(Eigen::Array2i({4, 2})),
|
||||
PixelMaskEqual(Eigen::Array2i({5, 2}))));
|
||||
begin = {1, 1};
|
||||
end = {4, 2};
|
||||
ray = RayToPixelMask(begin, end, subpixel_scale);
|
||||
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
|
||||
PixelMaskEqual(Eigen::Array2i({2, 1})),
|
||||
PixelMaskEqual(Eigen::Array2i({3, 2})),
|
||||
PixelMaskEqual(Eigen::Array2i({4, 2}))));
|
||||
}
|
||||
|
||||
TEST(RayToPixelMaskTest, MultiScaleAxisAlignedX) {
|
||||
int subpixel_scale;
|
||||
const int num_cells_x = 20;
|
||||
const int num_cells_y = 20;
|
||||
double resolution = 0.1;
|
||||
Eigen::Vector2d max = {1.0, 1.0};
|
||||
std::vector<Eigen::Array2i> base_scale_ray;
|
||||
for (subpixel_scale = 1; subpixel_scale < 10000; subpixel_scale *= 2) {
|
||||
double superscaled_resolution = resolution / subpixel_scale;
|
||||
MapLimits superscaled_limits(
|
||||
superscaled_resolution, max,
|
||||
CellLimits(num_cells_x * subpixel_scale, num_cells_y * subpixel_scale));
|
||||
Eigen::Array2i begin =
|
||||
superscaled_limits.GetCellIndex(Eigen::Vector2f({0.05, 0.05}));
|
||||
Eigen::Array2i end =
|
||||
superscaled_limits.GetCellIndex(Eigen::Vector2f({0.35, 0.05}));
|
||||
std::vector<Eigen::Array2i> ray =
|
||||
RayToPixelMask(begin, end, subpixel_scale);
|
||||
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({9, 6})),
|
||||
PixelMaskEqual(Eigen::Array2i({9, 7})),
|
||||
PixelMaskEqual(Eigen::Array2i({9, 8})),
|
||||
PixelMaskEqual(Eigen::Array2i({9, 9}))));
|
||||
}
|
||||
}
|
||||
|
||||
TEST(RayToPixelMaskTest, MultiScaleSkewedLine) {
|
||||
int subpixel_scale = 1;
|
||||
const int num_cells_x = 20;
|
||||
const int num_cells_y = 20;
|
||||
double resolution = 0.1;
|
||||
Eigen::Vector2d max = {1.0, 1.0};
|
||||
double superscaled_resolution = resolution / subpixel_scale;
|
||||
MapLimits superscaled_limits(
|
||||
superscaled_resolution, max,
|
||||
CellLimits(num_cells_x * subpixel_scale, num_cells_y * subpixel_scale));
|
||||
Eigen::Array2i begin =
|
||||
superscaled_limits.GetCellIndex(Eigen::Vector2f({0.01, 0.09}));
|
||||
Eigen::Array2i end =
|
||||
superscaled_limits.GetCellIndex(Eigen::Vector2f({0.21, 0.19}));
|
||||
|
||||
std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
|
||||
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({8, 7})),
|
||||
PixelMaskEqual(Eigen::Array2i({8, 8})),
|
||||
PixelMaskEqual(Eigen::Array2i({9, 8})),
|
||||
PixelMaskEqual(Eigen::Array2i({9, 9}))));
|
||||
subpixel_scale = 20;
|
||||
superscaled_resolution = resolution / subpixel_scale;
|
||||
superscaled_limits = MapLimits(
|
||||
superscaled_resolution, max,
|
||||
CellLimits(num_cells_x * subpixel_scale, num_cells_y * subpixel_scale));
|
||||
begin = superscaled_limits.GetCellIndex(Eigen::Vector2f({0.01, 0.09}));
|
||||
end = superscaled_limits.GetCellIndex(Eigen::Vector2f({0.21, 0.19}));
|
||||
ray = RayToPixelMask(begin, end, subpixel_scale);
|
||||
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({8, 7})),
|
||||
PixelMaskEqual(Eigen::Array2i({8, 8})),
|
||||
PixelMaskEqual(Eigen::Array2i({8, 9})),
|
||||
PixelMaskEqual(Eigen::Array2i({9, 9}))));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mapping
|
||||
} // namespace cartographer
|
||||
Loading…
Reference in New Issue