From d92040d0e6e7e7a8fbcd46096a7fb308e134f0c9 Mon Sep 17 00:00:00 2001 From: gaschler Date: Wed, 17 Jan 2018 20:43:20 +0100 Subject: [PATCH] Test Collator for multiple trajectories (#829) --- cartographer/sensor/collator_test.cc | 178 ++++++++++++++++++++------- 1 file changed, 134 insertions(+), 44 deletions(-) diff --git a/cartographer/sensor/collator_test.cc b/cartographer/sensor/collator_test.cc index cbd311e..9662fb3 100644 --- a/cartographer/sensor/collator_test.cc +++ b/cartographer/sensor/collator_test.cc @@ -19,10 +19,11 @@ #include #include -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/make_unique.h" #include "cartographer/common/time.h" -#include "cartographer/sensor/proto/sensor.pb.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/sensor/test_helpers.h" #include "cartographer/sensor/timed_point_cloud_data.h" #include "gtest/gtest.h" @@ -30,62 +31,151 @@ namespace cartographer { namespace sensor { namespace { +using test::CollatorInput; +using test::CollatorOutput; + TEST(Collator, Ordering) { + const int kTrajectoryId = 0; const std::array kSensorId = { {"horizontal_rangefinder", "vertical_rangefinder", "imu", "odometry"}}; - TimedPointCloudData zero{ - common::FromUniversal(0), Eigen::Vector3f::Zero(), {}}; - TimedPointCloudData first{ - common::FromUniversal(100), Eigen::Vector3f::Zero(), {}}; - TimedPointCloudData second{ - common::FromUniversal(200), Eigen::Vector3f::Zero(), {}}; - ImuData third{common::FromUniversal(300)}; - TimedPointCloudData fourth{ - common::FromUniversal(400), Eigen::Vector3f::Zero(), {}}; - TimedPointCloudData fifth{ - common::FromUniversal(500), Eigen::Vector3f::Zero(), {}}; - OdometryData sixth{common::FromUniversal(600), - transform::Rigid3d::Identity()}; - std::vector> received; + std::vector input_data; + // Send each sensor_id once to establish a common start time. + input_data.push_back( + CollatorInput::CreateTimedPointCloudData(kTrajectoryId, kSensorId[0], 0)); + input_data.push_back( + CollatorInput::CreateTimedPointCloudData(kTrajectoryId, kSensorId[1], 0)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId, kSensorId[2], 0)); + input_data.push_back( + CollatorInput::CreateOdometryData(kTrajectoryId, kSensorId[3], 0)); + + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId, kSensorId[0], 100)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId, kSensorId[1], 200)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId, kSensorId[2], 300)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId, kSensorId[0], 400)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId, kSensorId[1], 500)); + input_data.push_back( + CollatorInput::CreateOdometryData(kTrajectoryId, kSensorId[3], 600)); + + std::vector received; Collator collator; collator.AddTrajectory( - 0, std::unordered_set(kSensorId.begin(), kSensorId.end()), - [&received](const std::string& sensor_id, std::unique_ptr data) { - received.push_back(std::make_pair(sensor_id, data->GetTime())); + kTrajectoryId, + std::unordered_set(kSensorId.begin(), kSensorId.end()), + [&received, kTrajectoryId](const std::string& sensor_id, + std::unique_ptr data) { + received.push_back(CollatorOutput(kTrajectoryId, data->GetSensorId(), + data->GetTime())); }); - constexpr int kTrajectoryId = 0; + input_data[0].MoveToCollator(&collator); + input_data[1].MoveToCollator(&collator); + input_data[2].MoveToCollator(&collator); + input_data[3].MoveToCollator(&collator); - // Establish a common start time. - collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[0], zero)); - collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[1], zero)); - collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[2], zero)); - collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[3], zero)); - - collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[0], first)); - collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[3], sixth)); - collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[0], fourth)); - collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[1], second)); - collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[1], fifth)); - collator.AddSensorData(kTrajectoryId, MakeDispatchable(kSensorId[2], third)); + input_data[4].MoveToCollator(&collator); + input_data[9].MoveToCollator(&collator); + input_data[7].MoveToCollator(&collator); + input_data[5].MoveToCollator(&collator); + input_data[8].MoveToCollator(&collator); + input_data[6].MoveToCollator(&collator); + EXPECT_EQ(kTrajectoryId, collator.GetBlockingTrajectoryId().value()); ASSERT_EQ(7, received.size()); - EXPECT_EQ(100, common::ToUniversal(received[4].second)); - EXPECT_EQ(kSensorId[0], received[4].first); - EXPECT_EQ(200, common::ToUniversal(received[5].second)); - EXPECT_EQ(kSensorId[1], received[5].first); - EXPECT_EQ(300, common::ToUniversal(received[6].second)); - EXPECT_EQ(kSensorId[2], received[6].first); + EXPECT_EQ(input_data[4].expected_output, received[4]); + EXPECT_EQ(input_data[5].expected_output, received[5]); + EXPECT_EQ(input_data[6].expected_output, received[6]); + collator.FinishTrajectory(kTrajectoryId); collator.Flush(); + ASSERT_EQ(input_data.size(), received.size()); + for (size_t i = 4; i < input_data.size(); ++i) { + EXPECT_EQ(input_data[i].expected_output, received[i]); + } +} - ASSERT_EQ(10, received.size()); - EXPECT_EQ(kSensorId[0], received[7].first); - EXPECT_EQ(500, common::ToUniversal(received[8].second)); - EXPECT_EQ(kSensorId[1], received[8].first); - EXPECT_EQ(600, common::ToUniversal(received[9].second)); - EXPECT_EQ(kSensorId[3], received[9].first); +TEST(Collator, OrderingMultipleTrajectories) { + const int kTrajectoryId[] = {8, 5}; + const std::array kSensorId = {{"my_points", "some_imu"}}; + + std::vector input_data; + // Send each sensor_id once to establish a common start time. + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[0], kSensorId[0], 0)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId[0], kSensorId[1], 0)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[1], kSensorId[0], 0)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 0)); + + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[0], kSensorId[0], 100)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 200)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId[0], kSensorId[1], 300)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[1], kSensorId[0], 400)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[1], kSensorId[0], 400)); + input_data.push_back(CollatorInput::CreateTimedPointCloudData( + kTrajectoryId[1], kSensorId[0], 500)); + input_data.push_back( + CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 600)); + + std::vector received; + Collator collator; + collator.AddTrajectory( + kTrajectoryId[0], + std::unordered_set(kSensorId.begin(), kSensorId.end()), + [&received, kTrajectoryId](const std::string& sensor_id, + std::unique_ptr data) { + received.push_back(CollatorOutput(kTrajectoryId[0], data->GetSensorId(), + data->GetTime())); + }); + collator.AddTrajectory( + kTrajectoryId[1], + std::unordered_set(kSensorId.begin(), kSensorId.end()), + [&received, kTrajectoryId](const std::string& sensor_id, + std::unique_ptr data) { + received.push_back(CollatorOutput(kTrajectoryId[1], data->GetSensorId(), + data->GetTime())); + }); + + input_data[0].MoveToCollator(&collator); + input_data[1].MoveToCollator(&collator); + input_data[2].MoveToCollator(&collator); + input_data[3].MoveToCollator(&collator); + + input_data[4].MoveToCollator(&collator); + input_data[6].MoveToCollator(&collator); + EXPECT_EQ(kTrajectoryId[1], collator.GetBlockingTrajectoryId().value()); + input_data[7].MoveToCollator(&collator); + input_data[8].MoveToCollator(&collator); + EXPECT_EQ(kTrajectoryId[1], collator.GetBlockingTrajectoryId().value()); + input_data[5].MoveToCollator(&collator); + EXPECT_EQ(kTrajectoryId[0], collator.GetBlockingTrajectoryId().value()); + input_data[10].MoveToCollator(&collator); + input_data[9].MoveToCollator(&collator); + EXPECT_EQ(kTrajectoryId[0], collator.GetBlockingTrajectoryId().value()); + + ASSERT_EQ(5, received.size()); + EXPECT_EQ(input_data[4].expected_output, received[4]); + + collator.FinishTrajectory(kTrajectoryId[0]); + collator.FinishTrajectory(kTrajectoryId[1]); + collator.Flush(); + ASSERT_EQ(input_data.size(), received.size()); + for (size_t i = 4; i < input_data.size(); ++i) { + EXPECT_EQ(input_data[i].expected_output, received[i]); + } } } // namespace