Skip to content

Commit

Permalink
Fix raytracing origin
Browse files Browse the repository at this point in the history 
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: #947
Signed-off-by: Valerio Magnago <[email protected]>
  • Loading branch information
@ValerioMagnago
ValerioMagnago committed Aug 13, 2021
1 parent b8228ee commit 07c7d28
Show file tree
Hide file tree
Showing 21 changed files with 145 additions and 114 deletions.
2 changes: 1 addition & 1 deletion cartographer/io/min_max_range_filtering_points_processor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ void MinMaxRangeFilteringPointsProcessor::Process(
absl::flat_hash_set<int> to_remove;
for (size_t i = 0; i < batch->points.size(); ++i) {
const float range_squared =
(batch->points[i].position - batch->origin).squaredNorm();
(batch->points[i].position - batch->points[i].origin).squaredNorm();
if (!(min_range_squared_ <= range_squared &&
range_squared <= max_range_squared_)) {
to_remove.insert(i);
Expand Down
5 changes: 3 additions & 2 deletions cartographer/io/outlier_removing_points_processor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,11 +102,12 @@ void OutlierRemovingPointsProcessor::ProcessInPhaseTwo(
// by better ray casting, and also by marking the hits of the current range
// data to be excluded.
for (size_t i = 0; i < batch.points.size(); ++i) {
const Eigen::Vector3f delta = batch.points[i].position - batch.origin;
const Eigen::Vector3f delta =
batch.points[i].position - batch.points[i].origin;
const float length = delta.norm();
for (float x = 0; x < length; x += voxel_size_) {
const Eigen::Array3i index =
voxels_.GetCellIndex(batch.origin + (x / length) * delta);
voxels_.GetCellIndex(batch.points[i].origin + (x / length) * delta);
if (voxels_.value(index).hits > 0) {
++voxels_.mutable_value(index)->rays;
}
Expand Down
15 changes: 9 additions & 6 deletions cartographer/io/probability_grid_points_processor_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,12 +34,15 @@ namespace {

std::unique_ptr<PointsBatch> CreatePointsBatch() {
auto points_batch = ::absl::make_unique<PointsBatch>();
points_batch->origin = Eigen::Vector3f(0, 0, 0);
points_batch->points.push_back({Eigen::Vector3f{0.0f, 0.0f, 0.0f}});
points_batch->points.push_back({Eigen::Vector3f{0.0f, 1.0f, 2.0f}});
points_batch->points.push_back({Eigen::Vector3f{1.0f, 2.0f, 4.0f}});
points_batch->points.push_back({Eigen::Vector3f{0.0f, 3.0f, 5.0f}});
points_batch->points.push_back({Eigen::Vector3f{3.0f, 0.0f, 6.0f}});
const auto origin = Eigen::Vector3f(0, 0, 0);
points_batch->origin = origin;
points_batch->points.push_back({Eigen::Vector3f{0.0f, 0.0f, 0.0f}, origin});
points_batch->points.push_back({Eigen::Vector3f{0.0f, 1.0f, 2.0f}, origin});
points_batch->points.push_back({Eigen::Vector3f{1.0f, 2.0f, 4.0f}, origin});
points_batch->points.push_back({Eigen::Vector3f{0.0f, 3.0f, 5.0f}, origin});
points_batch->points.push_back({Eigen::Vector3f{3.0f, 0.0f, 6.0f}, origin});
points_batch->points.push_back({Eigen::Vector3f{3.0f, 0.0f, 6.0f}, origin});
points_batch->points.push_back({Eigen::Vector3f{3.0f, 0.0f, 6.0f}, origin});
return points_batch;
}

Expand Down
3 changes: 2 additions & 1 deletion cartographer/io/vertical_range_filtering_points_processor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,8 @@ void VerticalRangeFilteringPointsProcessor::Process(
std::unique_ptr<PointsBatch> batch) {
absl::flat_hash_set<int> to_remove;
for (size_t i = 0; i < batch->points.size(); ++i) {
const float distance_z = batch->points[i].position.z() - batch->origin.z();
const float distance_z =
batch->points[i].position.z() - batch->points[i].origin.z();
if (!(min_z_ <= distance_z && distance_z <= max_z_) ) {
to_remove.insert(i);
}
Expand Down
13 changes: 8 additions & 5 deletions cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,9 +39,11 @@ void GrowAsNeeded(const sensor::RangeData& range_data,
constexpr float kPadding = 1e-6f;
for (const sensor::RangefinderPoint& hit : range_data.returns) {
bounding_box.extend(hit.position.head<2>());
bounding_box.extend(hit.origin.head<2>());
}
for (const sensor::RangefinderPoint& miss : range_data.misses) {
bounding_box.extend(miss.position.head<2>());
bounding_box.extend(miss.origin.head<2>());
}
probability_grid->GrowLimits(bounding_box.min() -
kPadding * Eigen::Vector2f::Ones());
Expand All @@ -61,12 +63,12 @@ void CastRays(const sensor::RangeData& range_data,
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;
std::vector<Eigen::Array2i> begins;
ends.reserve(range_data.returns.size());
for (const sensor::RangefinderPoint& hit : range_data.returns) {
begins.push_back(superscaled_limits.GetCellIndex(hit.origin.head<2>()));
ends.push_back(superscaled_limits.GetCellIndex(hit.position.head<2>()));
probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
}
Expand All @@ -76,9 +78,9 @@ void CastRays(const sensor::RangeData& range_data,
}

// Now add the misses.
for (const Eigen::Array2i& end : ends) {
for (size_t i = 0; i < ends.size(); i++) {
std::vector<Eigen::Array2i> ray =
RayToPixelMask(begin, end, kSubpixelScale);
RayToPixelMask(begins[i], ends[i], kSubpixelScale);
for (const Eigen::Array2i& cell_index : ray) {
probability_grid->ApplyLookupTable(cell_index, miss_table);
}
Expand All @@ -87,7 +89,8 @@ void CastRays(const sensor::RangeData& range_data,
// Finally, compute and add empty rays based on misses in the range data.
for (const sensor::RangefinderPoint& missing_echo : range_data.misses) {
std::vector<Eigen::Array2i> ray = RayToPixelMask(
begin, superscaled_limits.GetCellIndex(missing_echo.position.head<2>()),
superscaled_limits.GetCellIndex(missing_echo.origin.head<2>()),
superscaled_limits.GetCellIndex(missing_echo.position.head<2>()),
kSubpixelScale);
for (const Eigen::Array2i& cell_index : ray) {
probability_grid->ApplyLookupTable(cell_index, miss_table);
Expand Down
12 changes: 6 additions & 6 deletions cartographer/mapping/2d/range_data_inserter_2d_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,12 +48,12 @@ class RangeDataInserterTest2D : public ::testing::Test {

void InsertPointCloud() {
sensor::RangeData range_data;
range_data.returns.push_back({Eigen::Vector3f{-3.5f, 0.5f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{-2.5f, 1.5f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{-1.5f, 2.5f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}});
range_data.origin.x() = -0.5f;
range_data.origin.y() = 0.5f;
const Eigen::Vector3f origin(-0.5f, 0.5f, 0.f);
range_data.origin = origin;
range_data.returns.push_back({Eigen::Vector3f{-3.5f, 0.5f, 0.f}, origin});
range_data.returns.push_back({Eigen::Vector3f{-2.5f, 1.5f, 0.f}, origin});
range_data.returns.push_back({Eigen::Vector3f{-1.5f, 2.5f, 0.f}, origin});
range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}, origin});
range_data_inserter_->Insert(range_data, &probability_grid_);
probability_grid_.FinishUpdate();
}
Expand Down
7 changes: 3 additions & 4 deletions cartographer/mapping/3d/range_data_inserter_3d.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,12 +25,11 @@ namespace mapping {
namespace {

void InsertMissesIntoGrid(const std::vector<uint16>& miss_table,
const Eigen::Vector3f& origin,
const sensor::PointCloud& returns,
HybridGrid* hybrid_grid,
const int num_free_space_voxels) {
const Eigen::Array3i origin_cell = hybrid_grid->GetCellIndex(origin);
for (const sensor::RangefinderPoint& hit : returns) {
const Eigen::Array3i origin_cell = hybrid_grid->GetCellIndex(hit.origin);
const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit.position);

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

// By not starting a new update after hits are inserted, we give hits priority
// (i.e. no hits will be ignored because of a miss in the same cell).
InsertMissesIntoGrid(miss_table_, range_data.origin, range_data.returns,
hybrid_grid, options_.num_free_space_voxels());
InsertMissesIntoGrid(miss_table_, range_data.returns, hybrid_grid,
options_.num_free_space_voxels());
if (intensity_hybrid_grid != nullptr) {
InsertIntensitiesIntoGrid(range_data.returns, intensity_hybrid_grid,
options_.intensity_threshold());
Expand Down
18 changes: 10 additions & 8 deletions cartographer/mapping/3d/range_data_inserter_3d_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,10 +43,11 @@ class RangeDataInserter3DTest : public ::testing::Test {
void InsertPointCloud() {
const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f);
const std::vector<sensor::RangefinderPoint> returns = {
{Eigen::Vector3f{-3.f, -1.f, 4.f}},
{Eigen::Vector3f{-2.f, 0.f, 4.f}},
{Eigen::Vector3f{-1.f, 1.f, 4.f}},
{Eigen::Vector3f{0.f, 2.f, 4.f}}};
{Eigen::Vector3f{-3.f, -1.f, 4.f}, origin},
{Eigen::Vector3f{-2.f, 0.f, 4.f}, origin},
{Eigen::Vector3f{-1.f, 1.f, 4.f}, origin},
{Eigen::Vector3f{0.f, 2.f, 4.f}, origin},
};
range_data_inserter_->Insert(
sensor::RangeData{origin, sensor::PointCloud(returns), {}},
&hybrid_grid_,
Expand All @@ -56,10 +57,11 @@ class RangeDataInserter3DTest : public ::testing::Test {
void InsertPointCloudWithIntensities() {
const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f);
const std::vector<sensor::RangefinderPoint> returns = {
{Eigen::Vector3f{-3.f, -1.f, 4.f}},
{Eigen::Vector3f{-2.f, 0.f, 4.f}},
{Eigen::Vector3f{-1.f, 1.f, 4.f}},
{Eigen::Vector3f{0.f, 2.f, 4.f}}};
{Eigen::Vector3f{-3.f, -1.f, 4.f}, origin},
{Eigen::Vector3f{-2.f, 0.f, 4.f}, origin},
{Eigen::Vector3f{-1.f, 1.f, 4.f}, origin},
{Eigen::Vector3f{0.f, 2.f, 4.f}, origin},
};
const std::vector<float> intensities{7.f, 8.f, 9.f, 10.f};
range_data_inserter_->Insert(
sensor::RangeData{origin, sensor::PointCloud(returns, intensities), {}},
Expand Down
2 changes: 1 addition & 1 deletion cartographer/mapping/3d/submap_3d.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ sensor::RangeData FilterRangeDataByMaxRange(const sensor::RangeData& range_data,
sensor::RangeData result{range_data.origin, {}, {}};
result.returns =
range_data.returns.copy_if([&](const sensor::RangefinderPoint& point) {
return (point.position - range_data.origin).norm() <= max_range;
return (point.position - point.origin).norm() <= max_range;
});
return result;
}
Expand Down
12 changes: 6 additions & 6 deletions cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -165,19 +165,19 @@ LocalTrajectoryBuilder2D::AddRangeData(
for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
const sensor::TimedRangefinderPoint& hit =
synchronized_data.ranges[i].point_time;
const Eigen::Vector3f origin_in_local =
range_data_poses[i] *
synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
sensor::RangefinderPoint hit_in_local =
range_data_poses[i] * sensor::ToRangefinderPoint(hit);
const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
range_data_poses[i] *
sensor::ToRangefinderPoint(
hit, synchronized_data.origins.at(
synchronized_data.ranges[i].origin_index));
const Eigen::Vector3f delta = hit_in_local.position - hit_in_local.origin;
const float range = delta.norm();
if (range >= options_.min_range()) {
if (range <= options_.max_range()) {
accumulated_range_data_.returns.push_back(hit_in_local);
} else {
hit_in_local.position =
origin_in_local +
hit_in_local.origin +
options_.missing_data_ray_length() / range * delta;
accumulated_range_data_.misses.push_back(hit_in_local);
}
Expand Down
6 changes: 3 additions & 3 deletions cartographer/mapping/internal/2d/normal_estimation_2d.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,10 +30,10 @@ float NormalTo2DAngle(const Eigen::Vector3f& v) {
float EstimateNormal(const sensor::PointCloud& returns,
const size_t estimation_point_index,
const size_t sample_window_begin,
const size_t sample_window_end,
const Eigen::Vector3f& sensor_origin) {
const size_t sample_window_end) {
const Eigen::Vector3f& estimation_point =
returns[estimation_point_index].position;
const Eigen::Vector3f& sensor_origin = returns[estimation_point_index].origin;
if (sample_window_end - sample_window_begin < 2) {
return NormalTo2DAngle(sensor_origin - estimation_point);
}
Expand Down Expand Up @@ -102,7 +102,7 @@ std::vector<float> EstimateNormals(
}
const float normal_estimate =
EstimateNormal(range_data.returns, current_point, sample_window_begin,
sample_window_end, range_data.origin);
sample_window_end);
normals.push_back(normal_estimate);
}
return normals;
Expand Down
46 changes: 26 additions & 20 deletions cartographer/mapping/internal/2d/normal_estimation_2d_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,15 +39,15 @@ TEST(NormalEstimation2DTest, SinglePoint) {
CreateNormalEstimationOptions2D(parameter_dictionary.get());
auto range_data = sensor::RangeData();
const size_t num_angles = 100;
range_data.origin.x() = 0.f;
range_data.origin.y() = 0.f;
const auto origin = Eigen::Vector3f::Zero();
for (size_t angle_idx = 0; angle_idx < num_angles; ++angle_idx) {
const double angle = static_cast<double>(angle_idx) /
static_cast<double>(num_angles) * 2. * M_PI -
M_PI;
range_data.returns = sensor::PointCloud(
{{Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}
.cast<float>()}});
.cast<float>(),
origin}});
std::vector<float> normals;
normals = EstimateNormals(range_data, options);
EXPECT_NEAR(common::NormalizeAngleDifference(angle - normals[0] - M_PI),
Expand All @@ -64,38 +64,43 @@ TEST(NormalEstimation2DTest, StraightLineGeometry) {
const proto::NormalEstimationOptions2D options =
CreateNormalEstimationOptions2D(parameter_dictionary.get());
auto range_data = sensor::RangeData();
range_data.returns.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{0.f, 1.f, 0.f}});
range_data.returns.push_back({Eigen::Vector3f{1.f, 1.f, 0.f}});
range_data.origin.x() = 0.f;
range_data.origin.y() = 0.f;
const auto origin = Eigen::Vector3f::Zero();
range_data.returns.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}, origin});
range_data.returns.push_back({Eigen::Vector3f{0.f, 1.f, 0.f}, origin});
range_data.returns.push_back({Eigen::Vector3f{1.f, 1.f, 0.f}, origin});
std::vector<float> normals;
normals = EstimateNormals(range_data, options);
for (const float normal : normals) {
EXPECT_NEAR(normal, -M_PI_2, 1e-4);
}
normals.clear();
range_data.returns = sensor::PointCloud({{Eigen::Vector3f{1.f, 1.f, 0.f}},
{Eigen::Vector3f{1.f, 0.f, 0.f}},
{Eigen::Vector3f{1.f, -1.f, 0.f}}});
range_data.returns = sensor::PointCloud({
{Eigen::Vector3f{1.f, 1.f, 0.f}, origin},
{Eigen::Vector3f{1.f, 0.f, 0.f}, origin},
{Eigen::Vector3f{1.f, -1.f, 0.f}, origin},
});
normals = EstimateNormals(range_data, options);
for (const float normal : normals) {
EXPECT_NEAR(std::abs(normal), M_PI, 1e-4);
}

normals.clear();
range_data.returns = sensor::PointCloud({{Eigen::Vector3f{1.f, -1.f, 0.f}},
{Eigen::Vector3f{0.f, -1.f, 0.f}},
{Eigen::Vector3f{-1.f, -1.f, 0.f}}});
range_data.returns = sensor::PointCloud({
{Eigen::Vector3f{1.f, -1.f, 0.f}, origin},
{Eigen::Vector3f{0.f, -1.f, 0.f}, origin},
{Eigen::Vector3f{-1.f, -1.f, 0.f}, origin},
});
normals = EstimateNormals(range_data, options);
for (const float normal : normals) {
EXPECT_NEAR(normal, M_PI_2, 1e-4);
}

normals.clear();
range_data.returns = sensor::PointCloud({{Eigen::Vector3f{-1.f, -1.f, 0.f}},
{Eigen::Vector3f{-1.f, 0.f, 0.f}},
{Eigen::Vector3f{-1.f, 1.f, 0.f}}});
range_data.returns = sensor::PointCloud({
{Eigen::Vector3f{-1.f, -1.f, 0.f}, origin},
{Eigen::Vector3f{-1.f, 0.f, 0.f}, origin},
{Eigen::Vector3f{-1.f, 1.f, 0.f}, origin},
});
normals = EstimateNormals(range_data, options);
for (const float normal : normals) {
EXPECT_NEAR(normal, 0, 1e-4);
Expand All @@ -115,16 +120,17 @@ TEST_P(CircularGeometry2DTest, NumSamplesPerNormal) {
const proto::NormalEstimationOptions2D options =
CreateNormalEstimationOptions2D(parameter_dictionary.get());
auto range_data = sensor::RangeData();
const auto origin = Eigen::Vector3f::Zero();
const size_t num_angles = 100;
for (size_t angle_idx = 0; angle_idx < num_angles; ++angle_idx) {
const double angle = static_cast<double>(angle_idx) /
static_cast<double>(num_angles) * 2. * M_PI -
M_PI;
range_data.returns.push_back(
{Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}.cast<float>()});
{Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}.cast<float>(),
origin});
}
range_data.origin.x() = 0.f;
range_data.origin.y() = 0.f;
range_data.origin = origin;
std::vector<float> normals;
normals = EstimateNormals(range_data, options);
for (size_t angle_idx = 0; angle_idx < num_angles; ++angle_idx) {
Expand Down
6 changes: 3 additions & 3 deletions cartographer/mapping/internal/2d/pose_graph_2d_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,8 @@ class PoseGraph2DTest : public ::testing::Test {
for (float t = 0.f; t < 2.f * M_PI; t += 0.005f) {
const float r = (std::sin(20.f * t) + 2.f) * std::sin(t + 2.f);
point_cloud_.push_back(
{Eigen::Vector3f{r * std::sin(t), r * std::cos(t), 0.f}});
{Eigen::Vector3f{r * std::sin(t), r * std::cos(t), 0.f},
Eigen::Vector3f{0.f, 0.f, 0.f}});
}

{
Expand Down Expand Up @@ -176,8 +177,7 @@ class PoseGraph2DTest : public ::testing::Test {
const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud(
point_cloud_,
transform::Embed3D(current_pose_.inverse().cast<float>()));
const sensor::RangeData range_data{
Eigen::Vector3f::Zero(), new_point_cloud, {}};
const sensor::RangeData range_data{new_point_cloud.begin()->origin, new_point_cloud, {}};
const transform::Rigid2d pose_estimate = noise * current_pose_;
constexpr int kTrajectoryId = 0;
active_submaps_->InsertRangeData(TransformRangeData(
Expand Down
19 changes: 10 additions & 9 deletions cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,13 +52,14 @@ CreateRealTimeCorrelativeScanMatcherTestOptions2D() {
class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
protected:
RealTimeCorrelativeScanMatcherTest() {
point_cloud_.push_back({Eigen::Vector3f{0.025f, 0.175f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.025f, 0.175f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.075f, 0.175f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.175f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.125f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.075f, 0.f}});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.025f, 0.f}});
Eigen::Vector3f origin(0.5f, -0.5f, 0.f);
point_cloud_.push_back({Eigen::Vector3f{0.025f, 0.175f, 0.f}, origin});
point_cloud_.push_back({Eigen::Vector3f{-0.025f, 0.175f, 0.f}, origin});
point_cloud_.push_back({Eigen::Vector3f{-0.075f, 0.175f, 0.f}, origin});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.175f, 0.f}, origin});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.125f, 0.f}, origin});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.075f, 0.f}, origin});
point_cloud_.push_back({Eigen::Vector3f{-0.125f, 0.025f, 0.f}, origin});
real_time_correlative_scan_matcher_ =
absl::make_unique<RealTimeCorrelativeScanMatcher2D>(
CreateRealTimeCorrelativeScanMatcherTestOptions2D());
Expand Down Expand Up @@ -87,7 +88,7 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
CreateTSDFRangeDataInserterOptions2D(parameter_dictionary.get()));
}
range_data_inserter_->Insert(
sensor::RangeData{Eigen::Vector3f(0.5f, -0.5f, 0.f), point_cloud_, {}},
sensor::RangeData{point_cloud_.begin()->origin, point_cloud_, {}},
grid_.get());
grid_->FinishUpdate();
}
Expand All @@ -109,7 +110,7 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
parameter_dictionary.get()));
}
range_data_inserter_->Insert(
sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}},
sensor::RangeData{point_cloud_.begin()->origin, point_cloud_, {}},
grid_.get());
grid_->FinishUpdate();
}
Expand Down
Loading

0 comments on commit 07c7d28

Please sign in to comment.