Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix raytracing origin #2

Merged
merged 1 commit into from
Apr 26, 2022
Merged

Fix raytracing origin #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion cartographer/io/min_max_range_filtering_points_processor.cc
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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