Skip to content

Commit 2e0aacc

Browse files
author
wanghao
committed
(fix) 对cartographer 2.0版本性能问题的修正
1 parent 105c034 commit 2e0aacc

12 files changed

+92
-63
lines changed

‎cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc

+12-11
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,8 @@ void GrowAsNeeded(const sensor::RangeData& range_data,
5252
void CastRays(const sensor::RangeData& range_data,
5353
const std::vector<uint16>& hit_table,
5454
const std::vector<uint16>& miss_table,
55-
const bool insert_free_space, ProbabilityGrid* probability_grid) {
55+
const bool insert_free_space, ProbabilityGrid* probability_grid,
56+
std::vector<Eigen::Array2i>& pixel_mask) {
5657
GrowAsNeeded(range_data, probability_grid);
5758

5859
const MapLimits& limits = probability_grid->limits();
@@ -77,19 +78,17 @@ void CastRays(const sensor::RangeData& range_data,
7778

7879
// Now add the misses.
7980
for (const Eigen::Array2i& end : ends) {
80-
std::vector<Eigen::Array2i> ray =
81-
RayToPixelMask(begin, end, kSubpixelScale);
82-
for (const Eigen::Array2i& cell_index : ray) {
81+
RayToPixelMask(begin, end, kSubpixelScale, pixel_mask);
82+
for (const Eigen::Array2i& cell_index : pixel_mask) {
8383
probability_grid->ApplyLookupTable(cell_index, miss_table);
8484
}
8585
}
8686

8787
// Finally, compute and add empty rays based on misses in the range data.
8888
for (const sensor::RangefinderPoint& missing_echo : range_data.misses) {
89-
std::vector<Eigen::Array2i> ray = RayToPixelMask(
90-
begin, superscaled_limits.GetCellIndex(missing_echo.position.head<2>()),
91-
kSubpixelScale);
92-
for (const Eigen::Array2i& cell_index : ray) {
89+
RayToPixelMask(begin, superscaled_limits.GetCellIndex(missing_echo.position.head<2>()),
90+
kSubpixelScale, pixel_mask);
91+
for (const Eigen::Array2i& cell_index : pixel_mask) {
9392
probability_grid->ApplyLookupTable(cell_index, miss_table);
9493
}
9594
}
@@ -119,16 +118,18 @@ ProbabilityGridRangeDataInserter2D::ProbabilityGridRangeDataInserter2D(
119118
hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
120119
Odds(options.hit_probability()))),
121120
miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(
122-
Odds(options.miss_probability()))) {}
121+
Odds(options.miss_probability()))) {
122+
pixel_mask_.reserve(3000);
123+
}
123124

124125
void ProbabilityGridRangeDataInserter2D::Insert(
125-
const sensor::RangeData& range_data, GridInterface* const grid) const {
126+
const sensor::RangeData& range_data, GridInterface* const grid) {
126127
ProbabilityGrid* const probability_grid = static_cast<ProbabilityGrid*>(grid);
127128
CHECK(probability_grid != nullptr);
128129
// By not finishing the update after hits are inserted, we give hits priority
129130
// (i.e. no hits will be ignored because of a miss in the same cell).
130131
CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(),
131-
probability_grid);
132+
probability_grid, pixel_mask_);
132133
probability_grid->FinishUpdate();
133134
}
134135

‎cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h

+2-1
Original file line numberDiff line numberDiff line change
@@ -48,12 +48,13 @@ class ProbabilityGridRangeDataInserter2D : public RangeDataInserterInterface {
4848

4949
// Inserts 'range_data' into 'probability_grid'.
5050
virtual void Insert(const sensor::RangeData& range_data,
51-
GridInterface* grid) const override;
51+
GridInterface* grid)override;
5252

5353
private:
5454
const proto::ProbabilityGridRangeDataInserterOptions2D options_;
5555
const std::vector<uint16> hit_table_;
5656
const std::vector<uint16> miss_table_;
57+
std::vector<Eigen::Array2i> pixel_mask_;
5758
};
5859

