diff --git a/cartographer/internal/mapping_3d/local_trajectory_builder.cc b/cartographer/internal/mapping_3d/local_trajectory_builder.cc index c821f2f..0845144 100644 --- a/cartographer/internal/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/internal/mapping_3d/local_trajectory_builder.cc @@ -229,7 +229,7 @@ LocalTrajectoryBuilder::InsertIntoSubmap( } active_submaps_.InsertRangeData(filtered_range_data_in_local, gravity_alignment); - const auto rotational_scan_matcher_histogram = + const Eigen::VectorXf rotational_scan_matcher_histogram = scan_matching::RotationalScanMatcher::ComputeHistogram( sensor::TransformPointCloud( filtered_range_data_in_tracking.returns, diff --git a/cartographer/io/outlier_removing_points_processor.cc b/cartographer/io/outlier_removing_points_processor.cc index 11f0eaa..5cf3619 100644 --- a/cartographer/io/outlier_removing_points_processor.cc +++ b/cartographer/io/outlier_removing_points_processor.cc @@ -94,7 +94,7 @@ void OutlierRemovingPointsProcessor::ProcessInPhaseTwo( const Eigen::Vector3f delta = batch.points[i] - batch.origin; const float length = delta.norm(); for (float x = 0; x < length; x += voxel_size_) { - const auto index = + const Eigen::Array3i index = voxels_.GetCellIndex(batch.origin + (x / length) * delta); if (voxels_.value(index).hits > 0) { ++voxels_.mutable_value(index)->rays; @@ -108,7 +108,8 @@ void OutlierRemovingPointsProcessor::ProcessInPhaseThree( constexpr double kMissPerHitLimit = 3; std::unordered_set to_remove; for (size_t i = 0; i < batch->points.size(); ++i) { - const auto voxel = voxels_.value(voxels_.GetCellIndex(batch->points[i])); + const VoxelData voxel = + voxels_.value(voxels_.GetCellIndex(batch->points[i])); if (!(voxel.rays < kMissPerHitLimit * voxel.hits)) { to_remove.insert(i); } diff --git a/cartographer/io/probability_grid_points_processor.cc b/cartographer/io/probability_grid_points_processor.cc index 12c856d..c6d7d19 100644 --- a/cartographer/io/probability_grid_points_processor.cc +++ b/cartographer/io/probability_grid_points_processor.cc @@ -134,9 +134,9 @@ std::unique_ptr DrawProbabilityGrid( } auto image = common::make_unique(cell_limits.num_x_cells, cell_limits.num_y_cells); - for (const auto& xy_index : + for (const Eigen::Array2i& xy_index : cartographer::mapping_2d::XYIndexRangeIterator(cell_limits)) { - const auto index = xy_index + *offset; + const Eigen::Array2i index = xy_index + *offset; constexpr uint8 kUnknownValue = 128; const uint8 value = probability_grid.IsKnown(index) diff --git a/cartographer/io/xray_points_processor.cc b/cartographer/io/xray_points_processor.cc index efaff35..3bf277c 100644 --- a/cartographer/io/xray_points_processor.cc +++ b/cartographer/io/xray_points_processor.cc @@ -149,7 +149,8 @@ void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation, } // Returns the (x, y) pixel of the given 'index'. - const auto voxel_index_to_pixel = [this](const Eigen::Array3i& index) { + const auto voxel_index_to_pixel = + [this](const Eigen::Array3i& index) -> Eigen::Array2i { // We flip the y axis, since matrices rows are counted from the top. return Eigen::Array2i(bounding_box_.max()[1] - index[1], bounding_box_.max()[2] - index[2]); diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc index 69dd2a3..4d8656f 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc +++ b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc @@ -37,7 +37,7 @@ class CeresScanMatcherTest : public ::testing::Test { : hybrid_grid_(1.f), expected_pose_( transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))) { - for (const auto& point : + for (const Eigen::Vector3f& point : {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f), 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), diff --git a/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc b/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc index e793523..4f8893b 100644 --- a/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc +++ b/cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc @@ -29,7 +29,7 @@ class InterpolatedGridTest : public ::testing::Test { protected: InterpolatedGridTest() : hybrid_grid_(0.1f), interpolated_grid_(hybrid_grid_) { - for (const auto& point : + for (const Eigen::Vector3f& point : {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f), 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), diff --git a/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc b/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc index 30b3a91..a1b5a65 100644 --- a/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc @@ -24,7 +24,8 @@ std::function CreateLowResolutionMatcher( const HybridGrid* low_resolution_grid, const sensor::PointCloud* points) { return [=](const transform::Rigid3f& pose) { float score = 0.f; - for (const auto& point : sensor::TransformPointCloud(*points, pose)) { + for (const Eigen::Vector3f& 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)); diff --git a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc index 5cc2acb..07d69f3 100644 --- a/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc @@ -39,7 +39,7 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { : hybrid_grid_(0.1f), expected_pose_(Eigen::Vector3d(-1., 0., 0.), Eigen::Quaterniond::Identity()) { - for (const auto& point : + for (const Eigen::Vector3f& point : {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f), 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), diff --git a/cartographer/sensor/compressed_point_cloud_test.cc b/cartographer/sensor/compressed_point_cloud_test.cc index 757ba06..3d2ee54 100644 --- a/cartographer/sensor/compressed_point_cloud_test.cc +++ b/cartographer/sensor/compressed_point_cloud_test.cc @@ -106,7 +106,7 @@ TEST(CompressPointCloudTest, CompressesNoGaps) { EXPECT_EQ(decompressed.size(), recompressed.size()); std::vector x_coord; - for (const auto& p : compressed) { + for (const Eigen::Vector3f& p : compressed) { x_coord.push_back(p[0]); } std::sort(x_coord.begin(), x_coord.end()); diff --git a/cartographer/sensor/point_cloud.cc b/cartographer/sensor/point_cloud.cc index 452f3de..bfe1bb0 100644 --- a/cartographer/sensor/point_cloud.cc +++ b/cartographer/sensor/point_cloud.cc @@ -48,7 +48,7 @@ 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 auto& point : point_cloud) { + for (const Eigen::Vector3f& point : point_cloud) { if (min_z <= point.z() && point.z() <= max_z) { cropped_point_cloud.push_back(point); } @@ -59,7 +59,7 @@ 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 auto& point : point_cloud) { + for (const Eigen::Vector4f& point : point_cloud) { if (min_z <= point.z() && point.z() <= max_z) { cropped_point_cloud.push_back(point); } diff --git a/cartographer/sensor/range_data.cc b/cartographer/sensor/range_data.cc index 6246df6..aa9a00b 100644 --- a/cartographer/sensor/range_data.cc +++ b/cartographer/sensor/range_data.cc @@ -58,11 +58,11 @@ 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 auto& point : range_data.returns) { + for (const Eigen::Vector3f& point : range_data.returns) { *proto.add_returns() = transform::ToProto(point); } proto.mutable_misses()->Reserve(range_data.misses.size()); - for (const auto& point : range_data.misses) { + for (const Eigen::Vector3f& point : range_data.misses) { *proto.add_misses() = transform::ToProto(point); } return proto; diff --git a/cartographer/sensor/timed_point_cloud_data.cc b/cartographer/sensor/timed_point_cloud_data.cc index afa9e7e..73a4bde 100644 --- a/cartographer/sensor/timed_point_cloud_data.cc +++ b/cartographer/sensor/timed_point_cloud_data.cc @@ -28,7 +28,7 @@ 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 auto& range : timed_point_cloud_data.ranges) { + for (const Eigen::Vector4f& range : timed_point_cloud_data.ranges) { *proto.add_point_data() = transform::ToProto(range); } return proto; diff --git a/cartographer/sensor/voxel_filter.cc b/cartographer/sensor/voxel_filter.cc index aad03d3..ab4c63c 100644 --- a/cartographer/sensor/voxel_filter.cc +++ b/cartographer/sensor/voxel_filter.cc @@ -81,7 +81,7 @@ PointCloudType FilterPointCloudUsingVoxels( const PointCloudType& point_cloud, mapping_3d::HybridGridBase* voxels) { PointCloudType results; - for (const auto& point : point_cloud) { + for (const typename PointCloudType::value_type& point : point_cloud) { auto* const value = voxels->mutable_value(voxels->GetCellIndex(point.template head<3>())); if (*value == 0) {