12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

TrajectoryCollator (#827) 

Introduces TrajectorCollator, which collates sensor data ignoring
other trajectories.
Tests the same.

[RFC=0008](https://github.com/googlecartographer/rfcs/blob/master/text/0008-collator-interface.md)
master
gaschler 2018-01-17 17:48:20 +01:00 committed by Wally B. Feed
parent ffdbf1c161
commit 70e378b7c5
4 changed files with 299 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
cartographer/sensor/test_helpers.h
View File

@ -20,6 +20,13 @@
#include <string>
#include <tuple>
#include "cartographer/common/make_unique.h"
#include "cartographer/common/time.h"
#include "cartographer/sensor/collator_interface.h"
#include "cartographer/sensor/dispatchable.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/sensor/timed_point_cloud_data.h"
#include "gmock/gmock.h"
namespace cartographer {
@ -29,6 +36,51 @@ MATCHER_P(Near, point, std::string(negation ? "Doesn't" : "Does") + " match.") {
return arg.isApprox(point, 0.001f);
}
namespace test {
typedef std::tuple<int /* trajectory_id */, std::string /* sensor_id */,
common::Time>
CollatorOutput;
struct CollatorInput {
static CollatorInput CreateImuData(int trajectory_id,
const std::string& sensor_id, int time) {
return CollatorInput{
trajectory_id,
MakeDispatchable(sensor_id, ImuData{common::FromUniversal(time)}),
CollatorOutput{trajectory_id, sensor_id, common::FromUniversal(time)}};
}
static CollatorInput CreateTimedPointCloudData(int trajectory_id,
const std::string& sensor_id,
int time) {
return CollatorInput{
trajectory_id,
MakeDispatchable(
sensor_id,
TimedPointCloudData{
common::FromUniversal(time), Eigen::Vector3f::Zero(), {}}),
CollatorOutput{trajectory_id, sensor_id, common::FromUniversal(time)}};
}
static CollatorInput CreateOdometryData(int trajectory_id,
const std::string& sensor_id,
int time) {
return CollatorInput{
trajectory_id,
MakeDispatchable(sensor_id,
OdometryData{common::FromUniversal(time),
transform::Rigid3d::Identity()}),
CollatorOutput{trajectory_id, sensor_id, common::FromUniversal(time)}};
}
void MoveToCollator(CollatorInterface* collator) {
collator->AddSensorData(trajectory_id, std::move(data));
}
const int trajectory_id;
std::unique_ptr<sensor::Data> data;
const CollatorOutput expected_output;
};
} // namespace test
} // namespace sensor
} // namespace cartographer

61
cartographer/sensor/trajectory_collator.cc Normal file
View File

@ -0,0 +1,61 @@
/*
* 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/sensor/trajectory_collator.h"
namespace cartographer {
namespace sensor {
void TrajectoryCollator::AddTrajectory(
const int trajectory_id,
const std::unordered_set<std::string>& expected_sensor_ids,
const Callback& callback) {
CHECK(trajectory_to_queue_.count(trajectory_id) == 0);
for (const auto& sensor_id : expected_sensor_ids) {
const auto queue_key = QueueKey{trajectory_id, sensor_id};
trajectory_to_queue_[trajectory_id].AddQueue(
queue_key, [callback, sensor_id](std::unique_ptr<Data> data) {
callback(sensor_id, std::move(data));
});
trajectory_to_queue_keys_[trajectory_id].push_back(queue_key);
}
}
void TrajectoryCollator::FinishTrajectory(const int trajectory_id) {
for (const auto& queue_key : trajectory_to_queue_keys_[trajectory_id]) {
trajectory_to_queue_.at(trajectory_id).MarkQueueAsFinished(queue_key);
}
}
void TrajectoryCollator::AddSensorData(const int trajectory_id,
std::unique_ptr<Data> data) {
QueueKey queue_key{trajectory_id, data->GetSensorId()};
trajectory_to_queue_.at(trajectory_id)
.Add(std::move(queue_key), std::move(data));
}
void TrajectoryCollator::Flush() {
for (auto& it : trajectory_to_queue_) {
it.second.Flush();
}
}
common::optional<int> TrajectoryCollator::GetBlockingTrajectoryId() const {
return common::optional<int>();
}
} // namespace sensor
} // namespace cartographer

65
cartographer/sensor/trajectory_collator.h Normal file
View File

@ -0,0 +1,65 @@
/*
* 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_SENSOR_TRAJECTORY_COLLATOR_H_
#define CARTOGRAPHER_SENSOR_TRAJECTORY_COLLATOR_H_
#include <memory>
#include <unordered_map>
#include <vector>
#include "cartographer/sensor/collator_interface.h"
#include "cartographer/sensor/ordered_multi_queue.h"
namespace cartographer {
namespace sensor {
// Waits to see at least one data item for all sensor ids and dispatches data
// in merge-sorted order. Contrary to 'Collator', it does not wait for other
// trajectories.
// Also contrary to 'Collator', whose output is deterministic, the sequence in
// which data is dispatched is not sorted, so non-deterministic input sequences
// will result in non-deterministic output.
class TrajectoryCollator : public CollatorInterface {
public:
TrajectoryCollator() {}
TrajectoryCollator(const TrajectoryCollator&) = delete;
TrajectoryCollator& operator=(const TrajectoryCollator&) = delete;
void AddTrajectory(int trajectory_id,
const std::unordered_set<std::string>& expected_sensor_ids,
const Callback& callback) override;
void FinishTrajectory(int trajectory_id) override;
void AddSensorData(int trajectory_id, std::unique_ptr<Data> data) override;
void Flush() override;
common::optional<int> GetBlockingTrajectoryId() const override;
private:
std::unordered_map<int, OrderedMultiQueue> trajectory_to_queue_;
// Map of trajectory ID to all associated QueueKeys.
std::unordered_map<int, std::vector<QueueKey>> trajectory_to_queue_keys_;
};
} // namespace sensor
} // namespace cartographer
#endif // CARTOGRAPHER_SENSOR_TRAJECTORY_COLLATOR_H_

121
cartographer/sensor/trajectory_collator_test.cc Normal file
View File

@ -0,0 +1,121 @@
/*
* 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/sensor/trajectory_collator.h"
#include <array>
#include <memory>
#include "cartographer/common/make_unique.h"
#include "cartographer/common/time.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"
namespace cartographer {
namespace sensor {
namespace {
using test::CollatorInput;
using test::CollatorOutput;
TEST(TrajectoryCollator, OrderingMultipleTrajectories) {
const int kTrajectoryId[] = {4, 7};
const std::array<std::string, 2> kSensorId = {{"my_points", "some_imu"}};
std::vector<CollatorInput> input_data;
input_data.push_back(CollatorInput::CreateTimedPointCloudData(
kTrajectoryId[0], kSensorId[0], 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::CreateImuData(kTrajectoryId[0], kSensorId[1], 0));
input_data.push_back(CollatorInput::CreateTimedPointCloudData(
kTrajectoryId[1], kSensorId[0], 100));
input_data.push_back(CollatorInput::CreateTimedPointCloudData(
kTrajectoryId[0], kSensorId[0], 50));
input_data.push_back(
CollatorInput::CreateImuData(kTrajectoryId[0], kSensorId[1], 60));
input_data.push_back(CollatorInput::CreateTimedPointCloudData(
kTrajectoryId[1], kSensorId[0], 150));
input_data.push_back(
CollatorInput::CreateImuData(kTrajectoryId[1], kSensorId[1], 120));
std::vector<CollatorOutput> received;
TrajectoryCollator collator;
collator.AddTrajectory(
kTrajectoryId[0],
std::unordered_set<std::string>(kSensorId.begin(), kSensorId.end()),
[&received, kTrajectoryId](const std::string& sensor_id,
std::unique_ptr<Data> data) {
received.push_back(CollatorOutput(kTrajectoryId[0], data->GetSensorId(),
data->GetTime()));
});
collator.AddTrajectory(
kTrajectoryId[1],
std::unordered_set<std::string>(kSensorId.begin(), kSensorId.end()),
[&received, kTrajectoryId](const std::string& sensor_id,
std::unique_ptr<Data> data) {
received.push_back(CollatorOutput(kTrajectoryId[1], data->GetSensorId(),
data->GetTime()));
});
// Send each sensor_id once to establish a common start time.
input_data[0].MoveToCollator(&collator);
input_data[1].MoveToCollator(&collator);
EXPECT_EQ(0, received.size());
input_data[2].MoveToCollator(&collator);
input_data[3].MoveToCollator(&collator);
EXPECT_EQ(2, received.size());
EXPECT_EQ(input_data[1].expected_output, received[0]);
EXPECT_EQ(input_data[0].expected_output, received[1]);
// Does not wait for other trajectory.
input_data[4].MoveToCollator(&collator);
EXPECT_EQ(3, received.size());
EXPECT_EQ(input_data[2].expected_output, received[2]);
input_data[5].MoveToCollator(&collator);
EXPECT_EQ(4, received.size());
EXPECT_EQ(input_data[3].expected_output, received[3]);
input_data[6].MoveToCollator(&collator);
EXPECT_EQ(5, received.size());
EXPECT_EQ(input_data[5].expected_output, received[4]);
// Sorts different sensors.
input_data[7].MoveToCollator(&collator);
EXPECT_EQ(5, received.size());
input_data[8].MoveToCollator(&collator);
EXPECT_EQ(7, received.size());
EXPECT_EQ(input_data[4].expected_output, received[5]);
EXPECT_EQ(input_data[8].expected_output, received[6]);
EXPECT_FALSE(collator.GetBlockingTrajectoryId().has_value());
collator.FinishTrajectory(kTrajectoryId[0]);
collator.FinishTrajectory(kTrajectoryId[1]);
collator.Flush();
ASSERT_EQ(input_data.size(), received.size());
}
} // namespace
} // namespace sensor
} // namespace cartographer