From 1187480fe6b484fedfae88de22a3cfb61760fd55 Mon Sep 17 00:00:00 2001 From: gaschler Date: Tue, 13 Mar 2018 11:08:01 +0100 Subject: [PATCH] Introduce RangeDataCollator (#975) Synchronizes and merge-sorts TimedPointCloudData by per-point timestamps from multiple sensors. [RFC=0017](https://github.com/googlecartographer/rfcs/blob/master/text/0017-synchronize-points.md) --- .../mapping/internal/range_data_collator.cc | 119 +++++++++++++++ .../mapping/internal/range_data_collator.h | 57 +++++++ .../internal/range_data_collator_test.cc | 143 ++++++++++++++++++ cartographer/sensor/timed_point_cloud_data.h | 10 ++ 4 files changed, 329 insertions(+) create mode 100644 cartographer/mapping/internal/range_data_collator.cc create mode 100644 cartographer/mapping/internal/range_data_collator.h create mode 100644 cartographer/mapping/internal/range_data_collator_test.cc diff --git a/cartographer/mapping/internal/range_data_collator.cc b/cartographer/mapping/internal/range_data_collator.cc new file mode 100644 index 0000000..f285d2d --- /dev/null +++ b/cartographer/mapping/internal/range_data_collator.cc @@ -0,0 +1,119 @@ +/* + * 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. + */ + +#include "cartographer/mapping/internal/range_data_collator.h" + +#include + +#include "cartographer/common/make_unique.h" +#include "cartographer/mapping/local_slam_result_data.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData( + const std::string& sensor_id, + const sensor::TimedPointCloudData& timed_point_cloud_data) { + CHECK(expected_sensor_ids_.count(sensor_id) != 0); + // TODO(gaschler): These two cases can probably be one. + if (id_to_pending_data_.count(sensor_id) != 0) { + current_start_ = current_end_; + // When we have two messages of the same sensor, move forward the older of + // 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); + return result; + } + id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data); + if (expected_sensor_ids_.size() != id_to_pending_data_.size()) { + return {}; + } + current_start_ = current_end_; + // We have messages from all sensors, move forward to oldest. + common::Time oldest_timestamp = common::Time::max(); + for (const auto& pair : id_to_pending_data_) { + oldest_timestamp = std::min(oldest_timestamp, pair.second.time); + } + current_end_ = oldest_timestamp; + return CropAndMerge(); +} + +sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() { + sensor::TimedPointCloudOriginData result{current_end_, {}, {}}; + bool warned_for_dropped_points = false; + 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; + + auto overlap_begin = ranges.begin(); + while (overlap_begin < ranges.end() && + data.time + common::FromSeconds((*overlap_begin)[3]) < + current_start_) { + ++overlap_begin; + } + auto overlap_end = overlap_begin; + while (overlap_end < ranges.end() && + data.time + common::FromSeconds((*overlap_end)[3]) <= current_end_) { + ++overlap_end; + } + if (ranges.begin() < overlap_begin && !warned_for_dropped_points) { + LOG(WARNING) << "Dropped " << std::distance(ranges.begin(), overlap_begin) + << " earlier points."; + warned_for_dropped_points = true; + } + + // Copy overlapping range. + 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_); + 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; + result.ranges.push_back(point); + } + } + + // Drop buffered points until overlap_end. + if (overlap_end == ranges.begin()) { + ++it; + } else if (overlap_end == ranges.end()) { + it = id_to_pending_data_.erase(it); + } else { + data = sensor::TimedPointCloudData{ + data.time, data.origin, + sensor::TimedPointCloud(overlap_end, ranges.end())}; + ++it; + } + } + + 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 result; +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/internal/range_data_collator.h b/cartographer/mapping/internal/range_data_collator.h new file mode 100644 index 0000000..68f3f48 --- /dev/null +++ b/cartographer/mapping/internal/range_data_collator.h @@ -0,0 +1,57 @@ +/* + * 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_MAPPING_INTERNAL_RANGE_DATA_COLLATOR_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_RANGE_DATA_COLLATOR_H_ + +#include + +#include "cartographer/common/make_unique.h" +#include "cartographer/sensor/timed_point_cloud_data.h" + +namespace cartographer { +namespace mapping { + +// Synchronizes TimedPointCloudData from different sensors. Input needs only be +// monotonous in 'TimedPointCloudData::time', output is monotonous in per-point +// timing. Up to one message per sensor is buffered, so a delay of the period of +// the slowest sensor may be introduced, which can be alleviated by passing +// subdivisions. +class RangeDataCollator { + public: + explicit RangeDataCollator( + const std::vector& expected_range_sensor_ids) + : expected_sensor_ids_(expected_range_sensor_ids.begin(), + expected_range_sensor_ids.end()) {} + + sensor::TimedPointCloudOriginData AddRangeData( + const std::string& sensor_id, + const sensor::TimedPointCloudData& timed_point_cloud_data); + + private: + sensor::TimedPointCloudOriginData CropAndMerge(); + + const std::set expected_sensor_ids_; + // Store at most one message for each sensor. + std::map id_to_pending_data_; + common::Time current_start_ = common::Time::min(); + common::Time current_end_ = common::Time::min(); +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_RANGE_DATA_COLLATOR_H_ diff --git a/cartographer/mapping/internal/range_data_collator_test.cc b/cartographer/mapping/internal/range_data_collator_test.cc new file mode 100644 index 0000000..8f0c7eb --- /dev/null +++ b/cartographer/mapping/internal/range_data_collator_test.cc @@ -0,0 +1,143 @@ +/* + * 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. + */ + +#include "cartographer/mapping/internal/range_data_collator.h" + +#include "cartographer/common/time.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +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)}; + 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); + } + return result; +} + +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]); + } + return std::is_sorted(timestamps.begin(), timestamps.end()); +} + +TEST(RangeDataCollatorTest, SingleSensor) { + const std::string sensor_id = "single_sensor"; + RangeDataCollator collator({sensor_id}); + auto output_0 = + collator.AddRangeData(sensor_id, CreateFakeRangeData(200, 300)); + 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)); + 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"; + RangeDataCollator collator({sensor_0, sensor_1}); + auto output_0 = + collator.AddRangeData(sensor_0, CreateFakeRangeData(200, 300)); + EXPECT_EQ(output_0.ranges.size(), 0); + auto output_1 = + collator.AddRangeData(sensor_1, CreateFakeRangeData(-1000, 310)); + EXPECT_EQ(output_1.origins.size(), 2); + EXPECT_EQ(common::ToUniversal(output_1.time), 300); + 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])), + -1000, 2); + EXPECT_EQ(output_1.ranges.back().point_time[3], 0.f); + EXPECT_TRUE(ArePointTimestampsSorted(output_1)); + auto output_2 = + collator.AddRangeData(sensor_0, CreateFakeRangeData(300, 500)); + EXPECT_EQ(output_2.origins.size(), 2); + EXPECT_EQ(common::ToUniversal(output_2.time), 310); + ASSERT_EQ(output_2.ranges.size(), 2); + EXPECT_NEAR(common::ToUniversal( + output_2.time + + common::FromSeconds(output_2.ranges[0].point_time[3])), + 300, 2); + EXPECT_EQ(output_2.ranges.back().point_time[3], 0.f); + EXPECT_TRUE(ArePointTimestampsSorted(output_2)); + // Sending the same sensor will flush everything before. + auto output_3 = + collator.AddRangeData(sensor_0, CreateFakeRangeData(600, 700)); + EXPECT_EQ(common::ToUniversal(output_3.time), 500); + 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_TRUE(ArePointTimestampsSorted(output_3)); +} + +TEST(RangeDataCollatorTest, ThreeSensors) { + 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)); + EXPECT_EQ(output_0.ranges.size(), 0); + auto output_1 = + collator.AddRangeData(sensor_1, CreateFakeRangeData(199, 250)); + EXPECT_EQ(output_1.ranges.size(), 0); + auto output_2 = + collator.AddRangeData(sensor_2, CreateFakeRangeData(210, 300)); + EXPECT_EQ(output_2.ranges.size(), kNumSamples + 1); + EXPECT_TRUE(ArePointTimestampsSorted(output_2)); + auto output_3 = + collator.AddRangeData(sensor_2, CreateFakeRangeData(400, 500)); + EXPECT_EQ(output_2.ranges.size() + output_3.ranges.size(), 3 * kNumSamples); + EXPECT_TRUE(ArePointTimestampsSorted(output_3)); +} + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/sensor/timed_point_cloud_data.h b/cartographer/sensor/timed_point_cloud_data.h index 6ad5836..2025ec4 100644 --- a/cartographer/sensor/timed_point_cloud_data.h +++ b/cartographer/sensor/timed_point_cloud_data.h @@ -30,6 +30,16 @@ struct TimedPointCloudData { sensor::TimedPointCloud ranges; }; +struct TimedPointCloudOriginData { + struct RangeMeasurement { + Eigen::Vector4f point_time; + size_t origin_index; + }; + common::Time time; + std::vector origins; + std::vector ranges; +}; + // Converts 'timed_point_cloud_data' to a proto::TimedPointCloudData. proto::TimedPointCloudData ToProto( const TimedPointCloudData& timed_point_cloud_data);