Skip to content

Commit 2628bdd

Browse files
ojuraakshay-prsd
ojura
authored and
akshay-prsd
committed
Introduce [Timed]RangefinderPoint. (cartographer-project#1357)
1 parent c8a659c commit 2628bdd

File tree

62 files changed

+491
-319
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

62 files changed

+491
-319
lines changed

‎cartographer/cloud/internal/handlers/add_rangefinder_data_handler_test.cc

+4-1
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,10 @@ const std::string kMessage = R"(
4141
x: 3.f y: 4.f z: 5.f
4242
}
4343
point_data {
44-
x: 6.f y: 7.f z: 8.f t: 9.f
44+
position {
45+
x: 6.f y: 7.f z: 8.f
46+
}
47+
time: 9.f
4548
}
4649
})";
4750

‎cartographer/io/min_max_range_filtering_points_processor.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@ void MinMaxRangeFiteringPointsProcessor::Process(
4040
std::unique_ptr<PointsBatch> batch) {
4141
std::unordered_set<int> to_remove;
4242
for (size_t i = 0; i < batch->points.size(); ++i) {
43-
const float range = (batch->points[i] - batch->origin).norm();
43+
const float range = (batch->points[i].position - batch->origin).norm();
4444
if (!(min_range_ <= range && range <= max_range_)) {
4545
to_remove.insert(i);
4646
}

‎cartographer/io/outlier_removing_points_processor.cc

+4-3
Original file line numberDiff line numberDiff line change
@@ -81,7 +81,8 @@ PointsProcessor::FlushResult OutlierRemovingPointsProcessor::Flush() {
8181
void OutlierRemovingPointsProcessor::ProcessInPhaseOne(
8282
const PointsBatch& batch) {
8383
for (size_t i = 0; i < batch.points.size(); ++i) {
84-
++voxels_.mutable_value(voxels_.GetCellIndex(batch.points[i]))->hits;
84+
++voxels_.mutable_value(voxels_.GetCellIndex(batch.points[i].position))
85+
->hits;
8586
}
8687
}
8788

@@ -91,7 +92,7 @@ void OutlierRemovingPointsProcessor::ProcessInPhaseTwo(
9192
// by better ray casting, and also by marking the hits of the current range
9293
// data to be excluded.
9394
for (size_t i = 0; i < batch.points.size(); ++i) {
94-
const Eigen::Vector3f delta = batch.points[i] - batch.origin;
95+
const Eigen::Vector3f delta = batch.points[i].position - batch.origin;
9596
const float length = delta.norm();
9697
for (float x = 0; x < length; x += voxel_size_) {
9798
const Eigen::Array3i index =
@@ -109,7 +110,7 @@ void OutlierRemovingPointsProcessor::ProcessInPhaseThree(
109110
std::unordered_set<int> to_remove;
110111
for (size_t i = 0; i < batch->points.size(); ++i) {
111112
const VoxelData voxel =
112-
voxels_.value(voxels_.GetCellIndex(batch->points[i]));
113+
voxels_.value(voxels_.GetCellIndex(batch->points[i].position));
113114
if (!(voxel.rays < kMissPerHitLimit * voxel.hits)) {
114115
to_remove.insert(i);
115116
}

‎cartographer/io/pcd_writing_points_processor.cc

+2-1
Original file line numberDiff line numberDiff line change
@@ -119,7 +119,8 @@ void PcdWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
119119
WriteBinaryPcdHeader(has_colors_, 0, file_writer_.get());
120120
}
121121
for (size_t i = 0; i < batch->points.size(); ++i) {
122-
WriteBinaryPcdPointCoordinate(batch->points[i], file_writer_.get());
122+
WriteBinaryPcdPointCoordinate(batch->points[i].position,
123+
file_writer_.get());
123124
if (!batch->colors.empty()) {
124125
WriteBinaryPcdPointColor(ToUint8Color(batch->colors[i]),
125126
file_writer_.get());

‎cartographer/io/ply_writing_points_processor.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -136,7 +136,7 @@ void PlyWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
136136
}
137137

138138
for (size_t i = 0; i < batch->points.size(); ++i) {
139-
WriteBinaryPlyPointCoordinate(batch->points[i], file_.get());
139+
WriteBinaryPlyPointCoordinate(batch->points[i].position, file_.get());
140140
if (has_colors_) {
141141
WriteBinaryPlyPointColor(ToUint8Color(batch->colors[i]), file_.get());
142142
}

‎cartographer/io/points_batch.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@ namespace io {
2121

2222
void RemovePoints(std::unordered_set<int> to_remove, PointsBatch* batch) {
2323
const int new_num_points = batch->points.size() - to_remove.size();
24-
std::vector<Eigen::Vector3f> points;
24+
sensor::PointCloud points;
2525
points.reserve(new_num_points);
2626
std::vector<float> intensities;
2727
if (!batch->intensities.empty()) {

‎cartographer/io/points_batch.h

+2-1
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
#ifndef CARTOGRAPHER_IO_POINTS_BATCH_H_
1818
#define CARTOGRAPHER_IO_POINTS_BATCH_H_
1919

20+
#include <cartographer/sensor/point_cloud.h>
2021
#include <array>
2122
#include <cstdint>
2223
#include <unordered_set>
@@ -52,7 +53,7 @@ struct PointsBatch {
5253
int trajectory_id;
5354

5455
// Geometry of the points in the map frame.
55-
std::vector<Eigen::Vector3f> points;
56+
sensor::PointCloud points;
5657

5758
// Intensities are optional and may be unspecified. The meaning of these
5859
// intensity values varies by device. For example, the VLP16 provides values

‎cartographer/io/probability_grid_points_processor_test.cc

+5-5
Original file line numberDiff line numberDiff line change
@@ -34,11 +34,11 @@ namespace {
3434
std::unique_ptr<PointsBatch> CreatePointsBatch() {
3535
auto points_batch = ::absl::make_unique<PointsBatch>();
3636
points_batch->origin = Eigen::Vector3f(0, 0, 0);
37-
points_batch->points.emplace_back(0.0f, 0.0f, 0.0f);
38-
points_batch->points.emplace_back(0.0f, 1.0f, 2.0f);
39-
points_batch->points.emplace_back(1.0f, 2.0f, 4.0f);
40-
points_batch->points.emplace_back(0.0f, 3.0f, 5.0f);
41-
points_batch->points.emplace_back(3.0f, 0.0f, 6.0f);
37+
points_batch->points.push_back({Eigen::Vector3f{0.0f, 0.0f, 0.0f}});
38+
points_batch->points.push_back({Eigen::Vector3f{0.0f, 1.0f, 2.0f}});
39+
points_batch->points.push_back({Eigen::Vector3f{1.0f, 2.0f, 4.0f}});
40+
points_batch->points.push_back({Eigen::Vector3f{0.0f, 3.0f, 5.0f}});
41+
points_batch->points.push_back({Eigen::Vector3f{3.0f, 0.0f, 6.0f}});
4242
return points_batch;
4343
}
4444

‎cartographer/io/xray_points_processor.cc

+2-2
Original file line numberDiff line numberDiff line change
@@ -196,9 +196,9 @@ void XRayPointsProcessor::Insert(const PointsBatch& batch,
196196
Aggregation* const aggregation) {
197197
constexpr FloatColor kDefaultColor = {{0.f, 0.f, 0.f}};
198198
for (size_t i = 0; i < batch.points.size(); ++i) {
199-
const Eigen::Vector3f camera_point = transform_ * batch.points[i];
199+
const sensor::RangefinderPoint camera_point = transform_ * batch.points[i];
200200
const Eigen::Array3i cell_index =
201-
aggregation->voxels.GetCellIndex(camera_point);
201+
aggregation->voxels.GetCellIndex(camera_point.position);
202202
*aggregation->voxels.mutable_value(cell_index) = true;
203203
bounding_box_.extend(cell_index.matrix());
204204
ColumnData& column_data =

‎cartographer/io/xyz_writing_points_processor.cc

+2-2
Original file line numberDiff line numberDiff line change
@@ -48,8 +48,8 @@ PointsProcessor::FlushResult XyzWriterPointsProcessor::Flush() {
4848
}
4949

5050
void XyzWriterPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
51-
for (const Eigen::Vector3f& point : batch->points) {
52-
WriteXyzPoint(point, file_writer_.get());
51+
for (const sensor::RangefinderPoint& point : batch->points) {
52+
WriteXyzPoint(point.position, file_writer_.get());
5353
}
5454
next_->Process(std::move(batch));
5555
}

‎cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc

+8-8
Original file line numberDiff line numberDiff line change
@@ -37,11 +37,11 @@ void GrowAsNeeded(const sensor::RangeData& range_data,
3737
Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
3838
// Padding around bounding box to avoid numerical issues at cell boundaries.
3939
constexpr float kPadding = 1e-6f;
40-
for (const Eigen::Vector3f& hit : range_data.returns) {
41-
bounding_box.extend(hit.head<2>());
40+
for (const sensor::RangefinderPoint& hit : range_data.returns) {
41+
bounding_box.extend(hit.position.head<2>());
4242
}
43-
for (const Eigen::Vector3f& miss : range_data.misses) {
44-
bounding_box.extend(miss.head<2>());
43+
for (const sensor::RangefinderPoint& miss : range_data.misses) {
44+
bounding_box.extend(miss.position.head<2>());
4545
}
4646
probability_grid->GrowLimits(bounding_box.min() -
4747
kPadding * Eigen::Vector2f::Ones());
@@ -66,8 +66,8 @@ void CastRays(const sensor::RangeData& range_data,
6666
// Compute and add the end points.
6767
std::vector<Eigen::Array2i> ends;
6868
ends.reserve(range_data.returns.size());
69-
for (const Eigen::Vector3f& hit : range_data.returns) {
70-
ends.push_back(superscaled_limits.GetCellIndex(hit.head<2>()));
69+
for (const sensor::RangefinderPoint& hit : range_data.returns) {
70+
ends.push_back(superscaled_limits.GetCellIndex(hit.position.head<2>()));
7171
probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
7272
}
7373

@@ -85,9 +85,9 @@ void CastRays(const sensor::RangeData& range_data,
8585
}
8686

8787
// Finally, compute and add empty rays based on misses in the range data.
88-
for (const Eigen::Vector3f& missing_echo : range_data.misses) {
88+
for (const sensor::RangefinderPoint& missing_echo : range_data.misses) {
8989
std::vector<Eigen::Array2i> ray = RayToPixelMask(
90-
begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()),
90+
begin, superscaled_limits.GetCellIndex(missing_echo.position.head<2>()),
9191
kSubpixelScale);
9292
for (const Eigen::Array2i& cell_index : ray) {
9393
probability_grid->ApplyLookupTable(cell_index, miss_table);

‎cartographer/mapping/2d/range_data_inserter_2d_test.cc

+4-4
Original file line numberDiff line numberDiff line change
@@ -49,10 +49,10 @@ class RangeDataInserterTest2D : public ::testing::Test {
4949

5050
void InsertPointCloud() {
5151
sensor::RangeData range_data;
52-
range_data.returns.emplace_back(-3.5f, 0.5f, 0.f);
53-
range_data.returns.emplace_back(-2.5f, 1.5f, 0.f);
54-
range_data.returns.emplace_back(-1.5f, 2.5f, 0.f);
55-
range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
52+
range_data.returns.push_back({Eigen::Vector3f{-3.5f, 0.5f, 0.f}});
53+
range_data.returns.push_back({Eigen::Vector3f{-2.5f, 1.5f, 0.f}});
54+
range_data.returns.push_back({Eigen::Vector3f{-1.5f, 2.5f, 0.f}});
55+
range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
5656
range_data.origin.x() = -0.5f;
5757
range_data.origin.y() = 0.5f;
5858
range_data_inserter_->Insert(range_data, &probability_grid_);

‎cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc

+13-7
Original file line numberDiff line numberDiff line change
@@ -33,9 +33,11 @@ const float kSqrtTwoPi = std::sqrt(2.0 * M_PI);
3333
void GrowAsNeeded(const sensor::RangeData& range_data,
3434
const float truncation_distance, TSDF2D* const tsdf) {
3535
Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
36-
for (const Eigen::Vector3f& hit : range_data.returns) {
37-
const Eigen::Vector3f direction = (hit - range_data.origin).normalized();
38-
const Eigen::Vector3f end_position = hit + truncation_distance * direction;
36+
for (const sensor::RangefinderPoint& hit : range_data.returns) {
37+
const Eigen::Vector3f direction =
38+
(hit.position - range_data.origin).normalized();
39+
const Eigen::Vector3f end_position =
40+
hit.position + truncation_distance * direction;
3941
bounding_box.extend(end_position.head<2>());
4042
}
4143
// Padding around bounding box to avoid numerical issues at cell boundaries.
@@ -66,9 +68,12 @@ std::pair<Eigen::Array2i, Eigen::Array2i> SuperscaleRay(
6668

6769
struct RangeDataSorter {
6870
RangeDataSorter(Eigen::Vector3f origin) { origin_ = origin.head<2>(); }
69-
bool operator()(Eigen::Vector3f lhs, Eigen::Vector3f rhs) {
70-
const Eigen::Vector2f delta_lhs = (lhs.head<2>() - origin_).normalized();
71-
const Eigen::Vector2f delta_rhs = (lhs.head<2>() - origin_).normalized();
71+
bool operator()(const sensor::RangefinderPoint& lhs,
72+
const sensor::RangefinderPoint& rhs) {
73+
const Eigen::Vector2f delta_lhs =
74+
(lhs.position.head<2>() - origin_).normalized();
75+
const Eigen::Vector2f delta_rhs =
76+
(lhs.position.head<2>() - origin_).normalized();
7277
if ((delta_lhs[1] < 0.f) != (delta_rhs[1] < 0.f)) {
7378
return delta_lhs[1] < 0.f;
7479
} else if (delta_lhs[1] < 0.f) {
@@ -147,7 +152,8 @@ void TSDFRangeDataInserter2D::Insert(const sensor::RangeData& range_data,
147152
const Eigen::Vector2f origin = sorted_range_data.origin.head<2>();
148153
for (size_t hit_index = 0; hit_index < sorted_range_data.returns.size();
149154
++hit_index) {
150-
const Eigen::Vector2f hit = sorted_range_data.returns[hit_index].head<2>();
155+
const Eigen::Vector2f hit =
156+
sorted_range_data.returns[hit_index].position.head<2>();
151157
const float normal = normals.empty()
152158
? std::numeric_limits<float>::quiet_NaN()
153159
: normals[hit_index];

‎cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc

+8-8
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,7 @@ class RangeDataInserterTest2DTSDF : public ::testing::Test {
4949

5050
void InsertPoint() {
5151
sensor::RangeData range_data;
52-
range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
52+
range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
5353
range_data.origin.x() = -0.5f;
5454
range_data.origin.y() = -0.5f;
5555
range_data_inserter_->Insert(range_data, &tsdf_);
@@ -237,9 +237,9 @@ TEST_F(RangeDataInserterTest2DTSDF, InsertPointQuadraticWeight) {
237237
TEST_F(RangeDataInserterTest2DTSDF,
238238
InsertSmallAnglePointWithoutNormalProjection) {
239239
sensor::RangeData range_data;
240-
range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
241-
range_data.returns.emplace_back(5.5f, 3.5f, 0.f);
242-
range_data.returns.emplace_back(10.5f, 3.5f, 0.f);
240+
range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
241+
range_data.returns.push_back({Eigen::Vector3f{5.5f, 3.5f, 0.f}});
242+
range_data.returns.push_back({Eigen::Vector3f{10.5f, 3.5f, 0.f}});
243243
range_data.origin.x() = -0.5f;
244244
range_data.origin.y() = -0.5f;
245245
range_data_inserter_->Insert(range_data, &tsdf_);
@@ -264,8 +264,8 @@ TEST_F(RangeDataInserterTest2DTSDF, InsertSmallAnglePointWitNormalProjection) {
264264
options_.set_project_sdf_distance_to_scan_normal(true);
265265
range_data_inserter_ = absl::make_unique<TSDFRangeDataInserter2D>(options_);
266266
sensor::RangeData range_data;
267-
range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
268-
range_data.returns.emplace_back(5.5f, 3.5f, 0.f);
267+
range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
268+
range_data.returns.push_back({Eigen::Vector3f{5.5f, 3.5f, 0.f}});
269269
range_data.origin.x() = -0.5f;
270270
range_data.origin.y() = -0.5f;
271271
range_data_inserter_->Insert(range_data, &tsdf_);
@@ -293,8 +293,8 @@ TEST_F(RangeDataInserterTest2DTSDF,
293293
options_.set_update_weight_angle_scan_normal_to_ray_kernel_bandwith(bandwith);
294294
range_data_inserter_ = absl::make_unique<TSDFRangeDataInserter2D>(options_);
295295
sensor::RangeData range_data;
296-
range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
297-
range_data.returns.emplace_back(5.5f, 3.5f, 0.f);
296+
range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
297+
range_data.returns.push_back({Eigen::Vector3f{5.5f, 3.5f, 0.f}});
298298
range_data.origin.x() = -0.5f;
299299
range_data.origin.y() = -0.5f;
300300
range_data_inserter_->Insert(range_data, &tsdf_);

‎cartographer/mapping/3d/range_data_inserter_3d.cc

+4-4
Original file line numberDiff line numberDiff line change
@@ -30,8 +30,8 @@ void InsertMissesIntoGrid(const std::vector<uint16>& miss_table,
3030
HybridGrid* hybrid_grid,
3131
const int num_free_space_voxels) {
3232
const Eigen::Array3i origin_cell = hybrid_grid->GetCellIndex(origin);
33-
for (const Eigen::Vector3f& hit : returns) {
34-
const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit);
33+
for (const sensor::RangefinderPoint& hit : returns) {
34+
const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit.position);
3535

3636
const Eigen::Array3i delta = hit_cell - origin_cell;
3737
const int num_samples = delta.cwiseAbs().maxCoeff();
@@ -79,8 +79,8 @@ void RangeDataInserter3D::Insert(const sensor::RangeData& range_data,
7979
HybridGrid* hybrid_grid) const {
8080
CHECK_NOTNULL(hybrid_grid);
8181

82-
for (const Eigen::Vector3f& hit : range_data.returns) {
83-
const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit);
82+
for (const sensor::RangefinderPoint& hit : range_data.returns) {
83+
const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit.position);
8484
hybrid_grid->ApplyLookupTable(hit_cell, hit_table_);
8585
}
8686

‎cartographer/mapping/3d/range_data_inserter_3d_test.cc

+4-2
Original file line numberDiff line numberDiff line change
@@ -41,8 +41,10 @@ class RangeDataInserter3DTest : public ::testing::Test {
4141

4242
void InsertPointCloud() {
4343
const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f);
44-
sensor::PointCloud returns = {
45-
{-3.f, -1.f, 4.f}, {-2.f, 0.f, 4.f}, {-1.f, 1.f, 4.f}, {0.f, 2.f, 4.f}};
44+
sensor::PointCloud returns = {{Eigen::Vector3f{-3.f, -1.f, 4.f}},
45+
{Eigen::Vector3f{-2.f, 0.f, 4.f}},
46+
{Eigen::Vector3f{-1.f, 1.f, 4.f}},
47+
{Eigen::Vector3f{0.f, 2.f, 4.f}}};
4648
range_data_inserter_->Insert(sensor::RangeData{origin, returns, {}},
4749
&hybrid_grid_);
4850
}

‎cartographer/mapping/3d/submap_3d.cc

+2-2
Original file line numberDiff line numberDiff line change
@@ -41,8 +41,8 @@ struct PixelData {
4141
sensor::RangeData FilterRangeDataByMaxRange(const sensor::RangeData& range_data,
4242
const float max_range) {
4343
sensor::RangeData result{range_data.origin, {}, {}};
44-
for (const Eigen::Vector3f& hit : range_data.returns) {
45-
if ((hit - range_data.origin).norm() <= max_range) {
44+
for (const sensor::RangefinderPoint& hit : range_data.returns) {
45+
if ((hit.position - range_data.origin).norm() <= max_range) {
4646
result.returns.push_back(hit);
4747
}
4848
}

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

+11-8
Original file line numberDiff line numberDiff line change
@@ -130,10 +130,10 @@ LocalTrajectoryBuilder2D::AddRangeData(
130130

131131
CHECK(!synchronized_data.ranges.empty());
132132
// TODO(gaschler): Check if this can strictly be 0.
133-
CHECK_LE(synchronized_data.ranges.back().point_time[3], 0.f);
133+
CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);
134134
const common::Time time_first_point =
135135
time +
136-
common::FromSeconds(synchronized_data.ranges.front().point_time[3]);
136+
common::FromSeconds(synchronized_data.ranges.front().point_time.time);
137137
if (time_first_point < extrapolator_->GetLastPoseTime()) {
138138
LOG(INFO) << "Extrapolator is still initializing.";
139139
return nullptr;
@@ -143,7 +143,7 @@ LocalTrajectoryBuilder2D::AddRangeData(
143143
range_data_poses.reserve(synchronized_data.ranges.size());
144144
bool warned = false;
145145
for (const auto& range : synchronized_data.ranges) {
146-
common::Time time_point = time + common::FromSeconds(range.point_time[3]);
146+
common::Time time_point = time + common::FromSeconds(range.point_time.time);
147147
if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
148148
if (!warned) {
149149
LOG(ERROR)
@@ -166,20 +166,23 @@ LocalTrajectoryBuilder2D::AddRangeData(
166166
// Drop any returns below the minimum range and convert returns beyond the
167167
// maximum range into misses.
168168
for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
169-
const Eigen::Vector4f& hit = synchronized_data.ranges[i].point_time;
169+
const sensor::TimedRangefinderPoint& hit =
170+
synchronized_data.ranges[i].point_time;
170171
const Eigen::Vector3f origin_in_local =
171172
range_data_poses[i] *
172173
synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
173-
const Eigen::Vector3f hit_in_local = range_data_poses[i] * hit.head<3>();
174-
const Eigen::Vector3f delta = hit_in_local - origin_in_local;
174+
sensor::RangefinderPoint hit_in_local =
175+
range_data_poses[i] * sensor::ToRangefinderPoint(hit);
176+
const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
175177
const float range = delta.norm();
176178
if (range >= options_.min_range()) {
177179
if (range <= options_.max_range()) {
178180
accumulated_range_data_.returns.push_back(hit_in_local);
179181
} else {
180-
accumulated_range_data_.misses.push_back(
182+
hit_in_local.position =
181183
origin_in_local +
182-
options_.missing_data_ray_length() / range * delta);
184+
options_.missing_data_ray_length() / range * delta;
185+
accumulated_range_data_.misses.push_back(hit_in_local);
183186
}
184187
}
185188
}

0 commit comments

Comments
 (0)