206 lines
8.3 KiB
C++
206 lines
8.3 KiB
C++
/*
|
|
* 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,
|
|
bool fake_intensities) {
|
|
double duration = common::ToSeconds(common::FromUniversal(to) -
|
|
common::FromUniversal(from));
|
|
sensor::TimedPointCloudData result{
|
|
common::FromUniversal(to), Eigen::Vector3f(0., 1., 2.), {}, {}};
|
|
result.ranges.reserve(kNumSamples);
|
|
for (int i = 0; i < kNumSamples; ++i) {
|
|
double fraction = static_cast<double>(i) / (kNumSamples - 1);
|
|
float relative_time = (1. - fraction) * -duration;
|
|
result.ranges.push_back(
|
|
{Eigen::Vector3f{1., 2., static_cast<float>(fraction)}, relative_time});
|
|
if (fake_intensities) {
|
|
result.intensities.push_back(result.ranges.back().position.z());
|
|
}
|
|
}
|
|
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.time);
|
|
}
|
|
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, 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, false));
|
|
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.time)),
|
|
300, 2);
|
|
auto output_2 =
|
|
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);
|
|
EXPECT_EQ(output_2.ranges[0].point_time.time, 0.f);
|
|
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, false));
|
|
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.time)),
|
|
300, 2);
|
|
auto output_2 =
|
|
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);
|
|
EXPECT_EQ(output_2.ranges[0].point_time.time, 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, false));
|
|
EXPECT_EQ(output_0.ranges.size(), 0);
|
|
auto output_1 =
|
|
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);
|
|
EXPECT_NEAR(common::ToUniversal(
|
|
output_1.time +
|
|
common::FromSeconds(output_1.ranges[0].point_time.time)),
|
|
-1000, 2);
|
|
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, false));
|
|
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.time)),
|
|
300, 2);
|
|
EXPECT_EQ(output_2.ranges.back().point_time.time, 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, false));
|
|
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.time, 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, false));
|
|
EXPECT_EQ(output_0.ranges.size(), 0);
|
|
auto output_1 =
|
|
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, false));
|
|
EXPECT_EQ(output_2.ranges.size(), kNumSamples + 1);
|
|
EXPECT_TRUE(ArePointTimestampsSorted(output_2));
|
|
auto output_3 =
|
|
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
|