Skip to content

Commit 4020ebb

Browse files
ValerioMagnagoValerioMagnago
committed
Fix raytracing origin
Adds the laser ray origin to `RangefinderPoint` to enable correct raytracing of range data misses into a 2D probability grid also after scan accumulation. Previously, all rays were casted from their hit point to the tracking frame. This is wrong for any setup where the tracking frame is not the sensor frame and leads to artifacts in the map. Fixes: cartographer-project#947
1 parent b8228ee commit 4020ebb

21 files changed

+145
-114
lines changed

Diff for: ‎cartographer/io/min_max_range_filtering_points_processor.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -43,7 +43,7 @@ void MinMaxRangeFilteringPointsProcessor::Process(
4343
absl::flat_hash_set<int> to_remove;
4444
for (size_t i = 0; i < batch->points.size(); ++i) {
4545
const float range_squared =
46-
(batch->points[i].position - batch->origin).squaredNorm();
46+
(batch->points[i].position - batch->points[i].origin).squaredNorm();
4747
if (!(min_range_squared_ <= range_squared &&
4848
range_squared <= max_range_squared_)) {
4949
to_remove.insert(i);

Diff for: ‎cartographer/io/outlier_removing_points_processor.cc

+3-2
Original file line numberDiff line numberDiff line change
@@ -102,11 +102,12 @@ void OutlierRemovingPointsProcessor::ProcessInPhaseTwo(
102102
// by better ray casting, and also by marking the hits of the current range
103103
// data to be excluded.
104104
for (size_t i = 0; i < batch.points.size(); ++i) {
105-
const Eigen::Vector3f delta = batch.points[i].position - batch.origin;
105+
const Eigen::Vector3f delta =
106+
batch.points[i].position - batch.points[i].origin;
106107
const float length = delta.norm();
107108
for (float x = 0; x < length; x += voxel_size_) {
108109
const Eigen::Array3i index =
109-
voxels_.GetCellIndex(batch.origin + (x / length) * delta);
110+
voxels_.GetCellIndex(batch.points[i].origin + (x / length) * delta);
110111
if (voxels_.value(index).hits > 0) {
111112
++voxels_.mutable_value(index)->rays;
112113
}

Diff for: ‎cartographer/io/probability_grid_points_processor_test.cc

+9-6
Original file line numberDiff line numberDiff line change
@@ -34,12 +34,15 @@ namespace {
3434

3535
std::unique_ptr<PointsBatch> CreatePointsBatch() {
3636
auto points_batch = ::absl::make_unique<PointsBatch>();
37-
points_batch->origin = Eigen::Vector3f(0, 0, 0);
38-
points_batch->points.push_back({Eigen::Vector3f{0.0f, 0.0f, 0.0f}});
39-
points_batch->points.push_back({Eigen::Vector3f{0.0f, 1.0f, 2.0f}});
40-
points_batch->points.push_back({Eigen::Vector3f{1.0f, 2.0f, 4.0f}});
41-
points_batch->points.push_back({Eigen::Vector3f{0.0f, 3.0f, 5.0f}});
42-
points_batch->points.push_back({Eigen::Vector3f{3.0f, 0.0f, 6.0f}});
37+
const auto origin = Eigen::Vector3f(0, 0, 0);
38+
points_batch->origin = origin;
39+
points_batch->points.push_back({Eigen::Vector3f{0.0f, 0.0f, 0.0f}, origin});
40+
points_batch->points.push_back({Eigen::Vector3f{0.0f, 1.0f, 2.0f}, origin});
41+
points_batch->points.push_back({Eigen::Vector3f{1.0f, 2.0f, 4.0f}, origin});
42+
points_batch->points.push_back({Eigen::Vector3f{0.0f, 3.0f, 5.0f}, origin});
43+
points_batch->points.push_back({Eigen::Vector3f{3.0f, 0.0f, 6.0f}, origin});
44+
points_batch->points.push_back({Eigen::Vector3f{3.0f, 0.0f, 6.0f}, origin});
45+
points_batch->points.push_back({Eigen::Vector3f{3.0f, 0.0f, 6.0f}, origin});
4346
return points_batch;
4447
}
4548

Diff for: ‎cartographer/io/vertical_range_filtering_points_processor.cc

+2-1
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,8 @@ void VerticalRangeFilteringPointsProcessor::Process(
4242
std::unique_ptr<PointsBatch> batch) {
4343
absl::flat_hash_set<int> to_remove;
4444
for (size_t i = 0; i < batch->points.size(); ++i) {
45-
const float distance_z = batch->points[i].position.z() - batch->origin.z();
45+
const float distance_z =
46+
batch->points[i].position.z() - batch->points[i].origin.z();
4647
if (!(min_z_ <= distance_z && distance_z <= max_z_) ) {
4748
to_remove.insert(i);
4849
}

Diff for: ‎cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc

+8-5
Original file line numberDiff line numberDiff line change
@@ -39,9 +39,11 @@ void GrowAsNeeded(const sensor::RangeData& range_data,
3939
constexpr float kPadding = 1e-6f;
4040
for (const sensor::RangefinderPoint& hit : range_data.returns) {
4141
bounding_box.extend(hit.position.head<2>());
42+
bounding_box.extend(hit.origin.head<2>());
4243
}
4344
for (const sensor::RangefinderPoint& miss : range_data.misses) {
4445
bounding_box.extend(miss.position.head<2>());
46+
bounding_box.extend(miss.origin.head<2>());
4547
}
4648
probability_grid->GrowLimits(bounding_box.min() -
4749
kPadding * Eigen::Vector2f::Ones());
@@ -61,12 +63,12 @@ void CastRays(const sensor::RangeData& range_data,
6163
superscaled_resolution, limits.max(),
6264
CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
6365
limits.cell_limits().num_y_cells * kSubpixelScale));
64-
const Eigen::Array2i begin =
65-
superscaled_limits.GetCellIndex(range_data.origin.head<2>());
6666
// Compute and add the end points.
6767
std::vector<Eigen::Array2i> ends;
68+
std::vector<Eigen::Array2i> begins;
6869
ends.reserve(range_data.returns.size());
6970
for (const sensor::RangefinderPoint& hit : range_data.returns) {
71+
begins.push_back(superscaled_limits.GetCellIndex(hit.origin.head<2>()));
7072
ends.push_back(superscaled_limits.GetCellIndex(hit.position.head<2>()));
7173
probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
7274
}
@@ -76,9 +78,9 @@ void CastRays(const sensor::RangeData& range_data,
7678
}
7779

7880
// Now add the misses.
79-
for (const Eigen::Array2i& end : ends) {
81+
for (size_t i = 0; i < ends.size(); i++) {
8082
std::vector<Eigen::Array2i> ray =
81-
RayToPixelMask(begin, end, kSubpixelScale);
83+
RayToPixelMask(begins[i], ends[i], kSubpixelScale);
8284
for (const Eigen::Array2i& cell_index : ray) {
8385
probability_grid->ApplyLookupTable(cell_index, miss_table);
8486
}
@@ -87,7 +89,8 @@ void CastRays(const sensor::RangeData& range_data,
8789
// Finally, compute and add empty rays based on misses in the range data.
8890
for (const sensor::RangefinderPoint& missing_echo : range_data.misses) {
8991
std::vector<Eigen::Array2i> ray = RayToPixelMask(
90-
begin, superscaled_limits.GetCellIndex(missing_echo.position.head<2>()),
92+
superscaled_limits.GetCellIndex(missing_echo.origin.head<2>()),
93+
superscaled_limits.GetCellIndex(missing_echo.position.head<2>()),
9194
kSubpixelScale);
9295
for (const Eigen::Array2i& cell_index : ray) {
9396
probability_grid->ApplyLookupTable(cell_index, miss_table);

Diff for: ‎cartographer/mapping/2d/range_data_inserter_2d_test.cc

+6-6
Original file line numberDiff line numberDiff line change
@@ -48,12 +48,12 @@ class RangeDataInserterTest2D : public ::testing::Test {
4848

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

Diff for: ‎cartographer/mapping/3d/range_data_inserter_3d.cc

+3-4
Original file line numberDiff line numberDiff line change
@@ -25,12 +25,11 @@ namespace mapping {
2525
namespace {
2626

2727
void InsertMissesIntoGrid(const std::vector<uint16>& miss_table,
28-
const Eigen::Vector3f& origin,
2928
const sensor::PointCloud& returns,
3029
HybridGrid* hybrid_grid,
3130
const int num_free_space_voxels) {
32-
const Eigen::Array3i origin_cell = hybrid_grid->GetCellIndex(origin);
3331
for (const sensor::RangefinderPoint& hit : returns) {
32+
const Eigen::Array3i origin_cell = hybrid_grid->GetCellIndex(hit.origin);
3433
const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit.position);
3534

3635
const Eigen::Array3i delta = hit_cell - origin_cell;
@@ -104,8 +103,8 @@ void RangeDataInserter3D::Insert(
104103

105104
// By not starting a new update after hits are inserted, we give hits priority
106105
// (i.e. no hits will be ignored because of a miss in the same cell).
107-
InsertMissesIntoGrid(miss_table_, range_data.origin, range_data.returns,
108-
hybrid_grid, options_.num_free_space_voxels());
106+
InsertMissesIntoGrid(miss_table_, range_data.returns, hybrid_grid,
107+
options_.num_free_space_voxels());
109108
if (intensity_hybrid_grid != nullptr) {
110109
InsertIntensitiesIntoGrid(range_data.returns, intensity_hybrid_grid,
111110
options_.intensity_threshold());

Diff for: ‎cartographer/mapping/3d/range_data_inserter_3d_test.cc

+10-8
Original file line numberDiff line numberDiff line change
@@ -43,10 +43,11 @@ class RangeDataInserter3DTest : public ::testing::Test {
4343
void InsertPointCloud() {
4444
const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f);
4545
const std::vector<sensor::RangefinderPoint> returns = {
46-
{Eigen::Vector3f{-3.f, -1.f, 4.f}},
47-
{Eigen::Vector3f{-2.f, 0.f, 4.f}},
48-
{Eigen::Vector3f{-1.f, 1.f, 4.f}},
49-
{Eigen::Vector3f{0.f, 2.f, 4.f}}};
46+
{Eigen::Vector3f{-3.f, -1.f, 4.f}, origin},
47+
{Eigen::Vector3f{-2.f, 0.f, 4.f}, origin},
48+
{Eigen::Vector3f{-1.f, 1.f, 4.f}, origin},
49+
{Eigen::Vector3f{0.f, 2.f, 4.f}, origin},
50+
};
5051
range_data_inserter_->Insert(
5152
sensor::RangeData{origin, sensor::PointCloud(returns), {}},
5253
&hybrid_grid_,
@@ -56,10 +57,11 @@ class RangeDataInserter3DTest : public ::testing::Test {
5657
void InsertPointCloudWithIntensities() {
5758
const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f);
5859
const std::vector<sensor::RangefinderPoint> returns = {
59-
{Eigen::Vector3f{-3.f, -1.f, 4.f}},
60-
{Eigen::Vector3f{-2.f, 0.f, 4.f}},
61-
{Eigen::Vector3f{-1.f, 1.f, 4.f}},
62-
{Eigen::Vector3f{0.f, 2.f, 4.f}}};
60+
{Eigen::Vector3f{-3.f, -1.f, 4.f}, origin},
61+
{Eigen::Vector3f{-2.f, 0.f, 4.f}, origin},
62+
{Eigen::Vector3f{-1.f, 1.f, 4.f}, origin},
63+
{Eigen::Vector3f{0.f, 2.f, 4.f}, origin},
64+
};
6365
const std::vector<float> intensities{7.f, 8.f, 9.f, 10.f};
6466
range_data_inserter_->Insert(
6567
sensor::RangeData{origin, sensor::PointCloud(returns, intensities), {}},

Diff for: ‎cartographer/mapping/3d/submap_3d.cc

+1-1
Original file line numberDiff line numberDiff line change
@@ -43,7 +43,7 @@ sensor::RangeData FilterRangeDataByMaxRange(const sensor::RangeData& range_data,
4343
sensor::RangeData result{range_data.origin, {}, {}};
4444
result.returns =
4545
range_data.returns.copy_if([&](const sensor::RangefinderPoint& point) {
46-
return (point.position - range_data.origin).norm() <= max_range;
46+
return (point.position - point.origin).norm() <= max_range;
4747
});
4848
return result;
4949
}

Diff for: ‎cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

+6-6
Original file line numberDiff line numberDiff line change
@@ -165,19 +165,19 @@ LocalTrajectoryBuilder2D::AddRangeData(
165165
for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
166166
const sensor::TimedRangefinderPoint& hit =
167167
synchronized_data.ranges[i].point_time;
168-
const Eigen::Vector3f origin_in_local =
169-
range_data_poses[i] *
170-
synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
171168
sensor::RangefinderPoint hit_in_local =
172-
range_data_poses[i] * sensor::ToRangefinderPoint(hit);
173-
const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
169+
range_data_poses[i] *
170+
sensor::ToRangefinderPoint(
171+
hit, synchronized_data.origins.at(
172+
synchronized_data.ranges[i].origin_index));
173+
const Eigen::Vector3f delta = hit_in_local.position - hit_in_local.origin;
174174
const float range = delta.norm();
175175
if (range >= options_.min_range()) {
176176
if (range <= options_.max_range()) {
177177
accumulated_range_data_.returns.push_back(hit_in_local);
178178
} else {
179179
hit_in_local.position =
180-
origin_in_local +
180+
hit_in_local.origin +
181181
options_.missing_data_ray_length() / range * delta;
182182
accumulated_range_data_.misses.push_back(hit_in_local);
183183
}

Diff for: ‎cartographer/mapping/internal/2d/normal_estimation_2d.cc

+3-3
Original file line numberDiff line numberDiff line change
@@ -30,10 +30,10 @@ float NormalTo2DAngle(const Eigen::Vector3f& v) {
3030
float EstimateNormal(const sensor::PointCloud& returns,
3131
const size_t estimation_point_index,
3232
const size_t sample_window_begin,
33-
const size_t sample_window_end,
34-
const Eigen::Vector3f& sensor_origin) {
33+
const size_t sample_window_end) {
3534
const Eigen::Vector3f& estimation_point =
3635
returns[estimation_point_index].position;
36+
const Eigen::Vector3f& sensor_origin = returns[estimation_point_index].origin;
3737
if (sample_window_end - sample_window_begin < 2) {
3838
return NormalTo2DAngle(sensor_origin - estimation_point);
3939
}
@@ -102,7 +102,7 @@ std::vector<float> EstimateNormals(
102102
}
103103
const float normal_estimate =
104104
EstimateNormal(range_data.returns, current_point, sample_window_begin,
105-
sample_window_end, range_data.origin);
105+
sample_window_end);
106106
normals.push_back(normal_estimate);
107107
}
108108
return normals;

Diff for: ‎cartographer/mapping/internal/2d/normal_estimation_2d_test.cc

+26-20
Original file line numberDiff line numberDiff line change
@@ -39,15 +39,15 @@ TEST(NormalEstimation2DTest, SinglePoint) {
3939
CreateNormalEstimationOptions2D(parameter_dictionary.get());
4040
auto range_data = sensor::RangeData();
4141
const size_t num_angles = 100;
42-
range_data.origin.x() = 0.f;
43-
range_data.origin.y() = 0.f;
42+
const auto origin = Eigen::Vector3f::Zero();
4443
for (size_t angle_idx = 0; angle_idx < num_angles; ++angle_idx) {
4544
const double angle = static_cast<double>(angle_idx) /
4645
static_cast<double>(num_angles) * 2. * M_PI -
4746
M_PI;
4847
range_data.returns = sensor::PointCloud(
4948
{{Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}
50-
.cast<float>()}});
49+
.cast<float>(),
50+
origin}});
5151
std::vector<float> normals;
5252
normals = EstimateNormals(range_data, options);
5353
EXPECT_NEAR(common::NormalizeAngleDifference(angle - normals[0] - M_PI),
@@ -64,38 +64,43 @@ TEST(NormalEstimation2DTest, StraightLineGeometry) {
6464
const proto::NormalEstimationOptions2D options =
6565
CreateNormalEstimationOptions2D(parameter_dictionary.get());
6666
auto range_data = sensor::RangeData();
67-
range_data.returns.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}});
68-
range_data.returns.push_back({Eigen::Vector3f{0.f, 1.f, 0.f}});
69-
range_data.returns.push_back({Eigen::Vector3f{1.f, 1.f, 0.f}});
70-
range_data.origin.x() = 0.f;
71-
range_data.origin.y() = 0.f;
67+
const auto origin = Eigen::Vector3f::Zero();
68+
range_data.returns.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}, origin});
69+
range_data.returns.push_back({Eigen::Vector3f{0.f, 1.f, 0.f}, origin});
70+
range_data.returns.push_back({Eigen::Vector3f{1.f, 1.f, 0.f}, origin});
7271
std::vector<float> normals;
7372
normals = EstimateNormals(range_data, options);
7473
for (const float normal : normals) {
7574
EXPECT_NEAR(normal, -M_PI_2, 1e-4);
7675
}
7776
normals.clear();
78-
range_data.returns = sensor::PointCloud({{Eigen::Vector3f{1.f, 1.f, 0.f}},
79-
{Eigen::Vector3f{1.f, 0.f, 0.f}},
80-
{Eigen::Vector3f{1.f, -1.f, 0.f}}});
77+
range_data.returns = sensor::PointCloud({
78+
{Eigen::Vector3f{1.f, 1.f, 0.f}, origin},
79+
{Eigen::Vector3f{1.f, 0.f, 0.f}, origin},
80+
{Eigen::Vector3f{1.f, -1.f, 0.f}, origin},
81+
});
8182
normals = EstimateNormals(range_data, options);
8283
for (const float normal : normals) {
8384
EXPECT_NEAR(std::abs(normal), M_PI, 1e-4);
8485
}
8586

8687
normals.clear();
87-
range_data.returns = sensor::PointCloud({{Eigen::Vector3f{1.f, -1.f, 0.f}},
88-
{Eigen::Vector3f{0.f, -1.f, 0.f}},
89-
{Eigen::Vector3f{-1.f, -1.f, 0.f}}});
88+
range_data.returns = sensor::PointCloud({
89+
{Eigen::Vector3f{1.f, -1.f, 0.f}, origin},
90+
{Eigen::Vector3f{0.f, -1.f, 0.f}, origin},
91+
{Eigen::Vector3f{-1.f, -1.f, 0.f}, origin},
92+
});
9093
normals = EstimateNormals(range_data, options);
9194
for (const float normal : normals) {
9295
EXPECT_NEAR(normal, M_PI_2, 1e-4);
9396
}
9497

9598
normals.clear();
96-
range_data.returns = sensor::PointCloud({{Eigen::Vector3f{-1.f, -1.f, 0.f}},
97-
{Eigen::Vector3f{-1.f, 0.f, 0.f}},
98-
{Eigen::Vector3f{-1.f, 1.f, 0.f}}});
99+
range_data.returns = sensor::PointCloud({
100+
{Eigen::Vector3f{-1.f, -1.f, 0.f}, origin},
101+
{Eigen::Vector3f{-1.f, 0.f, 0.f}, origin},
102+
{Eigen::Vector3f{-1.f, 1.f, 0.f}, origin},
103+
});
99104
normals = EstimateNormals(range_data, options);
100105
for (const float normal : normals) {
101106
EXPECT_NEAR(normal, 0, 1e-4);
@@ -115,16 +120,17 @@ TEST_P(CircularGeometry2DTest, NumSamplesPerNormal) {
115120
const proto::NormalEstimationOptions2D options =
116121
CreateNormalEstimationOptions2D(parameter_dictionary.get());
117122
auto range_data = sensor::RangeData();
123+
const auto origin = Eigen::Vector3f::Zero();
118124
const size_t num_angles = 100;
119125
for (size_t angle_idx = 0; angle_idx < num_angles; ++angle_idx) {
120126
const double angle = static_cast<double>(angle_idx) /
121127
static_cast<double>(num_angles) * 2. * M_PI -
122128
M_PI;
123129
range_data.returns.push_back(
124-
{Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}.cast<float>()});
130+
{Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}.cast<float>(),
131+
origin});
125132
}
126-
range_data.origin.x() = 0.f;
127-
range_data.origin.y() = 0.f;
133+
range_data.origin = origin;
128134
std::vector<float> normals;
129135
normals = EstimateNormals(range_data, options);
130136
for (size_t angle_idx = 0; angle_idx < num_angles; ++angle_idx) {

Diff for: ‎cartographer/mapping/internal/2d/pose_graph_2d_test.cc

+3-3
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,8 @@ class PoseGraph2DTest : public ::testing::Test {
4545
for (float t = 0.f; t < 2.f * M_PI; t += 0.005f) {
4646
const float r = (std::sin(20.f * t) + 2.f) * std::sin(t + 2.f);
4747
point_cloud_.push_back(
48-
{Eigen::Vector3f{r * std::sin(t), r * std::cos(t), 0.f}});
48+
{Eigen::Vector3f{r * std::sin(t), r * std::cos(t), 0.f},
49+
Eigen::Vector3f{0.f, 0.f, 0.f}});
4950
}
5051

5152
{
@@ -176,8 +177,7 @@ class PoseGraph2DTest : public ::testing::Test {
176177
const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud(
177178
point_cloud_,
178179
transform::Embed3D(current_pose_.inverse().cast<float>()));
179-
const sensor::RangeData range_data{
180-
Eigen::Vector3f::Zero(), new_point_cloud, {}};
180+
const sensor::RangeData range_data{new_point_cloud.begin()->origin, new_point_cloud, {}};
181181
const transform::Rigid2d pose_estimate = noise * current_pose_;
182182
constexpr int kTrajectoryId = 0;
183183
active_submaps_->InsertRangeData(TransformRangeData(

Diff for: ‎cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc

+10-9
Original file line numberDiff line numberDiff line change
@@ -52,13 +52,14 @@ CreateRealTimeCorrelativeScanMatcherTestOptions2D() {
5252
class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
5353
protected:
5454
RealTimeCorrelativeScanMatcherTest() {
55-
point_cloud_.push_back({Eigen::Vector3f{0.025f, 0.175f, 0.f}});
56-
point_cloud_.push_back({Eigen::Vector3f{-0.025f, 0.175f, 0.f}});
57-
point_cloud_.push_back({Eigen::Vector3f{-0.075f, 0.175f, 0.f}});
58-
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.175f, 0.f}});
59-
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.125f, 0.f}});
60-
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.075f, 0.f}});
61-
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.025f, 0.f}});
55+
Eigen::Vector3f origin(0.5f, -0.5f, 0.f);
56+
point_cloud_.push_back({Eigen::Vector3f{0.025f, 0.175f, 0.f}, origin});
57+
point_cloud_.push_back({Eigen::Vector3f{-0.025f, 0.175f, 0.f}, origin});
58+
point_cloud_.push_back({Eigen::Vector3f{-0.075f, 0.175f, 0.f}, origin});
59+
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.175f, 0.f}, origin});
60+
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.125f, 0.f}, origin});
61+
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.075f, 0.f}, origin});
62+
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.025f, 0.f}, origin});
6263
real_time_correlative_scan_matcher_ =
6364
absl::make_unique<RealTimeCorrelativeScanMatcher2D>(
6465
CreateRealTimeCorrelativeScanMatcherTestOptions2D());
@@ -87,7 +88,7 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
8788
CreateTSDFRangeDataInserterOptions2D(parameter_dictionary.get()));
8889
}
8990
range_data_inserter_->Insert(
90-
sensor::RangeData{Eigen::Vector3f(0.5f, -0.5f, 0.f), point_cloud_, {}},
91+
sensor::RangeData{point_cloud_.begin()->origin, point_cloud_, {}},
9192
grid_.get());
9293
grid_->FinishUpdate();
9394
}
@@ -109,7 +110,7 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
109110
parameter_dictionary.get()));
110111
}
111112
range_data_inserter_->Insert(
112-
sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}},
113+
sensor::RangeData{point_cloud_.begin()->origin, point_cloud_, {}},
113114
grid_.get());
114115
grid_->FinishUpdate();
115116
}

0 commit comments

Comments
 (0)