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)master
gaschler
Wally B. Feed
parent
976736051c
commit
1187480fe6
|
|
@ -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 <memory>
|
||||
|
||||
#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
|
||||
|
|
@ -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 <memory>
|
||||
|
||||
#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<std::string>& 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<std::string> expected_sensor_ids_;
|
||||
// Store at most one message for each sensor.
|
||||
std::map<std::string, sensor::TimedPointCloudData> 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_
|
||||
|
|
@ -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<double>(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<float> 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
|
||||
|
|
@ -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<Eigen::Vector3f> origins;
|
||||
std::vector<RangeMeasurement> ranges;
|
||||
};
|
||||
|
||||
// Converts 'timed_point_cloud_data' to a proto::TimedPointCloudData.
|
||||
proto::TimedPointCloudData ToProto(
|
||||
const TimedPointCloudData& timed_point_cloud_data);
|
||||
|
|
|
|||
Loading…
Reference in New Issue