5960
} // namespace mapping

‎cartographer/mapping/2d/submap_2d.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -136,7 +136,7 @@ void Submap2D::ToResponseProto(
136136

137137
void Submap2D::InsertRangeData(
138138
const sensor::RangeData& range_data,
139-
const RangeDataInserterInterface* range_data_inserter) {
139+
RangeDataInserterInterface* range_data_inserter) {
140140
CHECK(grid_);
141141
CHECK(!insertion_finished());
142142
range_data_inserter->Insert(range_data, grid_.get());

‎cartographer/mapping/2d/submap_2d.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -58,7 +58,7 @@ class Submap2D : public Submap {
5858
// Insert 'range_data' into this submap using 'range_data_inserter'. The
5959
// submap must not be finished yet.
6060
void InsertRangeData(const sensor::RangeData& range_data,
61-
const RangeDataInserterInterface* range_data_inserter);
61+
RangeDataInserterInterface* range_data_inserter);
6262
void Finish();
6363

6464
private:

‎cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

+19-11
Original file line numberDiff line numberDiff line change
@@ -64,7 +64,7 @@ LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
6464

6565
std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
6666
const common::Time time, const transform::Rigid2d& pose_prediction,
67-
const sensor::PointCloud& filtered_gravity_aligned_point_cloud) {
67+
const sensor::RangeData& gravity_aligned_range_data) {
6868
if (active_submaps_.submaps().empty()) {
6969
return absl::make_unique<transform::Rigid2d>(pose_prediction);
7070
}
@@ -74,6 +74,13 @@ std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
7474
// the Ceres scan matcher.
7575
transform::Rigid2d initial_ceres_pose = pose_prediction;
7676

77+
const sensor::PointCloud filtered_gravity_aligned_point_cloud =
78+
sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,
79+
options_.adaptive_voxel_filter_options());
80+
if (filtered_gravity_aligned_point_cloud.empty()) {
81+
return nullptr;
82+
}
83+
7784
if (options_.use_online_correlative_scan_matching()) {
7885
const double score = real_time_correlative_scan_matcher_.Match(
7986
pose_prediction, filtered_gravity_aligned_point_cloud,
@@ -225,16 +232,9 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
225232
const transform::Rigid2d pose_prediction = transform::Project2D(
226233
non_gravity_aligned_pose_prediction * gravity_alignment.inverse());
227234

228-
const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
229-
sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,
230-
options_.adaptive_voxel_filter_options());
231-
if (filtered_gravity_aligned_point_cloud.empty()) {
232-
return nullptr;
233-
}
234-
235235
// local map frame <- gravity-aligned frame
236236
std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
237-
ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
237+
ScanMatch(time, pose_prediction, gravity_aligned_range_data);
238238
if (pose_estimate_2d == nullptr) {
239239
LOG(WARNING) << "Scan matching failed.";
240240
return nullptr;
@@ -247,7 +247,7 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
247247
TransformRangeData(gravity_aligned_range_data,
248248
transform::Embed3D(pose_estimate_2d->cast<float>()));
249249
std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
250-
time, range_data_in_local, filtered_gravity_aligned_point_cloud,
250+
time, range_data_in_local, gravity_aligned_range_data,
251251
pose_estimate, gravity_alignment.rotation());
252252

253253
const auto wall_time = std::chrono::steady_clock::now();
@@ -279,14 +279,22 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
279279
std::unique_ptr<LocalTrajectoryBuilder2D::InsertionResult>
280280
LocalTrajectoryBuilder2D::InsertIntoSubmap(
281281
const common::Time time, const sensor::RangeData& range_data_in_local,
282-
const sensor::PointCloud& filtered_gravity_aligned_point_cloud,
282+
const sensor::RangeData& gravity_aligned_range_data,
283283
const transform::Rigid3d& pose_estimate,
284284
const Eigen::Quaterniond& gravity_alignment) {
285285
if (motion_filter_.IsSimilar(time, pose_estimate)) {
286286
return nullptr;
287287
}
288288
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
289289
active_submaps_.InsertRangeData(range_data_in_local);
290+
291+
const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
292+
sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,
293+
options_.loop_closure_adaptive_voxel_filter_options());
294+
if (filtered_gravity_aligned_point_cloud.empty()) {
295+
return nullptr;
296+
}
297+
290298
return absl::make_unique<InsertionResult>(InsertionResult{
291299
std::make_shared<const TrajectoryNode::Data>(TrajectoryNode::Data{
292300
time,

‎cartographer/mapping/internal/2d/local_trajectory_builder_2d.h

+2-2
Original file line numberDiff line numberDiff line change
@@ -86,15 +86,15 @@ class LocalTrajectoryBuilder2D {
8686
const sensor::RangeData& range_data) const;
8787
std::unique_ptr<InsertionResult> InsertIntoSubmap(
8888
common::Time time, const sensor::RangeData& range_data_in_local,
89-
const sensor::PointCloud& filtered_gravity_aligned_point_cloud,
89+
const sensor::RangeData& gravity_aligned_range_data,
9090
const transform::Rigid3d& pose_estimate,
9191
const Eigen::Quaterniond& gravity_alignment);
9292

9393
// Scan matches 'filtered_gravity_aligned_point_cloud' and returns the
9494
// observed pose, or nullptr on failure.
9595
std::unique_ptr<transform::Rigid2d> ScanMatch(
9696
common::Time time, const transform::Rigid2d& pose_prediction,
97-
const sensor::PointCloud& filtered_gravity_aligned_point_cloud);
97+
const sensor::RangeData& gravity_aligned_range_data);
9898

9999
// Lazily constructs a PoseExtrapolator.
100100
void InitializeExtrapolator(common::Time time);

‎cartographer/mapping/internal/2d/ray_to_pixel_mask.cc

+7-7
Original file line numberDiff line numberDiff line change
@@ -31,19 +31,20 @@ bool isEqual(const Eigen::Array2i& lhs, const Eigen::Array2i& rhs) {
3131
// 'scaled_begin' and 'scaled_end'. 'scaled_begin' and 'scaled_end' are scaled
3232
// by 'subpixel_scale'. 'scaled_begin' and 'scaled_end' are expected to be
3333
// greater than zero. Return values are in pixels and not scaled.
34-
std::vector<Eigen::Array2i> RayToPixelMask(const Eigen::Array2i& scaled_begin,
34+
void RayToPixelMask(const Eigen::Array2i& scaled_begin,
3535
const Eigen::Array2i& scaled_end,
36-
int subpixel_scale) {
36+
int subpixel_scale,
37+
std::vector<Eigen::Array2i>& pixel_mask) {
3738
// For simplicity, we order 'scaled_begin' and 'scaled_end' by their x
3839
// coordinate.
3940
if (scaled_begin.x() > scaled_end.x()) {
40-
return RayToPixelMask(scaled_end, scaled_begin, subpixel_scale);
41+
return RayToPixelMask(scaled_end, scaled_begin, subpixel_scale, pixel_mask);
4142
}
4243

4344
CHECK_GE(scaled_begin.x(), 0);
4445
CHECK_GE(scaled_begin.y(), 0);
4546
CHECK_GE(scaled_end.y(), 0);
46-
std::vector<Eigen::Array2i> pixel_mask;
47+
pixel_mask.resize(0);
4748
// Special case: We have to draw a vertical line in full pixels, as
4849
// 'scaled_begin' and 'scaled_end' have the same full pixel x coordinate.
4950
if (scaled_begin.x() / subpixel_scale == scaled_end.x() / subpixel_scale) {
@@ -56,7 +57,7 @@ std::vector<Eigen::Array2i> RayToPixelMask(const Eigen::Array2i& scaled_begin,
5657
for (; current.y() <= end_y; ++current.y()) {
5758
if (!isEqual(pixel_mask.back(), current)) pixel_mask.push_back(current);
5859
}
59-
return pixel_mask;
60+
return;
6061
}
6162

6263
const int64 dx = scaled_end.x() - scaled_begin.x();
@@ -122,7 +123,7 @@ std::vector<Eigen::Array2i> RayToPixelMask(const Eigen::Array2i& scaled_begin,
122123
}
123124
CHECK_NE(sub_y, denominator);
124125
CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale);
125-
return pixel_mask;
126+
return;
126127
}
127128

128129
// Same for lines non-ascending in y coordinates.
@@ -152,7 +153,6 @@ std::vector<Eigen::Array2i> RayToPixelMask(const Eigen::Array2i& scaled_begin,
152153
}
153154
CHECK_NE(sub_y, 0);
154155
CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale);
155-
return pixel_mask;
156156
}
157157

158158
} // namespace mapping

‎cartographer/mapping/internal/2d/ray_to_pixel_mask.h

+3-2
Original file line numberDiff line numberDiff line change
@@ -28,9 +28,10 @@ namespace mapping {
2828
// 'scaled_begin' and 'scaled_end'. 'scaled_begin' and 'scaled_end' are scaled
2929
// by 'subpixel_scale'. 'scaled_begin' and 'scaled_end' are expected to be
3030
// greater than zero. Return values are in pixels and not scaled.
31-
std::vector<Eigen::Array2i> RayToPixelMask(const Eigen::Array2i& scaled_begin,
31+
void RayToPixelMask(const Eigen::Array2i& scaled_begin,
3232
const Eigen::Array2i& scaled_end,
33-
int subpixel_scale);
33+
int subpixel_scale,
34+
std::vector<Eigen::Array2i>& pixel_mask);
3435

3536
} // namespace mapping
3637
} // namespace cartographer

‎cartographer/mapping/internal/2d/ray_to_pixel_mask_test.cc

+33-18
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,8 @@ TEST(RayToPixelMaskTest, SingleCell) {
3535
const Eigen::Array2i& begin = {1, 1};
3636
const Eigen::Array2i& end = {1, 1};
3737
const int subpixel_scale = 1;
38-
std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
38+
std::vector<Eigen::Array2i> ray;
39+
RayToPixelMask(begin, end, subpixel_scale, ray);
3940
std::vector<Eigen::Array2i> ray_reference =
4041
std::vector<Eigen::Array2i>{begin};
4142
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(begin)));
@@ -45,11 +46,13 @@ TEST(RayToPixelMaskTest, AxisAlignedX) {
4546
const Eigen::Array2i& begin = {1, 1};
4647
const Eigen::Array2i& end = {3, 1};
4748
const int subpixel_scale = 1;
48-
std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
49+
std::vector<Eigen::Array2i> ray;
50+
RayToPixelMask(begin, end, subpixel_scale, ray);
4951
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
5052
PixelMaskEqual(Eigen::Array2i({2, 1})),
5153
PixelMaskEqual(Eigen::Array2i({3, 1}))));
52-
ray = RayToPixelMask(end, begin, subpixel_scale);
54+
ray.resize(0);
55+
RayToPixelMask(begin, end, subpixel_scale, ray);
5356
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
5457
PixelMaskEqual(Eigen::Array2i({2, 1})),
5558
PixelMaskEqual(Eigen::Array2i({3, 1}))));
@@ -61,11 +64,13 @@ TEST(RayToPixelMaskTest, AxisAlignedY) {
6164
const int subpixel_scale = 1;
6265
std::vector<Eigen::Array2i> ray_reference = std::vector<Eigen::Array2i>{
6366
Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({1, 3})};
64-
std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
67+
std::vector<Eigen::Array2i> ray;
68+
RayToPixelMask(begin, end, subpixel_scale, ray);
6569
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
6670
PixelMaskEqual(Eigen::Array2i({1, 2})),
6771
PixelMaskEqual(Eigen::Array2i({1, 3}))));
68-
ray = RayToPixelMask(end, begin, subpixel_scale);
72+
ray.resize(0);
73+
RayToPixelMask(begin, end, subpixel_scale, ray);
6974
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
7075
PixelMaskEqual(Eigen::Array2i({1, 2})),
7176
PixelMaskEqual(Eigen::Array2i({1, 3}))));
@@ -77,21 +82,25 @@ TEST(RayToPixelMaskTest, Diagonal) {
7782
const int subpixel_scale = 1;
7883
std::vector<Eigen::Array2i> ray_reference = std::vector<Eigen::Array2i>{
7984
Eigen::Array2i({1, 1}), Eigen::Array2i({2, 2}), Eigen::Array2i({3, 3})};
80-
std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
85+
std::vector<Eigen::Array2i> ray;
86+
RayToPixelMask(begin, end, subpixel_scale, ray);
8187
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
8288
PixelMaskEqual(Eigen::Array2i({2, 2})),
8389
PixelMaskEqual(Eigen::Array2i({3, 3}))));
84-
ray = RayToPixelMask(end, begin, subpixel_scale);
90+
ray.resize(0);
91+
RayToPixelMask(begin, end, subpixel_scale, ray);
8592
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
8693
PixelMaskEqual(Eigen::Array2i({2, 2})),
8794
PixelMaskEqual(Eigen::Array2i({3, 3}))));
8895
begin = Eigen::Array2i({1, 3});
8996
end = Eigen::Array2i({3, 1});
90-
ray = RayToPixelMask(begin, end, subpixel_scale);
97+
ray.resize(0);
98+
RayToPixelMask(begin, end, subpixel_scale, ray);
9199
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 3})),
92100
PixelMaskEqual(Eigen::Array2i({2, 2})),
93101
PixelMaskEqual(Eigen::Array2i({3, 1}))));
94-
ray = RayToPixelMask(end, begin, subpixel_scale);
102+
ray.resize(0);
103+
RayToPixelMask(begin, end, subpixel_scale, ray);
95104
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 3})),
96105
PixelMaskEqual(Eigen::Array2i({2, 2})),
97106
PixelMaskEqual(Eigen::Array2i({3, 1}))));
@@ -104,7 +113,8 @@ TEST(RayToPixelMaskTest, SteepLine) {
104113
std::vector<Eigen::Array2i> ray_reference = std::vector<Eigen::Array2i>{
105114
Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({1, 3}),
106115
Eigen::Array2i({2, 3}), Eigen::Array2i({2, 4}), Eigen::Array2i({2, 5})};
107-
std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
116+
std::vector<Eigen::Array2i> ray;
117+
RayToPixelMask(begin, end, subpixel_scale, ray);
108118
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
109119
PixelMaskEqual(Eigen::Array2i({1, 2})),
110120
PixelMaskEqual(Eigen::Array2i({1, 3})),
@@ -116,7 +126,8 @@ TEST(RayToPixelMaskTest, SteepLine) {
116126
ray_reference = std::vector<Eigen::Array2i>{
117127
Eigen::Array2i({1, 1}), Eigen::Array2i({1, 2}), Eigen::Array2i({2, 3}),
118128
Eigen::Array2i({2, 4})};
119-
ray = RayToPixelMask(begin, end, subpixel_scale);
129+
ray.resize(0);
130+
RayToPixelMask(begin, end, subpixel_scale, ray);
120131
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
121132
PixelMaskEqual(Eigen::Array2i({1, 2})),
122133
PixelMaskEqual(Eigen::Array2i({2, 3})),
@@ -127,7 +138,8 @@ TEST(RayToPixelMaskTest, FlatLine) {
127138
Eigen::Array2i begin = {1, 1};
128139
Eigen::Array2i end = {5, 2};
129140
const int subpixel_scale = 1;
130-
std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
141+
std::vector<Eigen::Array2i> ray;
142+
RayToPixelMask(begin, end, subpixel_scale, ray);
131143
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
132144
PixelMaskEqual(Eigen::Array2i({2, 1})),
133145
PixelMaskEqual(Eigen::Array2i({3, 1})),
@@ -136,7 +148,8 @@ TEST(RayToPixelMaskTest, FlatLine) {
136148
PixelMaskEqual(Eigen::Array2i({5, 2}))));
137149
begin = {1, 1};
138150
end = {4, 2};
139-
ray = RayToPixelMask(begin, end, subpixel_scale);
151+
ray.resize(0);
152+
RayToPixelMask(begin, end, subpixel_scale, ray);
140153
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({1, 1})),
141154
PixelMaskEqual(Eigen::Array2i({2, 1})),
142155
PixelMaskEqual(Eigen::Array2i({3, 2})),
@@ -150,6 +163,7 @@ TEST(RayToPixelMaskTest, MultiScaleAxisAlignedX) {
150163
double resolution = 0.1;
151164
Eigen::Vector2d max = {1.0, 1.0};
152165
std::vector<Eigen::Array2i> base_scale_ray;
166+
std::vector<Eigen::Array2i> ray;
153167
for (subpixel_scale = 1; subpixel_scale < 10000; subpixel_scale *= 2) {
154168
double superscaled_resolution = resolution / subpixel_scale;
155169
MapLimits superscaled_limits(
@@ -159,8 +173,8 @@ TEST(RayToPixelMaskTest, MultiScaleAxisAlignedX) {
159173
superscaled_limits.GetCellIndex(Eigen::Vector2f({0.05, 0.05}));
160174
Eigen::Array2i end =
161175
superscaled_limits.GetCellIndex(Eigen::Vector2f({0.35, 0.05}));
162-
std::vector<Eigen::Array2i> ray =
163-
RayToPixelMask(begin, end, subpixel_scale);
176+
ray.resize(0);
177+
RayToPixelMask(begin, end, subpixel_scale, ray);
164178
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({9, 6})),
165179
PixelMaskEqual(Eigen::Array2i({9, 7})),
166180
PixelMaskEqual(Eigen::Array2i({9, 8})),
@@ -182,8 +196,8 @@ TEST(RayToPixelMaskTest, MultiScaleSkewedLine) {
182196
superscaled_limits.GetCellIndex(Eigen::Vector2f({0.01, 0.09}));
183197
Eigen::Array2i end =
184198
superscaled_limits.GetCellIndex(Eigen::Vector2f({0.21, 0.19}));
185-
186-
std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, end, subpixel_scale);
199+
std::vector<Eigen::Array2i> ray;
200+
RayToPixelMask(begin, end, subpixel_scale, ray);
187201
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({8, 7})),
188202
PixelMaskEqual(Eigen::Array2i({8, 8})),
189203
PixelMaskEqual(Eigen::Array2i({9, 8})),
@@ -195,7 +209,8 @@ TEST(RayToPixelMaskTest, MultiScaleSkewedLine) {
195209
CellLimits(num_cells_x * subpixel_scale, num_cells_y * subpixel_scale));
196210
begin = superscaled_limits.GetCellIndex(Eigen::Vector2f({0.01, 0.09}));
197211
end = superscaled_limits.GetCellIndex(Eigen::Vector2f({0.21, 0.19}));
198-
ray = RayToPixelMask(begin, end, subpixel_scale);
212+
ray.resize(0);
213+
RayToPixelMask(begin, end, subpixel_scale, ray);
199214
EXPECT_THAT(ray, ElementsAre(PixelMaskEqual(Eigen::Array2i({8, 7})),
200215
PixelMaskEqual(Eigen::Array2i({8, 8})),
201216
PixelMaskEqual(Eigen::Array2i({8, 9})),

0 commit comments

Comments
 (0)