From e26e9f172b6e358bc27c02a34ef3a6c8dfe1f46d Mon Sep 17 00:00:00 2001 From: gaschler Date: Wed, 4 Apr 2018 15:20:10 +0200 Subject: [PATCH] RangeDataCollator handles empty range data. (#1036) FIXES=#1019 --- .../mapping/internal/range_data_collator.cc | 6 ++--- .../internal/range_data_collator_test.cc | 27 +++++++++++++++++++ 2 files changed, 30 insertions(+), 3 deletions(-) diff --git a/cartographer/mapping/internal/range_data_collator.cc b/cartographer/mapping/internal/range_data_collator.cc index f285d2d..ea1ff09 100644 --- a/cartographer/mapping/internal/range_data_collator.cc +++ b/cartographer/mapping/internal/range_data_collator.cc @@ -95,10 +95,10 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() { } // Drop buffered points until overlap_end. - if (overlap_end == ranges.begin()) { - ++it; - } else if (overlap_end == ranges.end()) { + if (overlap_end == ranges.end()) { it = id_to_pending_data_.erase(it); + } else if (overlap_end == ranges.begin()) { + ++it; } else { data = sensor::TimedPointCloudData{ data.time, data.origin, diff --git a/cartographer/mapping/internal/range_data_collator_test.cc b/cartographer/mapping/internal/range_data_collator_test.cc index 8f0c7eb..282d708 100644 --- a/cartographer/mapping/internal/range_data_collator_test.cc +++ b/cartographer/mapping/internal/range_data_collator_test.cc @@ -77,6 +77,33 @@ TEST(RangeDataCollatorTest, SingleSensor) { EXPECT_TRUE(ArePointTimestampsSorted(output_2)); } +TEST(RangeDataCollatorTest, SingleSensorEmptyData) { + const std::string sensor_id = "single_sensor"; + RangeDataCollator collator({sensor_id}); + sensor::TimedPointCloudData empty_data{common::FromUniversal(300)}; + auto output_0 = collator.AddRangeData(sensor_id, empty_data); + EXPECT_EQ(output_0.time, empty_data.time); + EXPECT_EQ(output_0.ranges.size(), empty_data.ranges.size()); + EXPECT_TRUE(ArePointTimestampsSorted(output_0)); + auto output_1 = + collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500)); + EXPECT_EQ(common::ToUniversal(output_1.time), 500); + EXPECT_EQ(output_1.origins.size(), 1); + ASSERT_EQ(output_1.ranges.size(), kNumSamples); + EXPECT_TRUE(ArePointTimestampsSorted(output_1)); + EXPECT_NEAR(common::ToUniversal( + output_1.time + + common::FromSeconds(output_1.ranges[0].point_time[3])), + 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_TRUE(ArePointTimestampsSorted(output_2)); +} + TEST(RangeDataCollatorTest, TwoSensors) { const std::string sensor_0 = "sensor_0"; const std::string sensor_1 = "sensor_1";