diff --git a/cartographer/mapping/internal/range_data_collator.cc b/cartographer/mapping/internal/range_data_collator.cc index 702565a..6c27ac3 100644 --- a/cartographer/mapping/internal/range_data_collator.cc +++ b/cartographer/mapping/internal/range_data_collator.cc @@ -25,10 +25,14 @@ namespace cartographer { namespace mapping { +constexpr float RangeDataCollator::kDefaultIntensityValue; + sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData( const std::string& sensor_id, - const sensor::TimedPointCloudData& timed_point_cloud_data) { + sensor::TimedPointCloudData timed_point_cloud_data) { CHECK_NE(expected_sensor_ids_.count(sensor_id), 0); + timed_point_cloud_data.intensities.resize( + timed_point_cloud_data.ranges.size(), kDefaultIntensityValue); // TODO(gaschler): These two cases can probably be one. if (id_to_pending_data_.count(sensor_id) != 0) { current_start_ = current_end_; @@ -36,10 +40,10 @@ sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData( // the two (do not send out current). current_end_ = id_to_pending_data_.at(sensor_id).time; auto result = CropAndMerge(); - id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data); + id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data)); return result; } - id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data); + id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data)); if (expected_sensor_ids_.size() != id_to_pending_data_.size()) { return {}; } @@ -59,7 +63,8 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() { for (auto it = id_to_pending_data_.begin(); it != id_to_pending_data_.end();) { sensor::TimedPointCloudData& data = it->second; - sensor::TimedPointCloud& ranges = it->second.ranges; + const sensor::TimedPointCloud& ranges = it->second.ranges; + const std::vector& intensities = it->second.intensities; auto overlap_begin = ranges.begin(); while (overlap_begin < ranges.end() && @@ -85,10 +90,14 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() { result.origins.push_back(data.origin); const float time_correction = static_cast(common::ToSeconds(data.time - current_end_)); + auto intensities_overlap_it = + intensities.begin() + (overlap_begin - ranges.begin()); + result.ranges.reserve(result.ranges.size() + + std::distance(overlap_begin, overlap_end)); for (auto overlap_it = overlap_begin; overlap_it != overlap_end; - ++overlap_it) { - sensor::TimedPointCloudOriginData::RangeMeasurement point{*overlap_it, - origin_index}; + ++overlap_it, ++intensities_overlap_it) { + sensor::TimedPointCloudOriginData::RangeMeasurement point{ + *overlap_it, *intensities_overlap_it, origin_index}; // current_end_ + point_time[3]_after == in_timestamp + // point_time[3]_before point.point_time.time += time_correction; @@ -102,9 +111,12 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() { } else if (overlap_end == ranges.begin()) { ++it; } else { + const auto intensities_overlap_end = + intensities.begin() + (overlap_end - ranges.begin()); data = sensor::TimedPointCloudData{ data.time, data.origin, - sensor::TimedPointCloud(overlap_end, ranges.end())}; + sensor::TimedPointCloud(overlap_end, ranges.end()), + std::vector(intensities_overlap_end, intensities.end())}; ++it; } } diff --git a/cartographer/mapping/internal/range_data_collator.h b/cartographer/mapping/internal/range_data_collator.h index 8328828..29cf138 100644 --- a/cartographer/mapping/internal/range_data_collator.h +++ b/cartographer/mapping/internal/range_data_collator.h @@ -37,9 +37,11 @@ class RangeDataCollator { : expected_sensor_ids_(expected_range_sensor_ids.begin(), expected_range_sensor_ids.end()) {} + // If timed_point_cloud_data has incomplete intensity data, we will fill the + // missing intensities with kDefaultIntensityValue. sensor::TimedPointCloudOriginData AddRangeData( const std::string& sensor_id, - const sensor::TimedPointCloudData& timed_point_cloud_data); + sensor::TimedPointCloudData timed_point_cloud_data); private: sensor::TimedPointCloudOriginData CropAndMerge(); @@ -49,6 +51,8 @@ class RangeDataCollator { std::map id_to_pending_data_; common::Time current_start_ = common::Time::min(); common::Time current_end_ = common::Time::min(); + + constexpr static float kDefaultIntensityValue = 0.f; }; } // namespace mapping diff --git a/cartographer/mapping/internal/range_data_collator_test.cc b/cartographer/mapping/internal/range_data_collator_test.cc index 8799c19..43ceb00 100644 --- a/cartographer/mapping/internal/range_data_collator_test.cc +++ b/cartographer/mapping/internal/range_data_collator_test.cc @@ -26,16 +26,21 @@ namespace { const int kNumSamples = 10; -sensor::TimedPointCloudData CreateFakeRangeData(int from, int to) { +sensor::TimedPointCloudData CreateFakeRangeData(int from, int to, + bool fake_intensities) { double duration = common::ToSeconds(common::FromUniversal(to) - common::FromUniversal(from)); sensor::TimedPointCloudData result{ - common::FromUniversal(to), Eigen::Vector3f(0., 1., 2.), {}}; + 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); - float relative_time = (1.f - fraction) * -duration; - result.ranges.push_back({Eigen::Vector3f{1., 2., 3.}, relative_time}); + float relative_time = (1. - fraction) * -duration; + result.ranges.push_back( + {Eigen::Vector3f{1., 2., static_cast(fraction)}, relative_time}); + if (fake_intensities) { + result.intensities.push_back(result.ranges.back().position.z()); + } } return result; } @@ -49,17 +54,23 @@ bool ArePointTimestampsSorted(const sensor::TimedPointCloudOriginData& data) { return std::is_sorted(timestamps.begin(), timestamps.end()); } +void IntensitiesAreConsistent(const sensor::TimedPointCloudOriginData& data) { + for (const auto& range : data.ranges) { + EXPECT_NEAR(range.point_time.position.z(), range.intensity, 1e-6); + } +} + TEST(RangeDataCollatorTest, SingleSensor) { const std::string sensor_id = "single_sensor"; RangeDataCollator collator({sensor_id}); auto output_0 = - collator.AddRangeData(sensor_id, CreateFakeRangeData(200, 300)); + collator.AddRangeData(sensor_id, CreateFakeRangeData(200, 300, false)); EXPECT_EQ(common::ToUniversal(output_0.time), 300); EXPECT_EQ(output_0.origins.size(), 1); EXPECT_EQ(output_0.ranges.size(), kNumSamples); EXPECT_TRUE(ArePointTimestampsSorted(output_0)); auto output_1 = - collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500)); + collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500, false)); EXPECT_EQ(common::ToUniversal(output_1.time), 500); EXPECT_EQ(output_1.origins.size(), 1); ASSERT_EQ(output_1.ranges.size(), kNumSamples); @@ -69,7 +80,7 @@ TEST(RangeDataCollatorTest, SingleSensor) { common::FromSeconds(output_1.ranges[0].point_time.time)), 300, 2); auto output_2 = - collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510)); + collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510, false)); EXPECT_EQ(common::ToUniversal(output_2.time), 510); EXPECT_EQ(output_2.origins.size(), 1); EXPECT_EQ(output_2.ranges.size(), 1); @@ -80,13 +91,14 @@ TEST(RangeDataCollatorTest, SingleSensor) { TEST(RangeDataCollatorTest, SingleSensorEmptyData) { const std::string sensor_id = "single_sensor"; RangeDataCollator collator({sensor_id}); - sensor::TimedPointCloudData empty_data{common::FromUniversal(300)}; + 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)); + collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500, false)); EXPECT_EQ(common::ToUniversal(output_1.time), 500); EXPECT_EQ(output_1.origins.size(), 1); ASSERT_EQ(output_1.ranges.size(), kNumSamples); @@ -96,7 +108,7 @@ TEST(RangeDataCollatorTest, SingleSensorEmptyData) { common::FromSeconds(output_1.ranges[0].point_time.time)), 300, 2); auto output_2 = - collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510)); + collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510, false)); EXPECT_EQ(common::ToUniversal(output_2.time), 510); EXPECT_EQ(output_2.origins.size(), 1); EXPECT_EQ(output_2.ranges.size(), 1); @@ -109,10 +121,10 @@ TEST(RangeDataCollatorTest, TwoSensors) { const std::string sensor_1 = "sensor_1"; RangeDataCollator collator({sensor_0, sensor_1}); auto output_0 = - collator.AddRangeData(sensor_0, CreateFakeRangeData(200, 300)); + collator.AddRangeData(sensor_0, CreateFakeRangeData(200, 300, false)); EXPECT_EQ(output_0.ranges.size(), 0); auto output_1 = - collator.AddRangeData(sensor_1, CreateFakeRangeData(-1000, 310)); + collator.AddRangeData(sensor_1, CreateFakeRangeData(-1000, 310, false)); EXPECT_EQ(output_1.origins.size(), 2); EXPECT_EQ(common::ToUniversal(output_1.time), 300); ASSERT_EQ(output_1.ranges.size(), 2 * kNumSamples - 1); @@ -123,7 +135,7 @@ TEST(RangeDataCollatorTest, TwoSensors) { 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)); + collator.AddRangeData(sensor_0, CreateFakeRangeData(300, 500, false)); EXPECT_EQ(output_2.origins.size(), 2); EXPECT_EQ(common::ToUniversal(output_2.time), 310); ASSERT_EQ(output_2.ranges.size(), 2); @@ -135,7 +147,7 @@ TEST(RangeDataCollatorTest, TwoSensors) { EXPECT_TRUE(ArePointTimestampsSorted(output_2)); // Sending the same sensor will flush everything before. auto output_3 = - collator.AddRangeData(sensor_0, CreateFakeRangeData(600, 700)); + collator.AddRangeData(sensor_0, CreateFakeRangeData(600, 700, false)); EXPECT_EQ(common::ToUniversal(output_3.time), 500); EXPECT_EQ( output_1.ranges.size() + output_2.ranges.size() + output_3.ranges.size(), @@ -150,21 +162,44 @@ TEST(RangeDataCollatorTest, ThreeSensors) { const std::string sensor_2 = "sensor_2"; RangeDataCollator collator({sensor_0, sensor_1, sensor_2}); auto output_0 = - collator.AddRangeData(sensor_0, CreateFakeRangeData(100, 200)); + collator.AddRangeData(sensor_0, CreateFakeRangeData(100, 200, false)); EXPECT_EQ(output_0.ranges.size(), 0); auto output_1 = - collator.AddRangeData(sensor_1, CreateFakeRangeData(199, 250)); + collator.AddRangeData(sensor_1, CreateFakeRangeData(199, 250, false)); EXPECT_EQ(output_1.ranges.size(), 0); auto output_2 = - collator.AddRangeData(sensor_2, CreateFakeRangeData(210, 300)); + collator.AddRangeData(sensor_2, CreateFakeRangeData(210, 300, false)); EXPECT_EQ(output_2.ranges.size(), kNumSamples + 1); EXPECT_TRUE(ArePointTimestampsSorted(output_2)); auto output_3 = - collator.AddRangeData(sensor_2, CreateFakeRangeData(400, 500)); + collator.AddRangeData(sensor_2, CreateFakeRangeData(400, 500, false)); EXPECT_EQ(output_2.ranges.size() + output_3.ranges.size(), 3 * kNumSamples); EXPECT_TRUE(ArePointTimestampsSorted(output_3)); } +TEST(RangeDataCollatorTest, ThreeSensorsWithIntensities) { + const std::string sensor_0 = "sensor_0"; + const std::string sensor_1 = "sensor_1"; + const std::string sensor_2 = "sensor_2"; + RangeDataCollator collator({sensor_0, sensor_1, sensor_2}); + auto output_0 = + collator.AddRangeData(sensor_0, CreateFakeRangeData(100, 200, true)); + EXPECT_EQ(output_0.ranges.size(), 0); + auto output_1 = + collator.AddRangeData(sensor_1, CreateFakeRangeData(199, 250, true)); + EXPECT_EQ(output_1.ranges.size(), 0); + auto output_2 = + collator.AddRangeData(sensor_2, CreateFakeRangeData(210, 300, true)); + EXPECT_EQ(output_2.ranges.size(), kNumSamples + 1); + EXPECT_TRUE(ArePointTimestampsSorted(output_2)); + IntensitiesAreConsistent(output_2); + auto output_3 = + collator.AddRangeData(sensor_2, CreateFakeRangeData(400, 500, true)); + EXPECT_EQ(output_2.ranges.size() + output_3.ranges.size(), 3 * kNumSamples); + EXPECT_TRUE(ArePointTimestampsSorted(output_3)); + IntensitiesAreConsistent(output_3); +} + } // namespace } // namespace mapping } // namespace cartographer diff --git a/cartographer/sensor/proto/sensor.proto b/cartographer/sensor/proto/sensor.proto index 0b445b3..62cc640 100644 --- a/cartographer/sensor/proto/sensor.proto +++ b/cartographer/sensor/proto/sensor.proto @@ -41,6 +41,7 @@ message TimedPointCloudData { transform.proto.Vector3f origin = 2; repeated transform.proto.Vector4f point_data_legacy = 3; repeated TimedRangefinderPoint point_data = 4; + repeated float intensities = 5; } // Proto representation of ::cartographer::sensor::RangeData. diff --git a/cartographer/sensor/timed_point_cloud_data.cc b/cartographer/sensor/timed_point_cloud_data.cc index 63a17af..2f01f08 100644 --- a/cartographer/sensor/timed_point_cloud_data.cc +++ b/cartographer/sensor/timed_point_cloud_data.cc @@ -31,10 +31,15 @@ proto::TimedPointCloudData ToProto( for (const TimedRangefinderPoint& range : timed_point_cloud_data.ranges) { *proto.add_point_data() = ToProto(range); } + for (const float intensity : timed_point_cloud_data.intensities) { + proto.add_intensities(intensity); + } return proto; } TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto) { + CHECK(proto.intensities().size() == 0 || + proto.intensities().size() == proto.point_data().size()); TimedPointCloud timed_point_cloud; if (proto.point_data().size() > 0) { timed_point_cloud.reserve(proto.point_data().size()); @@ -50,7 +55,9 @@ TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto) { } return TimedPointCloudData{common::FromUniversal(proto.timestamp()), transform::ToEigen(proto.origin()), - timed_point_cloud}; + timed_point_cloud, + std::vector(proto.intensities().begin(), + proto.intensities().end())}; } } // namespace sensor diff --git a/cartographer/sensor/timed_point_cloud_data.h b/cartographer/sensor/timed_point_cloud_data.h index 4901e35..8bdd928 100644 --- a/cartographer/sensor/timed_point_cloud_data.h +++ b/cartographer/sensor/timed_point_cloud_data.h @@ -28,11 +28,14 @@ struct TimedPointCloudData { common::Time time; Eigen::Vector3f origin; TimedPointCloud ranges; + // 'intensities' has to be same size as 'ranges', or empty. + std::vector intensities; }; struct TimedPointCloudOriginData { struct RangeMeasurement { TimedRangefinderPoint point_time; + float intensity; size_t origin_index; }; common::Time time;