diff --git a/cartographer/io/min_max_range_filtering_points_processor.cc b/cartographer/io/min_max_range_filtering_points_processor.cc index ba5fc79878..f779abc7b8 100644 --- a/cartographer/io/min_max_range_filtering_points_processor.cc +++ b/cartographer/io/min_max_range_filtering_points_processor.cc @@ -43,7 +43,7 @@ void MinMaxRangeFilteringPointsProcessor::Process( absl::flat_hash_set 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); diff --git a/cartographer/io/outlier_removing_points_processor.cc b/cartographer/io/outlier_removing_points_processor.cc index 585c36fe21..6c08603224 100644 --- a/cartographer/io/outlier_removing_points_processor.cc +++ b/cartographer/io/outlier_removing_points_processor.cc @@ -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; } diff --git a/cartographer/io/probability_grid_points_processor_test.cc b/cartographer/io/probability_grid_points_processor_test.cc index 1af0a25b94..4700488d10 100644 --- a/cartographer/io/probability_grid_points_processor_test.cc +++ b/cartographer/io/probability_grid_points_processor_test.cc @@ -34,12 +34,15 @@ namespace { std::unique_ptr CreatePointsBatch() { auto points_batch = ::absl::make_unique(); - 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; } diff --git a/cartographer/io/vertical_range_filtering_points_processor.cc b/cartographer/io/vertical_range_filtering_points_processor.cc index 325d359cc6..da4a442b3e 100644 --- a/cartographer/io/vertical_range_filtering_points_processor.cc +++ b/cartographer/io/vertical_range_filtering_points_processor.cc @@ -42,7 +42,8 @@ void VerticalRangeFilteringPointsProcessor::Process( std::unique_ptr batch) { absl::flat_hash_set 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); } diff --git a/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc b/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc index 511cb49e39..d401f326bf 100644 --- a/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc +++ b/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc @@ -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()); @@ -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 ends; + std::vector 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); } @@ -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 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); } @@ -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 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); diff --git a/cartographer/mapping/2d/range_data_inserter_2d_test.cc b/cartographer/mapping/2d/range_data_inserter_2d_test.cc index 6b182aa57f..0ca7138cf7 100644 --- a/cartographer/mapping/2d/range_data_inserter_2d_test.cc +++ b/cartographer/mapping/2d/range_data_inserter_2d_test.cc @@ -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(); } diff --git a/cartographer/mapping/3d/range_data_inserter_3d.cc b/cartographer/mapping/3d/range_data_inserter_3d.cc index 555323755a..5c0c95a876 100644 --- a/cartographer/mapping/3d/range_data_inserter_3d.cc +++ b/cartographer/mapping/3d/range_data_inserter_3d.cc @@ -25,12 +25,11 @@ namespace mapping { namespace { void InsertMissesIntoGrid(const std::vector& 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; @@ -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()); diff --git a/cartographer/mapping/3d/range_data_inserter_3d_test.cc b/cartographer/mapping/3d/range_data_inserter_3d_test.cc index a7f92b31c7..116f492ff3 100644 --- a/cartographer/mapping/3d/range_data_inserter_3d_test.cc +++ b/cartographer/mapping/3d/range_data_inserter_3d_test.cc @@ -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 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_, @@ -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 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 intensities{7.f, 8.f, 9.f, 10.f}; range_data_inserter_->Insert( sensor::RangeData{origin, sensor::PointCloud(returns, intensities), {}}, diff --git a/cartographer/mapping/3d/submap_3d.cc b/cartographer/mapping/3d/submap_3d.cc index 9facbe4a5a..cf0625d253 100644 --- a/cartographer/mapping/3d/submap_3d.cc +++ b/cartographer/mapping/3d/submap_3d.cc @@ -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; } diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc index 5bda7938a5..86e6a8ff78 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc @@ -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); } diff --git a/cartographer/mapping/internal/2d/normal_estimation_2d.cc b/cartographer/mapping/internal/2d/normal_estimation_2d.cc index a7d9542107..4eeed25921 100644 --- a/cartographer/mapping/internal/2d/normal_estimation_2d.cc +++ b/cartographer/mapping/internal/2d/normal_estimation_2d.cc @@ -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); } @@ -102,7 +102,7 @@ std::vector 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; diff --git a/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc b/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc index 336d31dde6..ca9987b20a 100644 --- a/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc +++ b/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc @@ -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(angle_idx) / static_cast(num_angles) * 2. * M_PI - M_PI; range_data.returns = sensor::PointCloud( {{Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.} - .cast()}}); + .cast(), + origin}}); std::vector normals; normals = EstimateNormals(range_data, options); EXPECT_NEAR(common::NormalizeAngleDifference(angle - normals[0] - M_PI), @@ -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 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); @@ -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(angle_idx) / static_cast(num_angles) * 2. * M_PI - M_PI; range_data.returns.push_back( - {Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}.cast()}); + {Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}.cast(), + origin}); } - range_data.origin.x() = 0.f; - range_data.origin.y() = 0.f; + range_data.origin = origin; std::vector normals; normals = EstimateNormals(range_data, options); for (size_t angle_idx = 0; angle_idx < num_angles; ++angle_idx) { diff --git a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc index 2ecdfd12fe..39790b4fd0 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc @@ -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}}); } { @@ -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())); - 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( diff --git a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc index 71fb0a9604..c8bc870005 100644 --- a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc @@ -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( CreateRealTimeCorrelativeScanMatcherTestOptions2D()); @@ -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(); } @@ -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(); } diff --git a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc index 8c9cf04813..58d966c83b 100644 --- a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc @@ -56,11 +56,11 @@ class TSDFSpaceCostFunction2DTest : public ::testing::Test { void InsertPointcloud() { auto range_data = sensor::RangeData(); + const Eigen::Vector3f origin(-0.5f, -0.5f, 0.f); for (float x = -.5; x < 0.5f; x += 0.1) { - range_data.returns.push_back({Eigen::Vector3f{x, 1.0f, 0.f}}); + range_data.returns.push_back({Eigen::Vector3f{x, 1.0f, 0.f}, origin}); } - range_data.origin.x() = -0.5f; - range_data.origin.y() = -0.5f; + range_data.origin = origin; range_data_inserter_->Insert(range_data, &tsdf_); tsdf_.FinishUpdate(); } diff --git a/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.cc b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.cc index 7e6bd3f1a2..0fa920ac21 100644 --- a/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.cc +++ b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.cc @@ -32,13 +32,15 @@ const float kSqrtTwoPi = std::sqrt(2.0 * M_PI); void GrowAsNeeded(const sensor::RangeData& range_data, const float truncation_distance, TSDF2D* const tsdf) { - Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>()); + Eigen::AlignedBox2f bounding_box; for (const sensor::RangefinderPoint& hit : range_data.returns) { - const Eigen::Vector3f direction = - (hit.position - range_data.origin).normalized(); + const Eigen::Vector3f direction = (hit.position - hit.origin).normalized(); const Eigen::Vector3f end_position = hit.position + truncation_distance * direction; + const Eigen::Vector3f start_position = + hit.origin - truncation_distance * direction; bounding_box.extend(end_position.head<2>()); + bounding_box.extend(start_position.head<2>()); } // Padding around bounding box to avoid numerical issues at cell boundaries. constexpr float kPadding = 1e-6f; @@ -151,9 +153,10 @@ void TSDFRangeDataInserter2D::Insert(const sensor::RangeData& range_data, options_.normal_estimation_options()); } - const Eigen::Vector2f origin = sorted_range_data.origin.head<2>(); for (size_t hit_index = 0; hit_index < sorted_range_data.returns.size(); ++hit_index) { + const Eigen::Vector2f origin = + sorted_range_data.returns[hit_index].origin.head<2>(); const Eigen::Vector2f hit = sorted_range_data.returns[hit_index].position.head<2>(); const float normal = normals.empty() diff --git a/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc index 0850e6ecf7..ccd9eb7dff 100644 --- a/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc +++ b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc @@ -49,9 +49,9 @@ class RangeDataInserterTest2DTSDF : public ::testing::Test { void InsertPoint() { auto range_data = sensor::RangeData(); - 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; + range_data.origin = Eigen::Vector3f{-0.5f, -0.5f, 0.f}; + range_data.returns.push_back( + {Eigen::Vector3f{-0.5f, 3.5f, 0.f}, range_data.origin}); range_data_inserter_->Insert(range_data, &tsdf_); tsdf_.FinishUpdate(); } @@ -237,11 +237,13 @@ TEST_F(RangeDataInserterTest2DTSDF, InsertPointQuadraticWeight) { TEST_F(RangeDataInserterTest2DTSDF, InsertSmallAnglePointWithoutNormalProjection) { auto range_data = sensor::RangeData(); - range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}}); - range_data.returns.push_back({Eigen::Vector3f{5.5f, 3.5f, 0.f}}); - range_data.returns.push_back({Eigen::Vector3f{10.5f, 3.5f, 0.f}}); - range_data.origin.x() = -0.5f; - range_data.origin.y() = -0.5f; + range_data.origin = Eigen::Vector3f(-0.5f, -0.5f, 0.0f); + range_data.returns.push_back( + {Eigen::Vector3f{-0.5f, 3.5f, 0.f}, range_data.origin}); + range_data.returns.push_back( + {Eigen::Vector3f{5.5f, 3.5f, 0.f}, range_data.origin}); + range_data.returns.push_back( + {Eigen::Vector3f{10.5f, 3.5f, 0.f}, range_data.origin}); range_data_inserter_->Insert(range_data, &tsdf_); tsdf_.FinishUpdate(); float x = 4.5f; @@ -264,10 +266,11 @@ TEST_F(RangeDataInserterTest2DTSDF, InsertSmallAnglePointWitNormalProjection) { options_.set_project_sdf_distance_to_scan_normal(true); range_data_inserter_ = absl::make_unique(options_); auto range_data = sensor::RangeData(); - range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}}); - range_data.returns.push_back({Eigen::Vector3f{5.5f, 3.5f, 0.f}}); - range_data.origin.x() = -0.5f; - range_data.origin.y() = -0.5f; + range_data.origin = Eigen::Vector3f(-0.5f, -0.5f, 0.0f); + range_data.returns.push_back( + {Eigen::Vector3f{-0.5f, 3.5f, 0.f}, range_data.origin}); + range_data.returns.push_back( + {Eigen::Vector3f{5.5f, 3.5f, 0.f}, range_data.origin}); range_data_inserter_->Insert(range_data, &tsdf_); tsdf_.FinishUpdate(); float x = 4.5f; @@ -294,10 +297,11 @@ TEST_F(RangeDataInserterTest2DTSDF, bandwidth); range_data_inserter_ = absl::make_unique(options_); auto range_data = sensor::RangeData(); - range_data.returns.push_back({Eigen::Vector3f{-0.5f, 3.5f, 0.f}}); - range_data.returns.push_back({Eigen::Vector3f{5.5f, 3.5f, 0.f}}); - range_data.origin.x() = -0.5f; - range_data.origin.y() = -0.5f; + range_data.origin = Eigen::Vector3f(-0.5f, -0.5f, 0.0f); + range_data.returns.push_back( + {Eigen::Vector3f{-0.5f, 3.5f, 0.f}, range_data.origin}); + range_data.returns.push_back( + {Eigen::Vector3f{5.5f, 3.5f, 0.f}, range_data.origin}); range_data_inserter_->Insert(range_data, &tsdf_); tsdf_.FinishUpdate(); float x = -0.5f; diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index b2a4f86bc1..49dc3e12e9 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -227,7 +227,8 @@ LocalTrajectoryBuilder3D::AddRangeData( const float range = delta.norm(); if (range >= options_.min_range()) { if (range <= options_.max_range()) { - accumulated_points.push_back(sensor::RangefinderPoint{hit_in_local}); + accumulated_points.push_back( + sensor::RangefinderPoint{hit_in_local, origin_in_local}); if (options_.use_intensities()) { accumulated_intensities.push_back(hit.intensity); } diff --git a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc index df610e3978..028ac47809 100644 --- a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc +++ b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc @@ -40,18 +40,19 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test { options_(CreateFastCorrelativeScanMatcher3DTestOptions3D(6)) {} void SetUp() override { - point_cloud_ = sensor::PointCloud({{Eigen::Vector3f(4.f, 0.f, 0.f)}, - {Eigen::Vector3f(4.5f, 0.f, 0.f)}, - {Eigen::Vector3f(5.f, 0.f, 0.f)}, - {Eigen::Vector3f(5.5f, 0.f, 0.f)}, - {Eigen::Vector3f(0.f, 4.f, 0.f)}, - {Eigen::Vector3f(0.f, 4.5f, 0.f)}, - {Eigen::Vector3f(0.f, 5.f, 0.f)}, - {Eigen::Vector3f(0.f, 5.5f, 0.f)}, - {Eigen::Vector3f(0.f, 0.f, 4.f)}, - {Eigen::Vector3f(0.f, 0.f, 4.5f)}, - {Eigen::Vector3f(0.f, 0.f, 5.f)}, - {Eigen::Vector3f(0.f, 0.f, 5.5f)}}); + point_cloud_ = sensor::PointCloud( + {{Eigen::Vector3f(4.f, 0.f, 0.f), Eigen::Vector3f(0.f, 0.f, 0.f)}, + {Eigen::Vector3f(4.5f, 0.f, 0.f), Eigen::Vector3f(0.f, 0.f, 0.f)}, + {Eigen::Vector3f(5.f, 0.f, 0.f), Eigen::Vector3f(0.f, 0.f, 0.f)}, + {Eigen::Vector3f(5.5f, 0.f, 0.f), Eigen::Vector3f(0.f, 0.f, 0.f)}, + {Eigen::Vector3f(0.f, 4.f, 0.f), Eigen::Vector3f(0.f, 0.f, 0.f)}, + {Eigen::Vector3f(0.f, 4.5f, 0.f), Eigen::Vector3f(0.f, 0.f, 0.f)}, + {Eigen::Vector3f(0.f, 5.f, 0.f), Eigen::Vector3f(0.f, 0.f, 0.f)}, + {Eigen::Vector3f(0.f, 5.5f, 0.f), Eigen::Vector3f(0.f, 0.f, 0.f)}, + {Eigen::Vector3f(0.f, 0.f, 4.f), Eigen::Vector3f(0.f, 0.f, 0.f)}, + {Eigen::Vector3f(0.f, 0.f, 4.5f), Eigen::Vector3f(0.f, 0.f, 0.f)}, + {Eigen::Vector3f(0.f, 0.f, 5.f), Eigen::Vector3f(0.f, 0.f, 0.f)}, + {Eigen::Vector3f(0.f, 0.f, 5.5f), Eigen::Vector3f(0.f, 0.f, 0.f)}}); } transform::Rigid3f GetRandomPose() { diff --git a/cartographer/sensor/proto/sensor.proto b/cartographer/sensor/proto/sensor.proto index 62cc640a93..4b6239e8cf 100644 --- a/cartographer/sensor/proto/sensor.proto +++ b/cartographer/sensor/proto/sensor.proto @@ -22,6 +22,7 @@ import "cartographer/transform/proto/transform.proto"; message RangefinderPoint { transform.proto.Vector3f position = 1; + transform.proto.Vector3f origin = 2; } message TimedRangefinderPoint { diff --git a/cartographer/sensor/rangefinder_point.h b/cartographer/sensor/rangefinder_point.h index 40e57492af..bb1b3238ce 100644 --- a/cartographer/sensor/rangefinder_point.h +++ b/cartographer/sensor/rangefinder_point.h @@ -30,6 +30,7 @@ namespace sensor { // Stores 3D position of a point observed by a rangefinder sensor. struct RangefinderPoint { Eigen::Vector3f position; + Eigen::Vector3f origin; }; // Stores 3D position of a point with its relative measurement time. @@ -44,6 +45,7 @@ inline RangefinderPoint operator*(const transform::Rigid3& lhs, const RangefinderPoint& rhs) { RangefinderPoint result = rhs; result.position = lhs * rhs.position; + result.origin = lhs * rhs.origin; return result; } @@ -67,13 +69,15 @@ inline bool operator==(const TimedRangefinderPoint& lhs, inline RangefinderPoint FromProto( const proto::RangefinderPoint& rangefinder_point_proto) { - return {transform::ToEigen(rangefinder_point_proto.position())}; + return {transform::ToEigen(rangefinder_point_proto.position()), + transform::ToEigen(rangefinder_point_proto.origin())}; } inline proto::RangefinderPoint ToProto( const RangefinderPoint& rangefinder_point) { proto::RangefinderPoint proto; *proto.mutable_position() = transform::ToProto(rangefinder_point.position); + *proto.mutable_origin() = transform::ToProto(rangefinder_point.origin); return proto; } @@ -93,8 +97,9 @@ inline proto::TimedRangefinderPoint ToProto( } inline RangefinderPoint ToRangefinderPoint( - const TimedRangefinderPoint& timed_rangefinder_point) { - return {timed_rangefinder_point.position}; + const TimedRangefinderPoint& timed_rangefinder_point, + const Eigen::Vector3f& origin) { + return {timed_rangefinder_point.position, origin}; } inline TimedRangefinderPoint ToTimedRangefinderPoint(