range_data_collator_test.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2018 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #include "cartographer/mapping/internal/range_data_collator.h"
18 
19 #include "cartographer/common/time.h"
20 #include "gmock/gmock.h"
21 #include "gtest/gtest.h"
22 
23 namespace cartographer {
24 namespace mapping {
25 namespace {
26 
27 const int kNumSamples = 10;
28 
29 sensor::TimedPointCloudData CreateFakeRangeData(int from, int to) {
30  double duration = common::ToSeconds(common::FromUniversal(to) -
31  common::FromUniversal(from));
32  sensor::TimedPointCloudData result{common::FromUniversal(to),
33  Eigen::Vector3f(0., 1., 2.),
34  sensor::TimedPointCloud(kNumSamples)};
35  for (int i = 0; i < kNumSamples; ++i) {
36  double fraction = static_cast<double>(i) / (kNumSamples - 1);
37  double relative_time = (1.f - fraction) * -duration;
38  result.ranges[i] = Eigen::Vector4f(1., 2., 3., relative_time);
39  }
40  return result;
41 }
42 
43 bool ArePointTimestampsSorted(const sensor::TimedPointCloudOriginData& data) {
44  std::vector<float> timestamps;
45  timestamps.reserve(data.ranges.size());
46  for (const auto& range : data.ranges) {
47  timestamps.push_back(range.point_time[3]);
48  }
49  return std::is_sorted(timestamps.begin(), timestamps.end());
50 }
51 
52 TEST(RangeDataCollatorTest, SingleSensor) {
53  const std::string sensor_id = "single_sensor";
54  RangeDataCollator collator({sensor_id});
55  auto output_0 =
56  collator.AddRangeData(sensor_id, CreateFakeRangeData(200, 300));
57  EXPECT_EQ(common::ToUniversal(output_0.time), 300);
58  EXPECT_EQ(output_0.origins.size(), 1);
59  EXPECT_EQ(output_0.ranges.size(), kNumSamples);
60  EXPECT_TRUE(ArePointTimestampsSorted(output_0));
61  auto output_1 =
62  collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500));
63  EXPECT_EQ(common::ToUniversal(output_1.time), 500);
64  EXPECT_EQ(output_1.origins.size(), 1);
65  ASSERT_EQ(output_1.ranges.size(), kNumSamples);
66  EXPECT_TRUE(ArePointTimestampsSorted(output_1));
67  EXPECT_NEAR(common::ToUniversal(
68  output_1.time +
69  common::FromSeconds(output_1.ranges[0].point_time[3])),
70  300, 2);
71  auto output_2 =
72  collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510));
73  EXPECT_EQ(common::ToUniversal(output_2.time), 510);
74  EXPECT_EQ(output_2.origins.size(), 1);
75  EXPECT_EQ(output_2.ranges.size(), 1);
76  EXPECT_EQ(output_2.ranges[0].point_time[3], 0.f);
77  EXPECT_TRUE(ArePointTimestampsSorted(output_2));
78 }
79 
80 TEST(RangeDataCollatorTest, SingleSensorEmptyData) {
81  const std::string sensor_id = "single_sensor";
82  RangeDataCollator collator({sensor_id});
83  sensor::TimedPointCloudData empty_data{common::FromUniversal(300)};
84  auto output_0 = collator.AddRangeData(sensor_id, empty_data);
85  EXPECT_EQ(output_0.time, empty_data.time);
86  EXPECT_EQ(output_0.ranges.size(), empty_data.ranges.size());
87  EXPECT_TRUE(ArePointTimestampsSorted(output_0));
88  auto output_1 =
89  collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500));
90  EXPECT_EQ(common::ToUniversal(output_1.time), 500);
91  EXPECT_EQ(output_1.origins.size(), 1);
92  ASSERT_EQ(output_1.ranges.size(), kNumSamples);
93  EXPECT_TRUE(ArePointTimestampsSorted(output_1));
94  EXPECT_NEAR(common::ToUniversal(
95  output_1.time +
96  common::FromSeconds(output_1.ranges[0].point_time[3])),
97  300, 2);
98  auto output_2 =
99  collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510));
100  EXPECT_EQ(common::ToUniversal(output_2.time), 510);
101  EXPECT_EQ(output_2.origins.size(), 1);
102  EXPECT_EQ(output_2.ranges.size(), 1);
103  EXPECT_EQ(output_2.ranges[0].point_time[3], 0.f);
104  EXPECT_TRUE(ArePointTimestampsSorted(output_2));
105 }
106 
107 TEST(RangeDataCollatorTest, TwoSensors) {
108  const std::string sensor_0 = "sensor_0";
109  const std::string sensor_1 = "sensor_1";
110  RangeDataCollator collator({sensor_0, sensor_1});
111  auto output_0 =
112  collator.AddRangeData(sensor_0, CreateFakeRangeData(200, 300));
113  EXPECT_EQ(output_0.ranges.size(), 0);
114  auto output_1 =
115  collator.AddRangeData(sensor_1, CreateFakeRangeData(-1000, 310));
116  EXPECT_EQ(output_1.origins.size(), 2);
117  EXPECT_EQ(common::ToUniversal(output_1.time), 300);
118  ASSERT_EQ(output_1.ranges.size(), 2 * kNumSamples - 1);
119  EXPECT_NEAR(common::ToUniversal(
120  output_1.time +
121  common::FromSeconds(output_1.ranges[0].point_time[3])),
122  -1000, 2);
123  EXPECT_EQ(output_1.ranges.back().point_time[3], 0.f);
124  EXPECT_TRUE(ArePointTimestampsSorted(output_1));
125  auto output_2 =
126  collator.AddRangeData(sensor_0, CreateFakeRangeData(300, 500));
127  EXPECT_EQ(output_2.origins.size(), 2);
128  EXPECT_EQ(common::ToUniversal(output_2.time), 310);
129  ASSERT_EQ(output_2.ranges.size(), 2);
130  EXPECT_NEAR(common::ToUniversal(
131  output_2.time +
132  common::FromSeconds(output_2.ranges[0].point_time[3])),
133  300, 2);
134  EXPECT_EQ(output_2.ranges.back().point_time[3], 0.f);
135  EXPECT_TRUE(ArePointTimestampsSorted(output_2));
136  // Sending the same sensor will flush everything before.
137  auto output_3 =
138  collator.AddRangeData(sensor_0, CreateFakeRangeData(600, 700));
139  EXPECT_EQ(common::ToUniversal(output_3.time), 500);
140  EXPECT_EQ(
141  output_1.ranges.size() + output_2.ranges.size() + output_3.ranges.size(),
142  3 * kNumSamples);
143  EXPECT_EQ(output_3.ranges.back().point_time[3], 0.f);
144  EXPECT_TRUE(ArePointTimestampsSorted(output_3));
145 }
146 
147 TEST(RangeDataCollatorTest, ThreeSensors) {
148  const std::string sensor_0 = "sensor_0";
149  const std::string sensor_1 = "sensor_1";
150  const std::string sensor_2 = "sensor_2";
151  RangeDataCollator collator({sensor_0, sensor_1, sensor_2});
152  auto output_0 =
153  collator.AddRangeData(sensor_0, CreateFakeRangeData(100, 200));
154  EXPECT_EQ(output_0.ranges.size(), 0);
155  auto output_1 =
156  collator.AddRangeData(sensor_1, CreateFakeRangeData(199, 250));
157  EXPECT_EQ(output_1.ranges.size(), 0);
158  auto output_2 =
159  collator.AddRangeData(sensor_2, CreateFakeRangeData(210, 300));
160  EXPECT_EQ(output_2.ranges.size(), kNumSamples + 1);
161  EXPECT_TRUE(ArePointTimestampsSorted(output_2));
162  auto output_3 =
163  collator.AddRangeData(sensor_2, CreateFakeRangeData(400, 500));
164  EXPECT_EQ(output_2.ranges.size() + output_3.ranges.size(), 3 * kNumSamples);
165  EXPECT_TRUE(ArePointTimestampsSorted(output_3));
166 }
167 
168 } // namespace
169 } // namespace mapping
170 } // namespace cartographer
cartographer::common::FromUniversal
Time FromUniversal(const int64 ticks)
Definition: time.cc:34
cartographer::common::FromSeconds
Duration FromSeconds(const double seconds)
Definition: time.cc:24
range_data_collator.h
cartographer::common::ToUniversal
int64 ToUniversal(const Time time)
Definition: time.cc:36
cartographer::common::ToSeconds
double ToSeconds(const Duration duration)
Definition: time.cc:29
cartographer::sensor::TimedPointCloud
std::vector< Eigen::Vector4f > TimedPointCloud
Definition: point_cloud.h:39
cartographer::mapping::TEST
TEST(TrajectoryConnectivityStateTest, UnknownTrajectory)
Definition: trajectory_connectivity_state_test.cc:24

cartographer
Author(s): The Cartographer Authors
autogenerated on Mon Feb 28 2022 22:00:58