From 73c3d477d722ec0939c82950ef4dd5a965294cb6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Fri, 10 Aug 2018 22:36:37 +0200 Subject: [PATCH] Introduce [Timed]RangefinderPoint. (#1357) --- .../add_rangefinder_data_handler_test.cc | 5 +- ...in_max_range_filtering_points_processor.cc | 2 +- .../io/outlier_removing_points_processor.cc | 7 +- .../io/pcd_writing_points_processor.cc | 3 +- .../io/ply_writing_points_processor.cc | 2 +- cartographer/io/points_batch.cc | 2 +- cartographer/io/points_batch.h | 3 +- .../probability_grid_points_processor_test.cc | 10 +- cartographer/io/xray_points_processor.cc | 4 +- .../io/xyz_writing_points_processor.cc | 4 +- ...probability_grid_range_data_inserter_2d.cc | 16 +-- .../mapping/2d/range_data_inserter_2d_test.cc | 8 +- .../mapping/2d/tsdf_range_data_inserter_2d.cc | 20 ++-- .../2d/tsdf_range_data_inserter_2d_test.cc | 16 +-- .../mapping/3d/range_data_inserter_3d.cc | 8 +- .../mapping/3d/range_data_inserter_3d_test.cc | 6 +- cartographer/mapping/3d/submap_3d.cc | 4 +- .../2d/local_trajectory_builder_2d.cc | 19 +-- .../internal/2d/normal_estimation_2d.cc | 12 +- .../internal/2d/normal_estimation_2d_test.cc | 30 ++--- .../mapping/internal/2d/pose_graph_2d_test.cc | 3 +- .../ceres_scan_matcher_2d_test.cc | 2 +- .../correlative_scan_matcher_2d.cc | 8 +- .../correlative_scan_matcher_test.cc | 28 ++--- .../fast_correlative_scan_matcher_2d_test.cc | 24 ++-- .../occupied_space_cost_function_2d.cc | 5 +- .../occupied_space_cost_function_2d_test.cc | 2 +- ...l_time_correlative_scan_matcher_2d_test.cc | 14 +-- .../tsdf_match_cost_function_2d.cc | 5 +- .../tsdf_match_cost_function_2d_test.cc | 10 +- .../3d/local_trajectory_builder_3d.cc | 17 +-- .../3d/local_trajectory_builder_3d_test.cc | 41 +++---- .../ceres_scan_matcher_3d_test.cc | 2 +- .../fast_correlative_scan_matcher_3d.cc | 13 ++- .../fast_correlative_scan_matcher_3d_test.cc | 17 +-- .../scan_matching/low_resolution_matcher.cc | 4 +- .../occupied_space_cost_function_3d.h | 2 +- .../real_time_correlative_scan_matcher_3d.cc | 9 +- ...l_time_correlative_scan_matcher_3d_test.cc | 2 +- .../scan_matching/rotational_scan_matcher.cc | 26 +++-- .../constraints/constraint_builder_2d_test.cc | 2 +- .../constraints/constraint_builder_3d_test.cc | 4 +- .../mapping/internal/range_data_collator.cc | 12 +- .../internal/range_data_collator_test.cc | 30 ++--- .../mapping/internal/testing/test_helpers.cc | 6 +- cartographer/mapping/trajectory_node_test.cc | 9 +- cartographer/sensor/compressed_point_cloud.cc | 18 +-- cartographer/sensor/compressed_point_cloud.h | 8 +- .../sensor/compressed_point_cloud_test.cc | 16 +-- .../internal/ordered_multi_queue_test.cc | 5 +- cartographer/sensor/internal/test_helpers.h | 2 +- cartographer/sensor/internal/voxel_filter.cc | 22 ++-- cartographer/sensor/internal/voxel_filter.h | 4 +- .../sensor/internal/voxel_filter_test.cc | 18 +-- cartographer/sensor/point_cloud.cc | 17 ++- cartographer/sensor/point_cloud.h | 5 +- cartographer/sensor/point_cloud_test.cc | 24 ++-- cartographer/sensor/proto/sensor.proto | 18 ++- cartographer/sensor/range_data.cc | 41 ++++--- cartographer/sensor/rangefinder_point.h | 108 ++++++++++++++++++ cartographer/sensor/timed_point_cloud_data.cc | 22 ++-- cartographer/sensor/timed_point_cloud_data.h | 4 +- 62 files changed, 491 insertions(+), 319 deletions(-) create mode 100644 cartographer/sensor/rangefinder_point.h diff --git a/cartographer/cloud/internal/handlers/add_rangefinder_data_handler_test.cc b/cartographer/cloud/internal/handlers/add_rangefinder_data_handler_test.cc index 4d95784..a7af01e 100644 --- a/cartographer/cloud/internal/handlers/add_rangefinder_data_handler_test.cc +++ b/cartographer/cloud/internal/handlers/add_rangefinder_data_handler_test.cc @@ -41,7 +41,10 @@ const std::string kMessage = R"( x: 3.f y: 4.f z: 5.f } point_data { - x: 6.f y: 7.f z: 8.f t: 9.f + position { + x: 6.f y: 7.f z: 8.f + } + time: 9.f } })"; diff --git a/cartographer/io/min_max_range_filtering_points_processor.cc b/cartographer/io/min_max_range_filtering_points_processor.cc index 392c638..898a82c 100644 --- a/cartographer/io/min_max_range_filtering_points_processor.cc +++ b/cartographer/io/min_max_range_filtering_points_processor.cc @@ -40,7 +40,7 @@ void MinMaxRangeFiteringPointsProcessor::Process( std::unique_ptr batch) { std::unordered_set to_remove; for (size_t i = 0; i < batch->points.size(); ++i) { - const float range = (batch->points[i] - batch->origin).norm(); + const float range = (batch->points[i].position - batch->origin).norm(); if (!(min_range_ <= range && range <= max_range_)) { to_remove.insert(i); } diff --git a/cartographer/io/outlier_removing_points_processor.cc b/cartographer/io/outlier_removing_points_processor.cc index c33d25e..5cff194 100644 --- a/cartographer/io/outlier_removing_points_processor.cc +++ b/cartographer/io/outlier_removing_points_processor.cc @@ -81,7 +81,8 @@ PointsProcessor::FlushResult OutlierRemovingPointsProcessor::Flush() { void OutlierRemovingPointsProcessor::ProcessInPhaseOne( const PointsBatch& batch) { for (size_t i = 0; i < batch.points.size(); ++i) { - ++voxels_.mutable_value(voxels_.GetCellIndex(batch.points[i]))->hits; + ++voxels_.mutable_value(voxels_.GetCellIndex(batch.points[i].position)) + ->hits; } } @@ -91,7 +92,7 @@ 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] - batch.origin; + const Eigen::Vector3f delta = batch.points[i].position - batch.origin; const float length = delta.norm(); for (float x = 0; x < length; x += voxel_size_) { const Eigen::Array3i index = @@ -109,7 +110,7 @@ void OutlierRemovingPointsProcessor::ProcessInPhaseThree( std::unordered_set to_remove; for (size_t i = 0; i < batch->points.size(); ++i) { const VoxelData voxel = - voxels_.value(voxels_.GetCellIndex(batch->points[i])); + voxels_.value(voxels_.GetCellIndex(batch->points[i].position)); if (!(voxel.rays < kMissPerHitLimit * voxel.hits)) { to_remove.insert(i); } diff --git a/cartographer/io/pcd_writing_points_processor.cc b/cartographer/io/pcd_writing_points_processor.cc index a424931..abd2c98 100644 --- a/cartographer/io/pcd_writing_points_processor.cc +++ b/cartographer/io/pcd_writing_points_processor.cc @@ -119,7 +119,8 @@ void PcdWritingPointsProcessor::Process(std::unique_ptr batch) { WriteBinaryPcdHeader(has_colors_, 0, file_writer_.get()); } for (size_t i = 0; i < batch->points.size(); ++i) { - WriteBinaryPcdPointCoordinate(batch->points[i], file_writer_.get()); + WriteBinaryPcdPointCoordinate(batch->points[i].position, + file_writer_.get()); if (!batch->colors.empty()) { WriteBinaryPcdPointColor(ToUint8Color(batch->colors[i]), file_writer_.get()); diff --git a/cartographer/io/ply_writing_points_processor.cc b/cartographer/io/ply_writing_points_processor.cc index d564518..6c13d23 100644 --- a/cartographer/io/ply_writing_points_processor.cc +++ b/cartographer/io/ply_writing_points_processor.cc @@ -136,7 +136,7 @@ void PlyWritingPointsProcessor::Process(std::unique_ptr batch) { } for (size_t i = 0; i < batch->points.size(); ++i) { - WriteBinaryPlyPointCoordinate(batch->points[i], file_.get()); + WriteBinaryPlyPointCoordinate(batch->points[i].position, file_.get()); if (has_colors_) { WriteBinaryPlyPointColor(ToUint8Color(batch->colors[i]), file_.get()); } diff --git a/cartographer/io/points_batch.cc b/cartographer/io/points_batch.cc index c6df9dd..2639cd0 100644 --- a/cartographer/io/points_batch.cc +++ b/cartographer/io/points_batch.cc @@ -21,7 +21,7 @@ namespace io { void RemovePoints(std::unordered_set to_remove, PointsBatch* batch) { const int new_num_points = batch->points.size() - to_remove.size(); - std::vector points; + sensor::PointCloud points; points.reserve(new_num_points); std::vector intensities; if (!batch->intensities.empty()) { diff --git a/cartographer/io/points_batch.h b/cartographer/io/points_batch.h index 5bdbd0b..abcf4ca 100644 --- a/cartographer/io/points_batch.h +++ b/cartographer/io/points_batch.h @@ -17,6 +17,7 @@ #ifndef CARTOGRAPHER_IO_POINTS_BATCH_H_ #define CARTOGRAPHER_IO_POINTS_BATCH_H_ +#include #include #include #include @@ -52,7 +53,7 @@ struct PointsBatch { int trajectory_id; // Geometry of the points in the map frame. - std::vector points; + sensor::PointCloud points; // Intensities are optional and may be unspecified. The meaning of these // intensity values varies by device. For example, the VLP16 provides values diff --git a/cartographer/io/probability_grid_points_processor_test.cc b/cartographer/io/probability_grid_points_processor_test.cc index 31a35a2..b3fc73f 100644 --- a/cartographer/io/probability_grid_points_processor_test.cc +++ b/cartographer/io/probability_grid_points_processor_test.cc @@ -34,11 +34,11 @@ namespace { std::unique_ptr CreatePointsBatch() { auto points_batch = ::absl::make_unique(); points_batch->origin = Eigen::Vector3f(0, 0, 0); - points_batch->points.emplace_back(0.0f, 0.0f, 0.0f); - points_batch->points.emplace_back(0.0f, 1.0f, 2.0f); - points_batch->points.emplace_back(1.0f, 2.0f, 4.0f); - points_batch->points.emplace_back(0.0f, 3.0f, 5.0f); - points_batch->points.emplace_back(3.0f, 0.0f, 6.0f); + 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}}); return points_batch; } diff --git a/cartographer/io/xray_points_processor.cc b/cartographer/io/xray_points_processor.cc index 31856af..4e16655 100644 --- a/cartographer/io/xray_points_processor.cc +++ b/cartographer/io/xray_points_processor.cc @@ -196,9 +196,9 @@ void XRayPointsProcessor::Insert(const PointsBatch& batch, Aggregation* const aggregation) { constexpr FloatColor kDefaultColor = {{0.f, 0.f, 0.f}}; for (size_t i = 0; i < batch.points.size(); ++i) { - const Eigen::Vector3f camera_point = transform_ * batch.points[i]; + const sensor::RangefinderPoint camera_point = transform_ * batch.points[i]; const Eigen::Array3i cell_index = - aggregation->voxels.GetCellIndex(camera_point); + aggregation->voxels.GetCellIndex(camera_point.position); *aggregation->voxels.mutable_value(cell_index) = true; bounding_box_.extend(cell_index.matrix()); ColumnData& column_data = diff --git a/cartographer/io/xyz_writing_points_processor.cc b/cartographer/io/xyz_writing_points_processor.cc index 6a25c8e..befea10 100644 --- a/cartographer/io/xyz_writing_points_processor.cc +++ b/cartographer/io/xyz_writing_points_processor.cc @@ -48,8 +48,8 @@ PointsProcessor::FlushResult XyzWriterPointsProcessor::Flush() { } void XyzWriterPointsProcessor::Process(std::unique_ptr batch) { - for (const Eigen::Vector3f& point : batch->points) { - WriteXyzPoint(point, file_writer_.get()); + for (const sensor::RangefinderPoint& point : batch->points) { + WriteXyzPoint(point.position, file_writer_.get()); } next_->Process(std::move(batch)); } 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 3581f5c..8039158 100644 --- a/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc +++ b/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc @@ -37,11 +37,11 @@ void GrowAsNeeded(const sensor::RangeData& range_data, Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>()); // Padding around bounding box to avoid numerical issues at cell boundaries. constexpr float kPadding = 1e-6f; - for (const Eigen::Vector3f& hit : range_data.returns) { - bounding_box.extend(hit.head<2>()); + for (const sensor::RangefinderPoint& hit : range_data.returns) { + bounding_box.extend(hit.position.head<2>()); } - for (const Eigen::Vector3f& miss : range_data.misses) { - bounding_box.extend(miss.head<2>()); + for (const sensor::RangefinderPoint& miss : range_data.misses) { + bounding_box.extend(miss.position.head<2>()); } probability_grid->GrowLimits(bounding_box.min() - kPadding * Eigen::Vector2f::Ones()); @@ -66,8 +66,8 @@ void CastRays(const sensor::RangeData& range_data, // Compute and add the end points. std::vector ends; ends.reserve(range_data.returns.size()); - for (const Eigen::Vector3f& hit : range_data.returns) { - ends.push_back(superscaled_limits.GetCellIndex(hit.head<2>())); + for (const sensor::RangefinderPoint& hit : range_data.returns) { + ends.push_back(superscaled_limits.GetCellIndex(hit.position.head<2>())); probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table); } @@ -85,9 +85,9 @@ void CastRays(const sensor::RangeData& range_data, } // Finally, compute and add empty rays based on misses in the range data. - for (const Eigen::Vector3f& missing_echo : range_data.misses) { + for (const sensor::RangefinderPoint& missing_echo : range_data.misses) { std::vector ray = RayToPixelMask( - begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()), + begin, 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 433f8a7..587573d 100644 --- a/cartographer/mapping/2d/range_data_inserter_2d_test.cc +++ b/cartographer/mapping/2d/range_data_inserter_2d_test.cc @@ -49,10 +49,10 @@ class RangeDataInserterTest2D : public ::testing::Test { void InsertPointCloud() { sensor::RangeData range_data; - range_data.returns.emplace_back(-3.5f, 0.5f, 0.f); - range_data.returns.emplace_back(-2.5f, 1.5f, 0.f); - range_data.returns.emplace_back(-1.5f, 2.5f, 0.f); - range_data.returns.emplace_back(-0.5f, 3.5f, 0.f); + 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; range_data_inserter_->Insert(range_data, &probability_grid_); diff --git a/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc b/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc index cad6e7a..c0246a0 100644 --- a/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc +++ b/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc @@ -33,9 +33,11 @@ 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>()); - for (const Eigen::Vector3f& hit : range_data.returns) { - const Eigen::Vector3f direction = (hit - range_data.origin).normalized(); - const Eigen::Vector3f end_position = hit + truncation_distance * direction; + for (const sensor::RangefinderPoint& hit : range_data.returns) { + const Eigen::Vector3f direction = + (hit.position - range_data.origin).normalized(); + const Eigen::Vector3f end_position = + hit.position + truncation_distance * direction; bounding_box.extend(end_position.head<2>()); } // Padding around bounding box to avoid numerical issues at cell boundaries. @@ -66,9 +68,12 @@ std::pair SuperscaleRay( struct RangeDataSorter { RangeDataSorter(Eigen::Vector3f origin) { origin_ = origin.head<2>(); } - bool operator()(Eigen::Vector3f lhs, Eigen::Vector3f rhs) { - const Eigen::Vector2f delta_lhs = (lhs.head<2>() - origin_).normalized(); - const Eigen::Vector2f delta_rhs = (lhs.head<2>() - origin_).normalized(); + bool operator()(const sensor::RangefinderPoint& lhs, + const sensor::RangefinderPoint& rhs) { + const Eigen::Vector2f delta_lhs = + (lhs.position.head<2>() - origin_).normalized(); + const Eigen::Vector2f delta_rhs = + (lhs.position.head<2>() - origin_).normalized(); if ((delta_lhs[1] < 0.f) != (delta_rhs[1] < 0.f)) { return delta_lhs[1] < 0.f; } else if (delta_lhs[1] < 0.f) { @@ -147,7 +152,8 @@ void TSDFRangeDataInserter2D::Insert(const sensor::RangeData& range_data, 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 hit = sorted_range_data.returns[hit_index].head<2>(); + const Eigen::Vector2f hit = + sorted_range_data.returns[hit_index].position.head<2>(); const float normal = normals.empty() ? std::numeric_limits::quiet_NaN() : normals[hit_index]; diff --git a/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc b/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc index b665143..5e05eca 100644 --- a/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc +++ b/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc @@ -49,7 +49,7 @@ class RangeDataInserterTest2DTSDF : public ::testing::Test { void InsertPoint() { sensor::RangeData range_data; - range_data.returns.emplace_back(-0.5f, 3.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; range_data_inserter_->Insert(range_data, &tsdf_); @@ -237,9 +237,9 @@ TEST_F(RangeDataInserterTest2DTSDF, InsertPointQuadraticWeight) { TEST_F(RangeDataInserterTest2DTSDF, InsertSmallAnglePointWithoutNormalProjection) { sensor::RangeData range_data; - range_data.returns.emplace_back(-0.5f, 3.5f, 0.f); - range_data.returns.emplace_back(5.5f, 3.5f, 0.f); - range_data.returns.emplace_back(10.5f, 3.5f, 0.f); + 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_inserter_->Insert(range_data, &tsdf_); @@ -264,8 +264,8 @@ TEST_F(RangeDataInserterTest2DTSDF, InsertSmallAnglePointWitNormalProjection) { options_.set_project_sdf_distance_to_scan_normal(true); range_data_inserter_ = absl::make_unique(options_); sensor::RangeData range_data; - range_data.returns.emplace_back(-0.5f, 3.5f, 0.f); - range_data.returns.emplace_back(5.5f, 3.5f, 0.f); + 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_inserter_->Insert(range_data, &tsdf_); @@ -293,8 +293,8 @@ TEST_F(RangeDataInserterTest2DTSDF, options_.set_update_weight_angle_scan_normal_to_ray_kernel_bandwith(bandwith); range_data_inserter_ = absl::make_unique(options_); sensor::RangeData range_data; - range_data.returns.emplace_back(-0.5f, 3.5f, 0.f); - range_data.returns.emplace_back(5.5f, 3.5f, 0.f); + 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_inserter_->Insert(range_data, &tsdf_); diff --git a/cartographer/mapping/3d/range_data_inserter_3d.cc b/cartographer/mapping/3d/range_data_inserter_3d.cc index 698ea2d..2a336a8 100644 --- a/cartographer/mapping/3d/range_data_inserter_3d.cc +++ b/cartographer/mapping/3d/range_data_inserter_3d.cc @@ -30,8 +30,8 @@ void InsertMissesIntoGrid(const std::vector& miss_table, HybridGrid* hybrid_grid, const int num_free_space_voxels) { const Eigen::Array3i origin_cell = hybrid_grid->GetCellIndex(origin); - for (const Eigen::Vector3f& hit : returns) { - const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit); + for (const sensor::RangefinderPoint& hit : returns) { + const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit.position); const Eigen::Array3i delta = hit_cell - origin_cell; const int num_samples = delta.cwiseAbs().maxCoeff(); @@ -79,8 +79,8 @@ void RangeDataInserter3D::Insert(const sensor::RangeData& range_data, HybridGrid* hybrid_grid) const { CHECK_NOTNULL(hybrid_grid); - for (const Eigen::Vector3f& hit : range_data.returns) { - const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit); + for (const sensor::RangefinderPoint& hit : range_data.returns) { + const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit.position); hybrid_grid->ApplyLookupTable(hit_cell, hit_table_); } diff --git a/cartographer/mapping/3d/range_data_inserter_3d_test.cc b/cartographer/mapping/3d/range_data_inserter_3d_test.cc index 68efcff..eb051f1 100644 --- a/cartographer/mapping/3d/range_data_inserter_3d_test.cc +++ b/cartographer/mapping/3d/range_data_inserter_3d_test.cc @@ -41,8 +41,10 @@ class RangeDataInserter3DTest : public ::testing::Test { void InsertPointCloud() { const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f); - sensor::PointCloud returns = { - {-3.f, -1.f, 4.f}, {-2.f, 0.f, 4.f}, {-1.f, 1.f, 4.f}, {0.f, 2.f, 4.f}}; + sensor::PointCloud 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}}}; range_data_inserter_->Insert(sensor::RangeData{origin, returns, {}}, &hybrid_grid_); } diff --git a/cartographer/mapping/3d/submap_3d.cc b/cartographer/mapping/3d/submap_3d.cc index 00bc26e..26878cc 100644 --- a/cartographer/mapping/3d/submap_3d.cc +++ b/cartographer/mapping/3d/submap_3d.cc @@ -41,8 +41,8 @@ struct PixelData { sensor::RangeData FilterRangeDataByMaxRange(const sensor::RangeData& range_data, const float max_range) { sensor::RangeData result{range_data.origin, {}, {}}; - for (const Eigen::Vector3f& hit : range_data.returns) { - if ((hit - range_data.origin).norm() <= max_range) { + for (const sensor::RangefinderPoint& hit : range_data.returns) { + if ((hit.position - range_data.origin).norm() <= max_range) { result.returns.push_back(hit); } } diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc index 9df3d93..b25488f 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc @@ -130,10 +130,10 @@ LocalTrajectoryBuilder2D::AddRangeData( CHECK(!synchronized_data.ranges.empty()); // TODO(gaschler): Check if this can strictly be 0. - CHECK_LE(synchronized_data.ranges.back().point_time[3], 0.f); + CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f); const common::Time time_first_point = time + - common::FromSeconds(synchronized_data.ranges.front().point_time[3]); + common::FromSeconds(synchronized_data.ranges.front().point_time.time); if (time_first_point < extrapolator_->GetLastPoseTime()) { LOG(INFO) << "Extrapolator is still initializing."; return nullptr; @@ -143,7 +143,7 @@ LocalTrajectoryBuilder2D::AddRangeData( range_data_poses.reserve(synchronized_data.ranges.size()); bool warned = false; for (const auto& range : synchronized_data.ranges) { - common::Time time_point = time + common::FromSeconds(range.point_time[3]); + common::Time time_point = time + common::FromSeconds(range.point_time.time); if (time_point < extrapolator_->GetLastExtrapolatedTime()) { if (!warned) { LOG(ERROR) @@ -166,20 +166,23 @@ LocalTrajectoryBuilder2D::AddRangeData( // Drop any returns below the minimum range and convert returns beyond the // maximum range into misses. for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) { - const Eigen::Vector4f& hit = synchronized_data.ranges[i].point_time; + 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); - const Eigen::Vector3f hit_in_local = range_data_poses[i] * hit.head<3>(); - const Eigen::Vector3f delta = hit_in_local - origin_in_local; + sensor::RangefinderPoint hit_in_local = + range_data_poses[i] * sensor::ToRangefinderPoint(hit); + const Eigen::Vector3f delta = hit_in_local.position - origin_in_local; 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 { - accumulated_range_data_.misses.push_back( + hit_in_local.position = origin_in_local + - options_.missing_data_ray_length() / range * delta); + 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 de5550b..a7d9542 100644 --- a/cartographer/mapping/internal/2d/normal_estimation_2d.cc +++ b/cartographer/mapping/internal/2d/normal_estimation_2d.cc @@ -32,7 +32,8 @@ float EstimateNormal(const sensor::PointCloud& returns, const size_t sample_window_begin, const size_t sample_window_end, const Eigen::Vector3f& sensor_origin) { - const Eigen::Vector3f& estimation_point = returns[estimation_point_index]; + const Eigen::Vector3f& estimation_point = + returns[estimation_point_index].position; if (sample_window_end - sample_window_begin < 2) { return NormalTo2DAngle(sensor_origin - estimation_point); } @@ -42,7 +43,7 @@ float EstimateNormal(const sensor::PointCloud& returns, for (size_t sample_point_index = sample_window_begin; sample_point_index < sample_window_end; ++sample_point_index) { if (sample_point_index == estimation_point_index) continue; - const Eigen::Vector3f& sample_point = returns[sample_point_index]; + const Eigen::Vector3f& sample_point = returns[sample_point_index].position; const Eigen::Vector3f& tangent = estimation_point - sample_point; Eigen::Vector3f sample_normal = {-tangent[1], tangent[0], 0.f}; constexpr float kMinNormalLength = 1e-6f; @@ -83,11 +84,11 @@ std::vector EstimateNormals( const float sample_radius = normal_estimation_options.sample_radius(); for (size_t current_point = 0; current_point < range_data.returns.size(); ++current_point) { - const Eigen::Vector3f& hit = range_data.returns[current_point]; + const Eigen::Vector3f& hit = range_data.returns[current_point].position; size_t sample_window_begin = current_point; for (; sample_window_begin > 0 && current_point - sample_window_begin < max_num_samples / 2 && - (hit - range_data.returns[sample_window_begin - 1]).norm() < + (hit - range_data.returns[sample_window_begin - 1].position).norm() < sample_radius; --sample_window_begin) { } @@ -95,7 +96,8 @@ std::vector EstimateNormals( for (; sample_window_end < range_data.returns.size() && sample_window_end - current_point < ceil(max_num_samples / 2.0) + 1 && - (hit - range_data.returns[sample_window_end]).norm() < sample_radius; + (hit - range_data.returns[sample_window_end].position).norm() < + sample_radius; ++sample_window_end) { } const float normal_estimate = diff --git a/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc b/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc index 889d6a2..e51ff14 100644 --- a/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc +++ b/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc @@ -46,7 +46,8 @@ TEST(NormalEstimation2DTest, SinglePoint) { static_cast(num_angles) * 2. * M_PI - M_PI; range_data.returns.clear(); - range_data.returns.emplace_back(std::cos(angle), std::sin(angle), 0.f); + range_data.returns.push_back( + {Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}.cast()}); std::vector normals; normals = EstimateNormals(range_data, options); EXPECT_NEAR(common::NormalizeAngleDifference(angle - normals[0] - M_PI), @@ -63,9 +64,9 @@ TEST(NormalEstimation2DTest, StraightLineGeometry) { const proto::NormalEstimationOptions2D options = CreateNormalEstimationOptions2D(parameter_dictionary.get()); sensor::RangeData range_data; - range_data.returns.emplace_back(-1.f, 1.f, 0.f); - range_data.returns.emplace_back(0.f, 1.f, 0.f); - range_data.returns.emplace_back(1.f, 1.f, 0.f); + 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; std::vector normals; @@ -75,9 +76,9 @@ TEST(NormalEstimation2DTest, StraightLineGeometry) { } normals.clear(); range_data.returns.clear(); - range_data.returns.emplace_back(1.f, 1.f, 0.f); - range_data.returns.emplace_back(1.f, 0.f, 0.f); - range_data.returns.emplace_back(1.f, -1.f, 0.f); + range_data.returns.push_back({Eigen::Vector3f{1.f, 1.f, 0.f}}); + range_data.returns.push_back({Eigen::Vector3f{1.f, 0.f, 0.f}}); + range_data.returns.push_back({Eigen::Vector3f{1.f, -1.f, 0.f}}); normals = EstimateNormals(range_data, options); for (const float normal : normals) { EXPECT_NEAR(std::abs(normal), M_PI, 1e-4); @@ -85,9 +86,9 @@ TEST(NormalEstimation2DTest, StraightLineGeometry) { normals.clear(); range_data.returns.clear(); - range_data.returns.emplace_back(1.f, -1.f, 0.f); - range_data.returns.emplace_back(0.f, -1.f, 0.f); - range_data.returns.emplace_back(-1.f, -1.f, 0.f); + 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}}); normals = EstimateNormals(range_data, options); for (const float normal : normals) { EXPECT_NEAR(normal, M_PI_2, 1e-4); @@ -95,9 +96,9 @@ TEST(NormalEstimation2DTest, StraightLineGeometry) { normals.clear(); range_data.returns.clear(); - range_data.returns.emplace_back(-1.f, -1.f, 0.f); - range_data.returns.emplace_back(-1.f, 0.f, 0.f); - range_data.returns.emplace_back(-1.f, 1.f, 0.f); + range_data.returns.push_back({Eigen::Vector3f{-1.f, -1.f, 0.f}}); + range_data.returns.push_back({Eigen::Vector3f{-1.f, 0.f, 0.f}}); + range_data.returns.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}}); normals = EstimateNormals(range_data, options); for (const float normal : normals) { EXPECT_NEAR(normal, 0, 1e-4); @@ -122,7 +123,8 @@ TEST_P(CircularGeometry2DTest, NumSamplesPerNormal) { const double angle = static_cast(angle_idx) / static_cast(num_angles) * 2. * M_PI - M_PI; - range_data.returns.emplace_back(std::cos(angle), std::sin(angle), 0.f); + range_data.returns.push_back( + {Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}.cast()}); } range_data.origin.x() = 0.f; range_data.origin.y() = 0.f; diff --git a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc index 2789238..79bed90 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc @@ -44,7 +44,8 @@ class PoseGraph2DTest : public ::testing::Test { // kMinProbability) to unknown space (== kMinProbability). 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_.emplace_back(r * std::sin(t), r * std::cos(t), 0.f); + point_cloud_.push_back( + {Eigen::Vector3f{r * std::sin(t), r * std::cos(t), 0.f}}); } { diff --git a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc index 162565b..88ec321 100644 --- a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc @@ -42,7 +42,7 @@ class CeresScanMatcherTest : public ::testing::Test { probability_grid_.limits().GetCellIndex(Eigen::Vector2f(-3.5f, 2.5f)), kMaxProbability); - point_cloud_.emplace_back(-3.f, 2.f, 0.f); + point_cloud_.push_back({Eigen::Vector3f{-3.f, 2.f, 0.f}}); auto parameter_dictionary = common::MakeDictionary(R"text( return { diff --git a/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.cc b/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.cc index a825f80..1747165 100644 --- a/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.cc +++ b/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.cc @@ -32,8 +32,8 @@ SearchParameters::SearchParameters(const double linear_search_window, // We set this value to something on the order of resolution to make sure that // the std::acos() below is defined. float max_scan_range = 3.f * resolution; - for (const Eigen::Vector3f& point : point_cloud) { - const float range = point.head<2>().norm(); + for (const sensor::RangefinderPoint& point : point_cloud) { + const float range = point.position.head<2>().norm(); max_scan_range = std::max(range, max_scan_range); } const double kSafetyMargin = 1. - 1e-3; @@ -116,9 +116,9 @@ std::vector DiscretizeScans( for (const sensor::PointCloud& scan : scans) { discrete_scans.emplace_back(); discrete_scans.back().reserve(scan.size()); - for (const Eigen::Vector3f& point : scan) { + for (const sensor::RangefinderPoint& point : scan) { const Eigen::Vector2f translated_point = - Eigen::Affine2f(initial_translation) * point.head<2>(); + Eigen::Affine2f(initial_translation) * point.position.head<2>(); discrete_scans.back().push_back( map_limits.GetCellIndex(translated_point)); } diff --git a/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_test.cc b/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_test.cc index 923b299..29a076b 100644 --- a/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_test.cc @@ -58,27 +58,27 @@ TEST(Candidate, Construction) { TEST(GenerateRotatedScans, GenerateRotatedScans) { sensor::PointCloud point_cloud; - point_cloud.emplace_back(-1.f, 1.f, 0.f); + point_cloud.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}}); const std::vector scans = GenerateRotatedScans(point_cloud, SearchParameters(0, 1, M_PI / 2., 0.)); EXPECT_EQ(3, scans.size()); - EXPECT_NEAR(1., scans[0][0].x(), 1e-6); - EXPECT_NEAR(1., scans[0][0].y(), 1e-6); - EXPECT_NEAR(-1., scans[1][0].x(), 1e-6); - EXPECT_NEAR(1., scans[1][0].y(), 1e-6); - EXPECT_NEAR(-1., scans[2][0].x(), 1e-6); - EXPECT_NEAR(-1., scans[2][0].y(), 1e-6); + EXPECT_NEAR(1., scans[0][0].position.x(), 1e-6); + EXPECT_NEAR(1., scans[0][0].position.y(), 1e-6); + EXPECT_NEAR(-1., scans[1][0].position.x(), 1e-6); + EXPECT_NEAR(1., scans[1][0].position.y(), 1e-6); + EXPECT_NEAR(-1., scans[2][0].position.x(), 1e-6); + EXPECT_NEAR(-1., scans[2][0].position.y(), 1e-6); } TEST(DiscretizeScans, DiscretizeScans) { sensor::PointCloud point_cloud; - point_cloud.emplace_back(0.025f, 0.175f, 0.f); - point_cloud.emplace_back(-0.025f, 0.175f, 0.f); - point_cloud.emplace_back(-0.075f, 0.175f, 0.f); - point_cloud.emplace_back(-0.125f, 0.175f, 0.f); - point_cloud.emplace_back(-0.125f, 0.125f, 0.f); - point_cloud.emplace_back(-0.125f, 0.075f, 0.f); - point_cloud.emplace_back(-0.125f, 0.025f, 0.f); + 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}}); const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6)); const std::vector scans = diff --git a/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc index ad6e7f0..c50252f 100644 --- a/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc @@ -150,12 +150,12 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(3); sensor::PointCloud point_cloud; - point_cloud.emplace_back(-2.5f, 0.5f, 0.f); - point_cloud.emplace_back(-2.f, 0.5f, 0.f); - point_cloud.emplace_back(0.f, -0.5f, 0.f); - point_cloud.emplace_back(0.5f, -1.6f, 0.f); - point_cloud.emplace_back(2.5f, 0.5f, 0.f); - point_cloud.emplace_back(2.5f, 1.7f, 0.f); + point_cloud.push_back({Eigen::Vector3f{-2.5f, 0.5f, 0.f}}); + point_cloud.push_back({Eigen::Vector3f{-2.f, 0.5f, 0.f}}); + point_cloud.push_back({Eigen::Vector3f{0.f, -0.5f, 0.f}}); + point_cloud.push_back({Eigen::Vector3f{0.5f, -1.6f, 0.f}}); + point_cloud.push_back({Eigen::Vector3f{2.5f, 0.5f, 0.f}}); + point_cloud.push_back({Eigen::Vector3f{2.5f, 1.7f, 0.f}}); for (int i = 0; i != 50; ++i) { const transform::Rigid2f expected_pose( @@ -200,12 +200,12 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(6); sensor::PointCloud unperturbed_point_cloud; - unperturbed_point_cloud.emplace_back(-2.5f, 0.5f, 0.f); - unperturbed_point_cloud.emplace_back(-2.25f, 0.5f, 0.f); - unperturbed_point_cloud.emplace_back(0.f, 0.5f, 0.f); - unperturbed_point_cloud.emplace_back(0.25f, 1.6f, 0.f); - unperturbed_point_cloud.emplace_back(2.5f, 0.5f, 0.f); - unperturbed_point_cloud.emplace_back(2.f, 1.8f, 0.f); + unperturbed_point_cloud.push_back({Eigen::Vector3f{-2.5f, 0.5f, 0.f}}); + unperturbed_point_cloud.push_back({Eigen::Vector3f{-2.25f, 0.5f, 0.f}}); + unperturbed_point_cloud.push_back({Eigen::Vector3f{0.f, 0.5f, 0.f}}); + unperturbed_point_cloud.push_back({Eigen::Vector3f{0.25f, 1.6f, 0.f}}); + unperturbed_point_cloud.push_back({Eigen::Vector3f{2.5f, 0.5f, 0.f}}); + unperturbed_point_cloud.push_back({Eigen::Vector3f{2.f, 1.8f, 0.f}}); for (int i = 0; i != 20; ++i) { const transform::Rigid2f perturbation( diff --git a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc index f3d7122..3cf182a 100644 --- a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc +++ b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc @@ -49,8 +49,9 @@ class OccupiedSpaceCostFunction2D { for (size_t i = 0; i < point_cloud_.size(); ++i) { // Note that this is a 2D point. The third component is a scaling factor. - const Eigen::Matrix point((T(point_cloud_[i].x())), - (T(point_cloud_[i].y())), T(1.)); + const Eigen::Matrix point((T(point_cloud_[i].position.x())), + (T(point_cloud_[i].position.y())), + T(1.)); const Eigen::Matrix world = transform * point; interpolator.Evaluate( (limits.max().x() - world[0]) / limits.resolution() - 0.5 + diff --git a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc index a52f1df..240e1c8 100644 --- a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc @@ -34,7 +34,7 @@ TEST(OccupiedSpaceCostFunction2DTest, SmokeTest) { ValueConversionTables conversion_tables; ProbabilityGrid grid(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), &conversion_tables); - sensor::PointCloud point_cloud = {Eigen::Vector3f{0.f, 0.f, 0.f}}; + sensor::PointCloud point_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}}}; ceres::Problem problem; std::unique_ptr cost_function( CreateOccupiedSpaceCostFunction2D(1.f, point_cloud, grid)); 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 87a4d43..0e70341 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,13 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { CreateProbabilityGridRangeDataInserterOptions2D( parameter_dictionary.get())); } - point_cloud_.emplace_back(0.025f, 0.175f, 0.f); - point_cloud_.emplace_back(-0.025f, 0.175f, 0.f); - point_cloud_.emplace_back(-0.075f, 0.175f, 0.f); - point_cloud_.emplace_back(-0.125f, 0.175f, 0.f); - point_cloud_.emplace_back(-0.125f, 0.125f, 0.f); - point_cloud_.emplace_back(-0.125f, 0.075f, 0.f); - point_cloud_.emplace_back(-0.125f, 0.025f, 0.f); + 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}}); range_data_inserter_->Insert( sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}}, &probability_grid_); diff --git a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.cc b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.cc index 5aa76e8..3259c8d 100644 --- a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.cc +++ b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.cc @@ -47,8 +47,9 @@ class TSDFMatchCostFunction2D { T summed_weight = T(0); for (size_t i = 0; i < point_cloud_.size(); ++i) { // Note that this is a 2D point. The third component is a scaling factor. - const Eigen::Matrix point((T(point_cloud_[i].x())), - (T(point_cloud_[i].y())), T(1.)); + const Eigen::Matrix point((T(point_cloud_[i].position.x())), + (T(point_cloud_[i].position.y())), + T(1.)); const Eigen::Matrix world = transform * point; const T point_weight = interpolated_grid_.GetWeight(world[0], world[1]); summed_weight += point_weight; 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 89d924d..015ce5b 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 @@ -58,7 +58,7 @@ class TSDFSpaceCostFunction2DTest : public ::testing::Test { void InsertPointcloud() { sensor::RangeData range_data; for (float x = -.5; x < 0.5f; x += 0.1) { - range_data.returns.emplace_back(x, 1.0f, 0.f); + range_data.returns.push_back({Eigen::Vector3f{x, 1.0f, 0.f}}); } range_data.origin.x() = -0.5f; range_data.origin.y() = -0.5f; @@ -73,7 +73,7 @@ class TSDFSpaceCostFunction2DTest : public ::testing::Test { }; TEST_F(TSDFSpaceCostFunction2DTest, MatchEmptyTSDF) { - const sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 0.f, 0.f}}; + const sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}}}; std::unique_ptr cost_function( CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); const std::array pose_estimate{{0., 0., 0.}}; @@ -89,7 +89,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, MatchEmptyTSDF) { TEST_F(TSDFSpaceCostFunction2DTest, ExactInitialPose) { InsertPointcloud(); - const sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 1.0f, 0.f}}; + const sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 1.0f, 0.f}}}; std::unique_ptr cost_function( CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); const std::array pose_estimate{{0., 0., 0.}}; @@ -109,7 +109,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, ExactInitialPose) { TEST_F(TSDFSpaceCostFunction2DTest, PertubatedInitialPose) { InsertPointcloud(); - sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 1.0f, 0.f}}; + sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 1.0f, 0.f}}}; std::unique_ptr cost_function( CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); std::array pose_estimate{{0., 0.1, 0.}}; @@ -140,7 +140,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, PertubatedInitialPose) { TEST_F(TSDFSpaceCostFunction2DTest, InvalidInitialPose) { InsertPointcloud(); - sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 1.0f, 0.f}}; + sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 1.0f, 0.f}}}; std::unique_ptr cost_function( CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); std::array pose_estimate{{0., 0.4, 0.}}; diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index c50076c..240cd4a 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -137,10 +137,10 @@ LocalTrajectoryBuilder3D::AddRangeData( } CHECK(!synchronized_data.ranges.empty()); - CHECK_LE(synchronized_data.ranges.back().point_time[3], 0.f); + CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f); const common::Time time_first_point = current_sensor_time + - common::FromSeconds(synchronized_data.ranges.front().point_time[3]); + common::FromSeconds(synchronized_data.ranges.front().point_time.time); if (time_first_point < extrapolator_->GetLastPoseTime()) { LOG(INFO) << "Extrapolator is still initializing."; return nullptr; @@ -156,7 +156,7 @@ LocalTrajectoryBuilder3D::AddRangeData( for (const auto& hit : hits) { common::Time time_point = - current_sensor_time + common::FromSeconds(hit.point_time[3]); + current_sensor_time + common::FromSeconds(hit.point_time.time); if (time_point < extrapolator_->GetLastExtrapolatedTime()) { if (!warned) { LOG(ERROR) @@ -176,11 +176,11 @@ LocalTrajectoryBuilder3D::AddRangeData( } for (size_t i = 0; i < hits.size(); ++i) { - const Eigen::Vector3f hit_in_local = - hits_poses[i] * hits[i].point_time.head<3>(); + sensor::RangefinderPoint hit_in_local = + hits_poses[i] * sensor::ToRangefinderPoint(hits[i].point_time); const Eigen::Vector3f origin_in_local = hits_poses[i] * synchronized_data.origins.at(hits[i].origin_index); - const Eigen::Vector3f delta = hit_in_local - origin_in_local; + const Eigen::Vector3f delta = hit_in_local.position - origin_in_local; const float range = delta.norm(); if (range >= options_.min_range()) { if (range <= options_.max_range()) { @@ -189,8 +189,9 @@ LocalTrajectoryBuilder3D::AddRangeData( // We insert a ray cropped to 'max_range' as a miss for hits beyond the // maximum range. This way the free space up to the maximum range will // be updated. - accumulated_range_data_.misses.push_back( - origin_in_local + options_.max_range() / range * delta); + hit_in_local.position = + origin_in_local + options_.max_range() / range * delta; + accumulated_range_data_.misses.push_back(hit_in_local); } } } diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc index f0a494e..423ee91 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc @@ -179,38 +179,39 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { sensor::TimedPointCloud directions_in_rangefinder_frame; for (int r = -8; r != 8; ++r) { for (int s = -250; s != 250; ++s) { - Eigen::Vector4f first_point; - first_point << Eigen::AngleAxisf(M_PI * s / 250., - Eigen::Vector3f::UnitZ()) * - Eigen::AngleAxisf(M_PI / 12. * r / 8., - Eigen::Vector3f::UnitY()) * - Eigen::Vector3f::UnitX(), - 0.; + const sensor::TimedRangefinderPoint first_point{ + Eigen::Vector3f{ + Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) * + Eigen::AngleAxisf(M_PI / 12. * r / 8., + Eigen::Vector3f::UnitY()) * + Eigen::Vector3f::UnitX()}, + 0.}; directions_in_rangefinder_frame.push_back(first_point); // Second orthogonal rangefinder. - Eigen::Vector4f second_point; - second_point << Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX()) * - Eigen::AngleAxisf(M_PI * s / 250., - Eigen::Vector3f::UnitZ()) * - Eigen::AngleAxisf(M_PI / 12. * r / 8., - Eigen::Vector3f::UnitY()) * - Eigen::Vector3f::UnitX(), - 0.; + const sensor::TimedRangefinderPoint second_point{ + Eigen::Vector3f{ + Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX()) * + Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) * + Eigen::AngleAxisf(M_PI / 12. * r / 8., + Eigen::Vector3f::UnitY()) * + Eigen::Vector3f::UnitX()}, + 0.}; directions_in_rangefinder_frame.push_back(second_point); } } // We simulate a 30 m edge length box around the origin, also containing // 50 cm radius spheres. sensor::TimedPointCloud returns_in_world_frame; - for (const Eigen::Vector4f& direction_in_world_frame : + for (const auto& direction_in_world_frame : sensor::TransformTimedPointCloud(directions_in_rangefinder_frame, pose.cast())) { const Eigen::Vector3f origin = pose.cast() * Eigen::Vector3f::Zero(); - Eigen::Vector4f return_point; - return_point << CollideWithBubbles( - origin, CollideWithBox(origin, direction_in_world_frame.head<3>())), - 0.; + const sensor::TimedRangefinderPoint return_point{ + CollideWithBubbles( + origin, + CollideWithBox(origin, direction_in_world_frame.position)), + 0.}; returns_in_world_frame.push_back(return_point); } return {Eigen::Vector3f::Zero(), diff --git a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc index 2244b78..52d4025 100644 --- a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc +++ b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc @@ -42,7 +42,7 @@ class CeresScanMatcher3DTest : public ::testing::Test { Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f), Eigen::Vector3f(-7.f, 3.f, 1.f)}) { - point_cloud_.push_back(point); + point_cloud_.push_back({point}); hybrid_grid_.SetProbability( hybrid_grid_.GetCellIndex(expected_pose_.cast() * point), 1.); } diff --git a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc index d5ebeb9..a44ba5c 100644 --- a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc +++ b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc @@ -167,9 +167,9 @@ FastCorrelativeScanMatcher3D::MatchFullSubmap( const Eigen::Quaterniond& global_submap_rotation, const TrajectoryNode::Data& constant_data, const float min_score) const { float max_point_distance = 0.f; - for (const Eigen::Vector3f& point : + for (const sensor::RangefinderPoint& point : constant_data.high_resolution_point_cloud) { - max_point_distance = std::max(max_point_distance, point.norm()); + max_point_distance = std::max(max_point_distance, point.position.norm()); } const int linear_window_size = (width_in_voxels_ + 1) / 2 + @@ -223,9 +223,10 @@ DiscreteScan3D FastCorrelativeScanMatcher3D::DiscretizeScan( const PrecomputationGrid3D& original_grid = precomputation_grid_stack_->Get(0); std::vector full_resolution_cell_indices; - for (const Eigen::Vector3f& point : + for (const sensor::RangefinderPoint& point : sensor::TransformPointCloud(point_cloud, pose)) { - full_resolution_cell_indices.push_back(original_grid.GetCellIndex(point)); + full_resolution_cell_indices.push_back( + original_grid.GetCellIndex(point.position)); } const int full_resolution_depth = std::min(options_.full_resolution_depth(), options_.branch_and_bound_depth()); @@ -271,8 +272,8 @@ std::vector FastCorrelativeScanMatcher3D::GenerateDiscreteScans( // We set this value to something on the order of resolution to make sure that // the std::acos() below is defined. float max_scan_range = 3.f * resolution_; - for (const Eigen::Vector3f& point : point_cloud) { - const float range = point.norm(); + for (const sensor::RangefinderPoint& point : point_cloud) { + const float range = point.position.norm(); max_scan_range = std::max(range, max_scan_range); } const float kSafetyMargin = 1.f - 1e-2f; 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 0709167..b2ba896 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 @@ -41,12 +41,12 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test { void SetUp() override { point_cloud_ = { - 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)}; + {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)}}; } transform::Rigid3f GetRandomPose() { @@ -157,7 +157,8 @@ TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatch) { const std::unique_ptr low_resolution_result = fast_correlative_scan_matcher->Match( transform::Rigid3d::Identity(), transform::Rigid3d::Identity(), - CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore); + CreateConstantData({{Eigen::Vector3f(42.f, 42.f, 42.f)}}), + kMinScore); EXPECT_THAT(low_resolution_result, testing::IsNull()) << low_resolution_result->low_resolution_score; } @@ -185,7 +186,7 @@ TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatchFullSubmap) { const std::unique_ptr low_resolution_result = fast_correlative_scan_matcher->MatchFullSubmap( Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(), - CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore); + CreateConstantData({{Eigen::Vector3f(42.f, 42.f, 42.f)}}), kMinScore); EXPECT_THAT(low_resolution_result, testing::IsNull()) << low_resolution_result->low_resolution_score; } diff --git a/cartographer/mapping/internal/3d/scan_matching/low_resolution_matcher.cc b/cartographer/mapping/internal/3d/scan_matching/low_resolution_matcher.cc index cead84b..754e9ed 100644 --- a/cartographer/mapping/internal/3d/scan_matching/low_resolution_matcher.cc +++ b/cartographer/mapping/internal/3d/scan_matching/low_resolution_matcher.cc @@ -24,11 +24,11 @@ std::function CreateLowResolutionMatcher( const HybridGrid* low_resolution_grid, const sensor::PointCloud* points) { return [=](const transform::Rigid3f& pose) { float score = 0.f; - for (const Eigen::Vector3f& point : + for (const sensor::RangefinderPoint& point : sensor::TransformPointCloud(*points, pose)) { // TODO(zhengj, whess): Interpolate the Grid to get better score. score += low_resolution_grid->GetProbability( - low_resolution_grid->GetCellIndex(point)); + low_resolution_grid->GetCellIndex(point.position)); } return score / points->size(); }; diff --git a/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h b/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h index a6aabb1..5e0cb80 100644 --- a/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h +++ b/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h @@ -71,7 +71,7 @@ class OccupiedSpaceCostFunction3D { T* const residual) const { for (size_t i = 0; i < point_cloud_.size(); ++i) { const Eigen::Matrix world = - transform * point_cloud_[i].cast(); + transform * point_cloud_[i].position.cast(); const T probability = interpolated_grid_.GetProbability(world[0], world[1], world[2]); residual[i] = scaling_factor_ * (1. - probability); diff --git a/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.cc b/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.cc index a9babd2..0426b2d 100644 --- a/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.cc +++ b/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.cc @@ -61,8 +61,8 @@ RealTimeCorrelativeScanMatcher3D::GenerateExhaustiveSearchTransforms( // We set this value to something on the order of resolution to make sure that // the std::acos() below is defined. float max_scan_range = 3.f * resolution; - for (const Eigen::Vector3f& point : point_cloud) { - const float range = point.norm(); + for (const sensor::RangefinderPoint& point : point_cloud) { + const float range = point.position.norm(); max_scan_range = std::max(range, max_scan_range); } const float kSafetyMargin = 1.f - 1e-3f; @@ -99,8 +99,9 @@ float RealTimeCorrelativeScanMatcher3D::ScoreCandidate( const sensor::PointCloud& transformed_point_cloud, const transform::Rigid3f& transform) const { float score = 0.f; - for (const Eigen::Vector3f& point : transformed_point_cloud) { - score += hybrid_grid.GetProbability(hybrid_grid.GetCellIndex(point)); + for (const sensor::RangefinderPoint& point : transformed_point_cloud) { + score += + hybrid_grid.GetProbability(hybrid_grid.GetCellIndex(point.position)); } score /= static_cast(transformed_point_cloud.size()); const float angle = transform::GetAngle(transform); diff --git a/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc b/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc index bf7d349..1d841a9 100644 --- a/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc +++ b/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc @@ -44,7 +44,7 @@ class RealTimeCorrelativeScanMatcher3DTest : public ::testing::Test { Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f), Eigen::Vector3f(-7.f, 3.f, 1.f)}) { - point_cloud_.push_back(point); + point_cloud_.push_back({point}); hybrid_grid_.SetProbability( hybrid_grid_.GetCellIndex(expected_pose_.cast() * point), 1.); } diff --git a/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc b/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc index 3722aae..3a779b8 100644 --- a/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc +++ b/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc @@ -52,8 +52,8 @@ void AddValueToHistogram(float angle, const float value, Eigen::Vector3f ComputeCentroid(const sensor::PointCloud& slice) { CHECK(!slice.empty()); Eigen::Vector3f sum = Eigen::Vector3f::Zero(); - for (const Eigen::Vector3f& point : slice) { - sum += point; + for (const sensor::RangefinderPoint& point : slice) { + sum += point.position; } return sum / static_cast(slice.size()); } @@ -68,16 +68,17 @@ void AddPointCloudSliceToHistogram(const sensor::PointCloud& slice, // will add the angle between points to the histogram with the maximum weight. // This is to reject, e.g., the angles observed on the ceiling and floor. const Eigen::Vector3f centroid = ComputeCentroid(slice); - Eigen::Vector3f last_point = slice.front(); - for (const Eigen::Vector3f& point : slice) { - const Eigen::Vector2f delta = (point - last_point).head<2>(); - const Eigen::Vector2f direction = (point - centroid).head<2>(); + Eigen::Vector3f last_point_position = slice.front().position; + for (const sensor::RangefinderPoint& point : slice) { + const Eigen::Vector2f delta = + (point.position - last_point_position).head<2>(); + const Eigen::Vector2f direction = (point.position - centroid).head<2>(); const float distance = delta.norm(); if (distance < kMinDistance || direction.norm() < kMinDistance) { continue; } if (distance > kMaxDistance) { - last_point = point; + last_point_position = point.position; continue; } const float angle = common::atan2(delta); @@ -97,13 +98,13 @@ sensor::PointCloud SortSlice(const sensor::PointCloud& slice) { } float angle; - Eigen::Vector3f point; + sensor::RangefinderPoint point; }; const Eigen::Vector3f centroid = ComputeCentroid(slice); std::vector by_angle; by_angle.reserve(slice.size()); - for (const Eigen::Vector3f& point : slice) { - const Eigen::Vector2f delta = (point - centroid).head<2>(); + for (const sensor::RangefinderPoint& point : slice) { + const Eigen::Vector2f delta = (point.position - centroid).head<2>(); if (delta.norm() < kMinDistance) { continue; } @@ -155,8 +156,9 @@ Eigen::VectorXf RotationalScanMatcher::ComputeHistogram( const sensor::PointCloud& point_cloud, const int histogram_size) { Eigen::VectorXf histogram = Eigen::VectorXf::Zero(histogram_size); std::map slices; - for (const Eigen::Vector3f& point : point_cloud) { - slices[common::RoundToInt(point.z() / kSliceHeight)].push_back(point); + for (const sensor::RangefinderPoint& point : point_cloud) { + slices[common::RoundToInt(point.position.z() / kSliceHeight)].push_back( + point); } for (const auto& slice : slices) { AddPointCloudSliceToHistogram(SortSlice(slice.second), &histogram); diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc b/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc index cf766e8..066bb8c 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc @@ -70,7 +70,7 @@ TEST_F(ConstraintBuilder2DTest, CallsBack) { TEST_F(ConstraintBuilder2DTest, FindsConstraints) { TrajectoryNode::Data node_data; node_data.filtered_gravity_aligned_point_cloud.push_back( - Eigen::Vector3f(0.1, 0.2, 0.3)); + {Eigen::Vector3f(0.1, 0.2, 0.3)}); node_data.gravity_alignment = Eigen::Quaterniond::Identity(); node_data.local_pose = transform::Rigid3d::Identity(); SubmapId submap_id{0, 1}; diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d_test.cc b/cartographer/mapping/internal/constraints/constraint_builder_3d_test.cc index acb44ca..849b4a5 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_3d_test.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_3d_test.cc @@ -75,9 +75,9 @@ TEST_F(ConstraintBuilder3DTest, FindsConstraints) { auto node_data = std::make_shared(); node_data->gravity_alignment = Eigen::Quaterniond::Identity(); node_data->high_resolution_point_cloud.push_back( - Eigen::Vector3f(0.1, 0.2, 0.3)); + {Eigen::Vector3f(0.1, 0.2, 0.3)}); node_data->low_resolution_point_cloud.push_back( - Eigen::Vector3f(0.1, 0.2, 0.3)); + {Eigen::Vector3f(0.1, 0.2, 0.3)}); node_data->rotational_scan_matcher_histogram = Eigen::VectorXf::Zero(3); node_data->local_pose = transform::Rigid3d::Identity(); node.constant_data = node_data; diff --git a/cartographer/mapping/internal/range_data_collator.cc b/cartographer/mapping/internal/range_data_collator.cc index 59f74ec..702565a 100644 --- a/cartographer/mapping/internal/range_data_collator.cc +++ b/cartographer/mapping/internal/range_data_collator.cc @@ -63,13 +63,14 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() { auto overlap_begin = ranges.begin(); while (overlap_begin < ranges.end() && - data.time + common::FromSeconds((*overlap_begin)[3]) < + data.time + common::FromSeconds((*overlap_begin).time) < current_start_) { ++overlap_begin; } auto overlap_end = overlap_begin; while (overlap_end < ranges.end() && - data.time + common::FromSeconds((*overlap_end)[3]) <= current_end_) { + data.time + common::FromSeconds((*overlap_end).time) <= + current_end_) { ++overlap_end; } if (ranges.begin() < overlap_begin && !warned_for_dropped_points) { @@ -82,14 +83,15 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() { if (overlap_begin < overlap_end) { std::size_t origin_index = result.origins.size(); result.origins.push_back(data.origin); - double time_correction = common::ToSeconds(data.time - current_end_); + const float time_correction = + static_cast(common::ToSeconds(data.time - current_end_)); for (auto overlap_it = overlap_begin; overlap_it != overlap_end; ++overlap_it) { sensor::TimedPointCloudOriginData::RangeMeasurement point{*overlap_it, origin_index}; // current_end_ + point_time[3]_after == in_timestamp + // point_time[3]_before - point.point_time[3] += time_correction; + point.point_time.time += time_correction; result.ranges.push_back(point); } } @@ -110,7 +112,7 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() { std::sort(result.ranges.begin(), result.ranges.end(), [](const sensor::TimedPointCloudOriginData::RangeMeasurement& a, const sensor::TimedPointCloudOriginData::RangeMeasurement& b) { - return a.point_time[3] < b.point_time[3]; + return a.point_time.time < b.point_time.time; }); return result; } diff --git a/cartographer/mapping/internal/range_data_collator_test.cc b/cartographer/mapping/internal/range_data_collator_test.cc index 282d708..8799c19 100644 --- a/cartographer/mapping/internal/range_data_collator_test.cc +++ b/cartographer/mapping/internal/range_data_collator_test.cc @@ -29,13 +29,13 @@ const int kNumSamples = 10; sensor::TimedPointCloudData CreateFakeRangeData(int from, int to) { double duration = common::ToSeconds(common::FromUniversal(to) - common::FromUniversal(from)); - sensor::TimedPointCloudData result{common::FromUniversal(to), - Eigen::Vector3f(0., 1., 2.), - sensor::TimedPointCloud(kNumSamples)}; + sensor::TimedPointCloudData result{ + common::FromUniversal(to), Eigen::Vector3f(0., 1., 2.), {}}; + result.ranges.reserve(kNumSamples); for (int i = 0; i < kNumSamples; ++i) { double fraction = static_cast(i) / (kNumSamples - 1); - double relative_time = (1.f - fraction) * -duration; - result.ranges[i] = Eigen::Vector4f(1., 2., 3., relative_time); + float relative_time = (1.f - fraction) * -duration; + result.ranges.push_back({Eigen::Vector3f{1., 2., 3.}, relative_time}); } return result; } @@ -44,7 +44,7 @@ bool ArePointTimestampsSorted(const sensor::TimedPointCloudOriginData& data) { std::vector timestamps; timestamps.reserve(data.ranges.size()); for (const auto& range : data.ranges) { - timestamps.push_back(range.point_time[3]); + timestamps.push_back(range.point_time.time); } return std::is_sorted(timestamps.begin(), timestamps.end()); } @@ -66,14 +66,14 @@ TEST(RangeDataCollatorTest, SingleSensor) { EXPECT_TRUE(ArePointTimestampsSorted(output_1)); EXPECT_NEAR(common::ToUniversal( output_1.time + - common::FromSeconds(output_1.ranges[0].point_time[3])), + common::FromSeconds(output_1.ranges[0].point_time.time)), 300, 2); auto output_2 = collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510)); EXPECT_EQ(common::ToUniversal(output_2.time), 510); EXPECT_EQ(output_2.origins.size(), 1); EXPECT_EQ(output_2.ranges.size(), 1); - EXPECT_EQ(output_2.ranges[0].point_time[3], 0.f); + EXPECT_EQ(output_2.ranges[0].point_time.time, 0.f); EXPECT_TRUE(ArePointTimestampsSorted(output_2)); } @@ -93,14 +93,14 @@ TEST(RangeDataCollatorTest, SingleSensorEmptyData) { EXPECT_TRUE(ArePointTimestampsSorted(output_1)); EXPECT_NEAR(common::ToUniversal( output_1.time + - common::FromSeconds(output_1.ranges[0].point_time[3])), + common::FromSeconds(output_1.ranges[0].point_time.time)), 300, 2); auto output_2 = collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510)); EXPECT_EQ(common::ToUniversal(output_2.time), 510); EXPECT_EQ(output_2.origins.size(), 1); EXPECT_EQ(output_2.ranges.size(), 1); - EXPECT_EQ(output_2.ranges[0].point_time[3], 0.f); + EXPECT_EQ(output_2.ranges[0].point_time.time, 0.f); EXPECT_TRUE(ArePointTimestampsSorted(output_2)); } @@ -118,9 +118,9 @@ TEST(RangeDataCollatorTest, TwoSensors) { ASSERT_EQ(output_1.ranges.size(), 2 * kNumSamples - 1); EXPECT_NEAR(common::ToUniversal( output_1.time + - common::FromSeconds(output_1.ranges[0].point_time[3])), + common::FromSeconds(output_1.ranges[0].point_time.time)), -1000, 2); - EXPECT_EQ(output_1.ranges.back().point_time[3], 0.f); + EXPECT_EQ(output_1.ranges.back().point_time.time, 0.f); EXPECT_TRUE(ArePointTimestampsSorted(output_1)); auto output_2 = collator.AddRangeData(sensor_0, CreateFakeRangeData(300, 500)); @@ -129,9 +129,9 @@ TEST(RangeDataCollatorTest, TwoSensors) { ASSERT_EQ(output_2.ranges.size(), 2); EXPECT_NEAR(common::ToUniversal( output_2.time + - common::FromSeconds(output_2.ranges[0].point_time[3])), + common::FromSeconds(output_2.ranges[0].point_time.time)), 300, 2); - EXPECT_EQ(output_2.ranges.back().point_time[3], 0.f); + EXPECT_EQ(output_2.ranges.back().point_time.time, 0.f); EXPECT_TRUE(ArePointTimestampsSorted(output_2)); // Sending the same sensor will flush everything before. auto output_3 = @@ -140,7 +140,7 @@ TEST(RangeDataCollatorTest, TwoSensors) { EXPECT_EQ( output_1.ranges.size() + output_2.ranges.size() + output_3.ranges.size(), 3 * kNumSamples); - EXPECT_EQ(output_3.ranges.back().point_time[3], 0.f); + EXPECT_EQ(output_3.ranges.back().point_time.time, 0.f); EXPECT_TRUE(ArePointTimestampsSorted(output_3)); } diff --git a/cartographer/mapping/internal/testing/test_helpers.cc b/cartographer/mapping/internal/testing/test_helpers.cc index a3dc394..f8401dd 100644 --- a/cartographer/mapping/internal/testing/test_helpers.cc +++ b/cartographer/mapping/internal/testing/test_helpers.cc @@ -56,8 +56,10 @@ GenerateFakeRangeMeasurements(const Eigen::Vector3f& translation, for (double angle = 0.; angle < M_PI; angle += 0.01) { for (double height : {-0.4, -0.2, 0.0, 0.2, 0.4}) { constexpr double kRadius = 5; - point_cloud.emplace_back(kRadius * std::cos(angle), - kRadius * std::sin(angle), height, 0.); + point_cloud.push_back({Eigen::Vector3d{kRadius * std::cos(angle), + kRadius * std::sin(angle), height} + .cast(), + 0.}); } } const Eigen::Vector3f kVelocity = translation / duration; diff --git a/cartographer/mapping/trajectory_node_test.cc b/cartographer/mapping/trajectory_node_test.cc index b2ee936..029681c 100644 --- a/cartographer/mapping/trajectory_node_test.cc +++ b/cartographer/mapping/trajectory_node_test.cc @@ -32,10 +32,13 @@ TEST(TrajectoryNodeTest, ToAndFromProto) { const TrajectoryNode::Data expected{ common::FromUniversal(42), Eigen::Quaterniond(1., 2., -3., -4.), - sensor::CompressedPointCloud({{1.f, 2.f, 0.f}, {0.f, 0.f, 1.f}}) + sensor::CompressedPointCloud( + {{Eigen::Vector3f{1.f, 2.f, 0.f}}, {Eigen::Vector3f{0.f, 0.f, 1.f}}}) + .Decompress(), + sensor::CompressedPointCloud({{Eigen::Vector3f{2.f, 3.f, 4.f}}}) + .Decompress(), + sensor::CompressedPointCloud({{Eigen::Vector3f{-1.f, 2.f, 0.f}}}) .Decompress(), - sensor::CompressedPointCloud({{2.f, 3.f, 4.f}}).Decompress(), - sensor::CompressedPointCloud({{-1.f, 2.f, 0.f}}).Decompress(), Eigen::VectorXf::Unit(20, 4), transform::Rigid3d({1., 2., 3.}, Eigen::Quaterniond(4., 5., -6., -7.).normalized())}; diff --git a/cartographer/sensor/compressed_point_cloud.cc b/cartographer/sensor/compressed_point_cloud.cc index 8e52f91..d840e47 100644 --- a/cartographer/sensor/compressed_point_cloud.cc +++ b/cartographer/sensor/compressed_point_cloud.cc @@ -56,9 +56,9 @@ CompressedPointCloud::ConstIterator::EndIterator( return end_iterator; } -Eigen::Vector3f CompressedPointCloud::ConstIterator::operator*() const { +RangefinderPoint CompressedPointCloud::ConstIterator::operator*() const { CHECK_GT(remaining_points_, 0); - return current_point_; + return {current_point_}; } CompressedPointCloud::ConstIterator& CompressedPointCloud::ConstIterator:: @@ -109,14 +109,14 @@ CompressedPointCloud::CompressedPointCloud(const PointCloud& point_cloud) CHECK_LE(point_cloud.size(), std::numeric_limits::max()); for (int point_index = 0; point_index < static_cast(point_cloud.size()); ++point_index) { - const Eigen::Vector3f& point = point_cloud[point_index]; - CHECK_LT(point.cwiseAbs().maxCoeff() / kPrecision, + const RangefinderPoint& point = point_cloud[point_index]; + CHECK_LT(point.position.cwiseAbs().maxCoeff() / kPrecision, 1 << kMaxBitsPerDirection) - << "Point out of bounds: " << point; + << "Point out of bounds: " << point.position; Eigen::Array3i raster_point; Eigen::Array3i block_coordinate; for (int i = 0; i < 3; ++i) { - raster_point[i] = common::RoundToInt(point[i] / kPrecision); + raster_point[i] = common::RoundToInt(point.position[i] / kPrecision); block_coordinate[i] = raster_point[i] >> kBitsPerCoordinate; raster_point[i] &= kCoordinateMask; } @@ -170,14 +170,14 @@ CompressedPointCloud::ConstIterator CompressedPointCloud::end() const { PointCloud CompressedPointCloud::Decompress() const { PointCloud decompressed; - for (const Eigen::Vector3f& point : *this) { + for (const RangefinderPoint& point : *this) { decompressed.push_back(point); } return decompressed; } -bool sensor::CompressedPointCloud::operator==( - const sensor::CompressedPointCloud& right_hand_container) const { +bool CompressedPointCloud::operator==( + const CompressedPointCloud& right_hand_container) const { return point_data_ == right_hand_container.point_data_ && num_points_ == right_hand_container.num_points_; } diff --git a/cartographer/sensor/compressed_point_cloud.h b/cartographer/sensor/compressed_point_cloud.h index de2b083..6cb01cc 100644 --- a/cartographer/sensor/compressed_point_cloud.h +++ b/cartographer/sensor/compressed_point_cloud.h @@ -61,10 +61,10 @@ class CompressedPointCloud { class CompressedPointCloud::ConstIterator { public: using iterator_category = std::forward_iterator_tag; - using value_type = Eigen::Vector3f; + using value_type = RangefinderPoint; using difference_type = int64; - using pointer = const Eigen::Vector3f*; - using reference = const Eigen::Vector3f&; + using pointer = const RangefinderPoint*; + using reference = const RangefinderPoint&; // Creates begin iterator. explicit ConstIterator(const CompressedPointCloud* compressed_point_cloud); @@ -73,7 +73,7 @@ class CompressedPointCloud::ConstIterator { static ConstIterator EndIterator( const CompressedPointCloud* compressed_point_cloud); - Eigen::Vector3f operator*() const; + RangefinderPoint operator*() const; ConstIterator& operator++(); bool operator!=(const ConstIterator& it) const; diff --git a/cartographer/sensor/compressed_point_cloud_test.cc b/cartographer/sensor/compressed_point_cloud_test.cc index 3d2ee54..341ad78 100644 --- a/cartographer/sensor/compressed_point_cloud_test.cc +++ b/cartographer/sensor/compressed_point_cloud_test.cc @@ -42,13 +42,13 @@ constexpr float kPrecision = 0.001f; // Matcher for 3-d vectors w.r.t. to the target precision. MATCHER_P(ApproximatelyEquals, expected, std::string("is equal to ") + PrintToString(expected)) { - return (arg - expected).isZero(kPrecision); + return (arg.position - expected).isZero(kPrecision); } // Helper function to test the mapping of a single point. Includes test for // recompressing the same point again. void TestPoint(const Eigen::Vector3f& p) { - CompressedPointCloud compressed({p}); + CompressedPointCloud compressed({{p}}); EXPECT_EQ(1, compressed.size()); EXPECT_THAT(*compressed.begin(), ApproximatelyEquals(p)); CompressedPointCloud recompressed({*compressed.begin()}); @@ -70,9 +70,9 @@ TEST(CompressPointCloudTest, CompressesPointsCorrectly) { } TEST(CompressPointCloudTest, Compresses) { - const CompressedPointCloud compressed({Eigen::Vector3f(0.838f, 0, 0), - Eigen::Vector3f(0.839f, 0, 0), - Eigen::Vector3f(0.840f, 0, 0)}); + const CompressedPointCloud compressed({{Eigen::Vector3f(0.838f, 0, 0)}, + {Eigen::Vector3f(0.839f, 0, 0)}, + {Eigen::Vector3f(0.840f, 0, 0)}}); EXPECT_FALSE(compressed.empty()); EXPECT_EQ(3, compressed.size()); const PointCloud decompressed = compressed.Decompress(); @@ -98,7 +98,7 @@ TEST(CompressPointCloudTest, CompressesEmptyPointCloud) { TEST(CompressPointCloudTest, CompressesNoGaps) { PointCloud point_cloud; for (int i = 0; i < 3000; ++i) { - point_cloud.push_back(Eigen::Vector3f(kPrecision * i - 1.5f, 0, 0)); + point_cloud.push_back({Eigen::Vector3f(kPrecision * i - 1.5f, 0, 0)}); } const CompressedPointCloud compressed(point_cloud); const PointCloud decompressed = compressed.Decompress(); @@ -106,8 +106,8 @@ TEST(CompressPointCloudTest, CompressesNoGaps) { EXPECT_EQ(decompressed.size(), recompressed.size()); std::vector x_coord; - for (const Eigen::Vector3f& p : compressed) { - x_coord.push_back(p[0]); + for (const RangefinderPoint& p : compressed) { + x_coord.push_back(p.position[0]); } std::sort(x_coord.begin(), x_coord.end()); for (size_t i = 1; i < x_coord.size(); ++i) { diff --git a/cartographer/sensor/internal/ordered_multi_queue_test.cc b/cartographer/sensor/internal/ordered_multi_queue_test.cc index 196707d..1a98787 100644 --- a/cartographer/sensor/internal/ordered_multi_queue_test.cc +++ b/cartographer/sensor/internal/ordered_multi_queue_test.cc @@ -45,9 +45,8 @@ class OrderedMultiQueueTest : public ::testing::Test { std::unique_ptr MakeImu(const int ordinal) { return MakeDispatchable( - "imu", - sensor::ImuData{common::FromUniversal(ordinal), Eigen::Vector3d::Zero(), - Eigen::Vector3d::Zero()}); + "imu", ImuData{common::FromUniversal(ordinal), Eigen::Vector3d::Zero(), + Eigen::Vector3d::Zero()}); } std::vector> values_; diff --git a/cartographer/sensor/internal/test_helpers.h b/cartographer/sensor/internal/test_helpers.h index d3bf3d2..0b32d89 100644 --- a/cartographer/sensor/internal/test_helpers.h +++ b/cartographer/sensor/internal/test_helpers.h @@ -76,7 +76,7 @@ struct CollatorInput { } const int trajectory_id; - std::unique_ptr data; + std::unique_ptr data; const CollatorOutput expected_output; }; diff --git a/cartographer/sensor/internal/voxel_filter.cc b/cartographer/sensor/internal/voxel_filter.cc index 1960245..4185b33 100644 --- a/cartographer/sensor/internal/voxel_filter.cc +++ b/cartographer/sensor/internal/voxel_filter.cc @@ -28,8 +28,8 @@ namespace { PointCloud FilterByMaxRange(const PointCloud& point_cloud, const float max_range) { PointCloud result; - for (const Eigen::Vector3f& point : point_cloud) { - if (point.norm() <= max_range) { + for (const RangefinderPoint& point : point_cloud) { + if (point.position.norm() <= max_range) { result.push_back(point); } } @@ -80,8 +80,9 @@ PointCloud AdaptivelyVoxelFiltered( PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) { PointCloud results; - for (const Eigen::Vector3f& point : point_cloud) { - auto it_inserted = voxel_set_.insert(IndexToKey(GetCellIndex(point))); + for (const RangefinderPoint& point : point_cloud) { + auto it_inserted = + voxel_set_.insert(IndexToKey(GetCellIndex(point.position))); if (it_inserted.second) { results.push_back(point); } @@ -91,9 +92,9 @@ PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) { TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) { TimedPointCloud results; - for (const Eigen::Vector4f& point : timed_point_cloud) { + for (const TimedRangefinderPoint& point : timed_point_cloud) { auto it_inserted = - voxel_set_.insert(IndexToKey(GetCellIndex(point.head<3>()))); + voxel_set_.insert(IndexToKey(GetCellIndex(point.position))); if (it_inserted.second) { results.push_back(point); } @@ -101,14 +102,13 @@ TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) { return results; } -std::vector -VoxelFilter::Filter( - const std::vector& +std::vector VoxelFilter::Filter( + const std::vector& range_measurements) { - std::vector results; + std::vector results; for (const auto& range_measurement : range_measurements) { auto it_inserted = voxel_set_.insert( - IndexToKey(GetCellIndex(range_measurement.point_time.head<3>()))); + IndexToKey(GetCellIndex(range_measurement.point_time.position))); if (it_inserted.second) { results.push_back(range_measurement); } diff --git a/cartographer/sensor/internal/voxel_filter.h b/cartographer/sensor/internal/voxel_filter.h index 480ceea..a8edbbe 100644 --- a/cartographer/sensor/internal/voxel_filter.h +++ b/cartographer/sensor/internal/voxel_filter.h @@ -46,8 +46,8 @@ class VoxelFilter { TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud); // Same for RangeMeasurement. - std::vector Filter( - const std::vector& + std::vector Filter( + const std::vector& range_measurements); private: diff --git a/cartographer/sensor/internal/voxel_filter_test.cc b/cartographer/sensor/internal/voxel_filter_test.cc index da57c40..320a99e 100644 --- a/cartographer/sensor/internal/voxel_filter_test.cc +++ b/cartographer/sensor/internal/voxel_filter_test.cc @@ -27,19 +27,19 @@ namespace { using ::testing::ContainerEq; TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) { - PointCloud point_cloud = {{0.f, 0.f, 0.f}, - {0.1f, -0.1f, 0.1f}, - {0.3f, -0.1f, 0.f}, - {0.f, 0.f, 0.1f}}; + PointCloud point_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}}, + {Eigen::Vector3f{0.1f, -0.1f, 0.1f}}, + {Eigen::Vector3f{0.3f, -0.1f, 0.f}}, + {Eigen::Vector3f{0.f, 0.f, 0.1f}}}; EXPECT_THAT(VoxelFilter(0.3f).Filter(point_cloud), ContainerEq(PointCloud{point_cloud[0], point_cloud[2]})); } TEST(VoxelFilterTest, HandlesLargeCoordinates) { - PointCloud point_cloud = {{100000.f, 0.f, 0.f}, - {100000.001f, -0.0001f, 0.0001f}, - {100000.003f, -0.0001f, 0.f}, - {-200000.f, 0.f, 0.f}}; + PointCloud point_cloud = {{Eigen::Vector3f{100000.f, 0.f, 0.f}}, + {Eigen::Vector3f{100000.001f, -0.0001f, 0.0001f}}, + {Eigen::Vector3f{100000.003f, -0.0001f, 0.f}}, + {Eigen::Vector3f{-200000.f, 0.f, 0.f}}}; EXPECT_THAT(VoxelFilter(0.01f).Filter(point_cloud), ContainerEq(PointCloud{point_cloud[0], point_cloud[3]})); } @@ -47,7 +47,7 @@ TEST(VoxelFilterTest, HandlesLargeCoordinates) { TEST(VoxelFilterTest, IgnoresTime) { TimedPointCloud timed_point_cloud; for (int i = 0; i < 100; ++i) { - timed_point_cloud.emplace_back(-100.f, 0.3f, 0.4f, 1.f * i); + timed_point_cloud.push_back({Eigen::Vector3f{-100.f, 0.3f, 0.4f}, 1.f * i}); } EXPECT_THAT(VoxelFilter(0.3f).Filter(timed_point_cloud), ContainerEq(TimedPointCloud{timed_point_cloud[0]})); diff --git a/cartographer/sensor/point_cloud.cc b/cartographer/sensor/point_cloud.cc index bfe1bb0..1d56520 100644 --- a/cartographer/sensor/point_cloud.cc +++ b/cartographer/sensor/point_cloud.cc @@ -26,7 +26,7 @@ PointCloud TransformPointCloud(const PointCloud& point_cloud, const transform::Rigid3f& transform) { PointCloud result; result.reserve(point_cloud.size()); - for (const Eigen::Vector3f& point : point_cloud) { + for (const RangefinderPoint& point : point_cloud) { result.emplace_back(transform * point); } return result; @@ -36,11 +36,8 @@ TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud, const transform::Rigid3f& transform) { TimedPointCloud result; result.reserve(point_cloud.size()); - for (const Eigen::Vector4f& point : point_cloud) { - Eigen::Vector4f result_point; - result_point.head<3>() = transform * point.head<3>(); - result_point[3] = point[3]; - result.emplace_back(result_point); + for (const TimedRangefinderPoint& point : point_cloud) { + result.push_back(transform * point); } return result; } @@ -48,8 +45,8 @@ TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud, PointCloud CropPointCloud(const PointCloud& point_cloud, const float min_z, const float max_z) { PointCloud cropped_point_cloud; - for (const Eigen::Vector3f& point : point_cloud) { - if (min_z <= point.z() && point.z() <= max_z) { + for (const RangefinderPoint& point : point_cloud) { + if (min_z <= point.position.z() && point.position.z() <= max_z) { cropped_point_cloud.push_back(point); } } @@ -59,8 +56,8 @@ PointCloud CropPointCloud(const PointCloud& point_cloud, const float min_z, TimedPointCloud CropTimedPointCloud(const TimedPointCloud& point_cloud, const float min_z, const float max_z) { TimedPointCloud cropped_point_cloud; - for (const Eigen::Vector4f& point : point_cloud) { - if (min_z <= point.z() && point.z() <= max_z) { + for (const TimedRangefinderPoint& point : point_cloud) { + if (min_z <= point.position.z() && point.position.z() <= max_z) { cropped_point_cloud.push_back(point); } } diff --git a/cartographer/sensor/point_cloud.h b/cartographer/sensor/point_cloud.h index b4ec700..dd96965 100644 --- a/cartographer/sensor/point_cloud.h +++ b/cartographer/sensor/point_cloud.h @@ -21,6 +21,7 @@ #include "Eigen/Core" #include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/sensor/rangefinder_point.h" #include "cartographer/transform/rigid_transform.h" #include "glog/logging.h" @@ -29,14 +30,14 @@ namespace sensor { // Stores 3D positions of points. // For 2D points, the third entry is 0.f. -typedef std::vector PointCloud; +using PointCloud = std::vector; // Stores 3D positions of points with their relative measurement time in the // fourth entry. Time is in seconds, increasing and relative to the moment when // the last point was acquired. So, the fourth entry for the last point is 0.f. // If timing is not available, all fourth entries are 0.f. For 2D points, the // third entry is 0.f (and the fourth entry is time). -typedef std::vector TimedPointCloud; +using TimedPointCloud = std::vector; struct PointCloudWithIntensities { TimedPointCloud points; diff --git a/cartographer/sensor/point_cloud_test.cc b/cartographer/sensor/point_cloud_test.cc index d7394a7..0a6d9df 100644 --- a/cartographer/sensor/point_cloud_test.cc +++ b/cartographer/sensor/point_cloud_test.cc @@ -27,26 +27,26 @@ namespace { TEST(PointCloudTest, TransformPointCloud) { PointCloud point_cloud; - point_cloud.emplace_back(0.5f, 0.5f, 1.f); - point_cloud.emplace_back(3.5f, 0.5f, 42.f); + point_cloud.push_back({Eigen::Vector3f{0.5f, 0.5f, 1.f}}); + point_cloud.push_back({Eigen::Vector3f{3.5f, 0.5f, 42.f}}); const PointCloud transformed_point_cloud = TransformPointCloud( point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2))); - EXPECT_NEAR(-0.5f, transformed_point_cloud[0].x(), 1e-6); - EXPECT_NEAR(0.5f, transformed_point_cloud[0].y(), 1e-6); - EXPECT_NEAR(-0.5f, transformed_point_cloud[1].x(), 1e-6); - EXPECT_NEAR(3.5f, transformed_point_cloud[1].y(), 1e-6); + EXPECT_NEAR(-0.5f, transformed_point_cloud[0].position.x(), 1e-6); + EXPECT_NEAR(0.5f, transformed_point_cloud[0].position.y(), 1e-6); + EXPECT_NEAR(-0.5f, transformed_point_cloud[1].position.x(), 1e-6); + EXPECT_NEAR(3.5f, transformed_point_cloud[1].position.y(), 1e-6); } TEST(PointCloudTest, TransformTimedPointCloud) { TimedPointCloud point_cloud; - point_cloud.emplace_back(0.5f, 0.5f, 1.f, 0.f); - point_cloud.emplace_back(3.5f, 0.5f, 42.f, 0.f); + point_cloud.push_back({Eigen::Vector3f{0.5f, 0.5f, 1.f}, 0.f}); + point_cloud.push_back({Eigen::Vector3f{3.5f, 0.5f, 42.f}, 0.f}); const TimedPointCloud transformed_point_cloud = TransformTimedPointCloud( point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2))); - EXPECT_NEAR(-0.5f, transformed_point_cloud[0].x(), 1e-6); - EXPECT_NEAR(0.5f, transformed_point_cloud[0].y(), 1e-6); - EXPECT_NEAR(-0.5f, transformed_point_cloud[1].x(), 1e-6); - EXPECT_NEAR(3.5f, transformed_point_cloud[1].y(), 1e-6); + EXPECT_NEAR(-0.5f, transformed_point_cloud[0].position.x(), 1e-6); + EXPECT_NEAR(0.5f, transformed_point_cloud[0].position.y(), 1e-6); + EXPECT_NEAR(-0.5f, transformed_point_cloud[1].position.x(), 1e-6); + EXPECT_NEAR(3.5f, transformed_point_cloud[1].position.y(), 1e-6); } } // namespace diff --git a/cartographer/sensor/proto/sensor.proto b/cartographer/sensor/proto/sensor.proto index 175775f..0b445b3 100644 --- a/cartographer/sensor/proto/sensor.proto +++ b/cartographer/sensor/proto/sensor.proto @@ -20,6 +20,15 @@ option java_outer_classname = "Sensor"; import "cartographer/transform/proto/transform.proto"; +message RangefinderPoint { + transform.proto.Vector3f position = 1; +} + +message TimedRangefinderPoint { + transform.proto.Vector3f position = 1; + float time = 2; +} + // Compressed collection of a 3D point cloud. message CompressedPointCloud { int32 num_points = 1; @@ -30,14 +39,17 @@ message CompressedPointCloud { message TimedPointCloudData { int64 timestamp = 1; transform.proto.Vector3f origin = 2; - repeated transform.proto.Vector4f point_data = 3; + repeated transform.proto.Vector4f point_data_legacy = 3; + repeated TimedRangefinderPoint point_data = 4; } // Proto representation of ::cartographer::sensor::RangeData. message RangeData { transform.proto.Vector3f origin = 1; - repeated transform.proto.Vector3f returns = 2; - repeated transform.proto.Vector3f misses = 3; + repeated transform.proto.Vector3f returns_legacy = 2; + repeated transform.proto.Vector3f misses_legacy = 3; + repeated RangefinderPoint returns = 4; + repeated RangefinderPoint misses = 5; } // Proto representation of ::cartographer::sensor::ImuData. diff --git a/cartographer/sensor/range_data.cc b/cartographer/sensor/range_data.cc index aa9a00b..2eea185 100644 --- a/cartographer/sensor/range_data.cc +++ b/cartographer/sensor/range_data.cc @@ -58,30 +58,41 @@ proto::RangeData ToProto(const RangeData& range_data) { proto::RangeData proto; *proto.mutable_origin() = transform::ToProto(range_data.origin); proto.mutable_returns()->Reserve(range_data.returns.size()); - for (const Eigen::Vector3f& point : range_data.returns) { - *proto.add_returns() = transform::ToProto(point); + for (const RangefinderPoint& point : range_data.returns) { + *proto.add_returns() = ToProto(point); } proto.mutable_misses()->Reserve(range_data.misses.size()); - for (const Eigen::Vector3f& point : range_data.misses) { - *proto.add_misses() = transform::ToProto(point); + for (const RangefinderPoint& point : range_data.misses) { + *proto.add_misses() = ToProto(point); } return proto; } RangeData FromProto(const proto::RangeData& proto) { PointCloud returns; - returns.reserve(proto.returns().size()); - std::transform( - proto.returns().begin(), proto.returns().end(), - std::back_inserter(returns), - static_cast( - transform::ToEigen)); + if (proto.returns_size() > 0) { + returns.reserve(proto.returns().size()); + for (const auto& point_proto : proto.returns()) { + returns.push_back(FromProto(point_proto)); + } + } else { + returns.reserve(proto.returns_legacy().size()); + for (const auto& point_proto : proto.returns_legacy()) { + returns.push_back({transform::ToEigen(point_proto)}); + } + } PointCloud misses; - misses.reserve(proto.misses().size()); - std::transform( - proto.misses().begin(), proto.misses().end(), std::back_inserter(misses), - static_cast( - transform::ToEigen)); + if (proto.misses_size() > 0) { + misses.reserve(proto.misses().size()); + for (const auto& point_proto : proto.misses()) { + misses.push_back(FromProto(point_proto)); + } + } else { + misses.reserve(proto.misses_legacy().size()); + for (const auto& point_proto : proto.misses_legacy()) { + misses.push_back({transform::ToEigen(point_proto)}); + } + } return RangeData{transform::ToEigen(proto.origin()), returns, misses}; } diff --git a/cartographer/sensor/rangefinder_point.h b/cartographer/sensor/rangefinder_point.h new file mode 100644 index 0000000..40e5749 --- /dev/null +++ b/cartographer/sensor/rangefinder_point.h @@ -0,0 +1,108 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_SENSOR_RANGEFINDER_POINT_H_ +#define CARTOGRAPHER_SENSOR_RANGEFINDER_POINT_H_ + +#include + +#include "Eigen/Core" +#include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace sensor { + +// Stores 3D position of a point observed by a rangefinder sensor. +struct RangefinderPoint { + Eigen::Vector3f position; +}; + +// Stores 3D position of a point with its relative measurement time. +// See point_cloud.h for more details. +struct TimedRangefinderPoint { + Eigen::Vector3f position; + float time; +}; + +template +inline RangefinderPoint operator*(const transform::Rigid3& lhs, + const RangefinderPoint& rhs) { + RangefinderPoint result = rhs; + result.position = lhs * rhs.position; + return result; +} + +template +inline TimedRangefinderPoint operator*(const transform::Rigid3& lhs, + const TimedRangefinderPoint& rhs) { + TimedRangefinderPoint result = rhs; + result.position = lhs * rhs.position; + return result; +} + +inline bool operator==(const RangefinderPoint& lhs, + const RangefinderPoint& rhs) { + return lhs.position == rhs.position; +} + +inline bool operator==(const TimedRangefinderPoint& lhs, + const TimedRangefinderPoint& rhs) { + return lhs.position == rhs.position && lhs.time == rhs.time; +} + +inline RangefinderPoint FromProto( + const proto::RangefinderPoint& rangefinder_point_proto) { + return {transform::ToEigen(rangefinder_point_proto.position())}; +} + +inline proto::RangefinderPoint ToProto( + const RangefinderPoint& rangefinder_point) { + proto::RangefinderPoint proto; + *proto.mutable_position() = transform::ToProto(rangefinder_point.position); + return proto; +} + +inline TimedRangefinderPoint FromProto( + const proto::TimedRangefinderPoint& timed_rangefinder_point_proto) { + return {transform::ToEigen(timed_rangefinder_point_proto.position()), + timed_rangefinder_point_proto.time()}; +} + +inline proto::TimedRangefinderPoint ToProto( + const TimedRangefinderPoint& timed_rangefinder_point) { + proto::TimedRangefinderPoint proto; + *proto.mutable_position() = + transform::ToProto(timed_rangefinder_point.position); + proto.set_time(timed_rangefinder_point.time); + return proto; +} + +inline RangefinderPoint ToRangefinderPoint( + const TimedRangefinderPoint& timed_rangefinder_point) { + return {timed_rangefinder_point.position}; +} + +inline TimedRangefinderPoint ToTimedRangefinderPoint( + const RangefinderPoint& rangefinder_point, const float time) { + return {rangefinder_point.position, time}; +} + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_RANGEFINDER_POINT_H_ diff --git a/cartographer/sensor/timed_point_cloud_data.cc b/cartographer/sensor/timed_point_cloud_data.cc index 73a4bde..63a17af 100644 --- a/cartographer/sensor/timed_point_cloud_data.cc +++ b/cartographer/sensor/timed_point_cloud_data.cc @@ -28,20 +28,26 @@ proto::TimedPointCloudData ToProto( proto.set_timestamp(common::ToUniversal(timed_point_cloud_data.time)); *proto.mutable_origin() = transform::ToProto(timed_point_cloud_data.origin); proto.mutable_point_data()->Reserve(timed_point_cloud_data.ranges.size()); - for (const Eigen::Vector4f& range : timed_point_cloud_data.ranges) { - *proto.add_point_data() = transform::ToProto(range); + for (const TimedRangefinderPoint& range : timed_point_cloud_data.ranges) { + *proto.add_point_data() = ToProto(range); } return proto; } TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto) { TimedPointCloud timed_point_cloud; - timed_point_cloud.reserve(proto.point_data().size()); - std::transform( - proto.point_data().begin(), proto.point_data().end(), - std::back_inserter(timed_point_cloud), - static_cast( - transform::ToEigen)); + if (proto.point_data().size() > 0) { + timed_point_cloud.reserve(proto.point_data().size()); + for (const auto& timed_point_proto : proto.point_data()) { + timed_point_cloud.push_back(FromProto(timed_point_proto)); + } + } else { + timed_point_cloud.reserve(proto.point_data_legacy().size()); + for (const auto& timed_point_proto : proto.point_data_legacy()) { + const Eigen::Vector4f timed_point = transform::ToEigen(timed_point_proto); + timed_point_cloud.push_back({timed_point.head<3>(), timed_point[3]}); + } + } return TimedPointCloudData{common::FromUniversal(proto.timestamp()), transform::ToEigen(proto.origin()), timed_point_cloud}; diff --git a/cartographer/sensor/timed_point_cloud_data.h b/cartographer/sensor/timed_point_cloud_data.h index 2025ec4..4901e35 100644 --- a/cartographer/sensor/timed_point_cloud_data.h +++ b/cartographer/sensor/timed_point_cloud_data.h @@ -27,12 +27,12 @@ namespace sensor { struct TimedPointCloudData { common::Time time; Eigen::Vector3f origin; - sensor::TimedPointCloud ranges; + TimedPointCloud ranges; }; struct TimedPointCloudOriginData { struct RangeMeasurement { - Eigen::Vector4f point_time; + TimedRangefinderPoint point_time; size_t origin_index; }; common::Time time;