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

Moves SensorCollator into sensor package. (#58)

master
Damon Kohler 2016-10-17 10:18:17 +02:00 committed by GitHub
parent c2aed53ce8
commit 2eca021003
4 changed files with 58 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
cartographer/mapping/CMakeLists.txt
View File

@ -91,18 +91,6 @@ google_library(mapping_probability_values
common_port
)
google_library(mapping_sensor_collator
USES_EIGEN
USES_GLOG
HDRS
sensor_collator.h
DEPENDS
common_make_unique
common_ordered_multi_queue
common_time
sensor_sensor_packet_period_histogram_builder
)
google_library(mapping_sparse_pose_graph
USES_GLOG
SRCS
@ -166,17 +154,6 @@ google_test(mapping_probability_values_test
mapping_probability_values
)
google_test(mapping_sensor_collator_test
SRCS
sensor_collator_test.cc
DEPENDS
common_lua_parameter_dictionary_test_helpers
common_make_unique
common_time
mapping_sensor_collator
sensor_proto_sensor
)
google_test(mapping_sparse_pose_graph_test
USES_GLOG
SRCS

23
cartographer/sensor/CMakeLists.txt
View File

@ -14,6 +14,18 @@
add_subdirectory("proto")
google_library(sensor_collator
USES_EIGEN
USES_GLOG
HDRS
collator.h
DEPENDS
common_make_unique
common_ordered_multi_queue
common_time
sensor_sensor_packet_period_histogram_builder
)
google_library(sensor_compressed_point_cloud
USES_EIGEN
SRCS
@ -102,6 +114,17 @@ google_library(sensor_voxel_filter
sensor_proto_adaptive_voxel_filter_options
)
google_test(sensor_collator_test
SRCS
collator_test.cc
DEPENDS
common_lua_parameter_dictionary_test_helpers
common_make_unique
common_time
sensor_collator
sensor_proto_sensor
)
google_test(sensor_compressed_point_cloud_test
SRCS
compressed_point_cloud_test.cc

32
cartographer/mapping/sensor_collator.h → cartographer/sensor/collator.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_SENSOR_COLLATOR_H_
#define CARTOGRAPHER_MAPPING_SENSOR_COLLATOR_H_
#ifndef CARTOGRAPHER_SENSOR_COLLATOR_H_
#define CARTOGRAPHER_SENSOR_COLLATOR_H_
#include <functional>
#include <memory>
@ -32,32 +32,32 @@
#include "glog/logging.h"
namespace cartographer {
namespace mapping {
namespace sensor {
struct SensorCollatorQueueKey {
struct CollatorQueueKey {
int trajectory_id;
string sensor_id;
bool operator<(const SensorCollatorQueueKey& other) const {
bool operator<(const CollatorQueueKey& other) const {
return std::forward_as_tuple(trajectory_id, sensor_id) <
std::forward_as_tuple(other.trajectory_id, other.sensor_id);
}
};
inline std::ostream& operator<<(std::ostream& out,
const SensorCollatorQueueKey& key) {
const CollatorQueueKey& key) {
return out << '(' << key.trajectory_id << ", " << key.sensor_id << ')';
}
template <typename SensorDataType>
class SensorCollator {
class Collator {
public:
using Callback = std::function<void(int64, std::unique_ptr<SensorDataType>)>;
SensorCollator() {}
Collator() {}
SensorCollator(const SensorCollator&) = delete;
SensorCollator& operator=(const SensorCollator&) = delete;
Collator(const Collator&) = delete;
Collator& operator=(const Collator&) = delete;
// Adds a trajectory to produce sorted sensor output for. Calls 'callback'
// for each collated sensor data.
@ -65,7 +65,7 @@ class SensorCollator {
const std::unordered_set<string>& expected_sensor_ids,
const Callback callback) {
for (const auto& sensor_id : expected_sensor_ids) {
const auto queue_key = SensorCollatorQueueKey{trajectory_id, sensor_id};
const auto queue_key = CollatorQueueKey{trajectory_id, sensor_id};
queue_.AddQueue(queue_key, [callback](std::unique_ptr<Value> value) {
callback(value->timestamp, std::move(value->sensor_data));
});
@ -89,7 +89,7 @@ class SensorCollator {
sensor_packet_period_histogram_builder_.Add(trajectory_id, timestamp,
sensor_id);
queue_.Add(
SensorCollatorQueueKey{trajectory_id, sensor_id}, timestamp,
CollatorQueueKey{trajectory_id, sensor_id}, timestamp,
common::make_unique<Value>(Value{timestamp, std::move(sensor_data)}));
}
@ -117,15 +117,15 @@ class SensorCollator {
};
// Queue keys are a pair of trajectory ID and sensor identifier.
common::OrderedMultiQueue<SensorCollatorQueueKey, int64, Value> queue_;
common::OrderedMultiQueue<CollatorQueueKey, int64, Value> queue_;
// Map of trajectory ID to all associated QueueKeys.
std::unordered_map<int, std::vector<SensorCollatorQueueKey>> queue_keys_;
std::unordered_map<int, std::vector<CollatorQueueKey>> queue_keys_;
sensor::SensorPacketPeriodHistogramBuilder
sensor_packet_period_histogram_builder_;
};
} // namespace mapping
} // namespace sensor
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_SENSOR_COLLATOR_H_
#endif // CARTOGRAPHER_SENSOR_COLLATOR_H_

38
cartographer/mapping/sensor_collator_test.cc → cartographer/sensor/collator_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping/sensor_collator.h"
#include "cartographer/sensor/collator.h"
#include <memory>
@ -25,14 +25,14 @@
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping {
namespace sensor {
namespace {
struct TestData {
string frame_id;
};
TEST(SensorCollator, Ordering) {
TEST(Collator, Ordering) {
TestData first{"horizontal_laser"};
TestData second{"vertical_laser"};
TestData third{"imu"};
@ -43,25 +43,25 @@ TEST(SensorCollator, Ordering) {
const std::unordered_set<string> frame_ids = {
"horizontal_laser", "vertical_laser", "imu", "something"};
std::vector<std::pair<int64, TestData>> received;
SensorCollator<TestData> sensor_collator;
sensor_collator.AddTrajectory(
Collator<TestData> collator;
collator.AddTrajectory(
0, frame_ids,
[&received](const int64 timestamp, std::unique_ptr<TestData> packet) {
received.push_back(std::make_pair(timestamp, *packet));
});
sensor_collator.AddSensorData(0, 100, first.frame_id,
common::make_unique<TestData>(first));
sensor_collator.AddSensorData(0, 600, sixth.frame_id,
common::make_unique<TestData>(sixth));
sensor_collator.AddSensorData(0, 400, fourth.frame_id,
common::make_unique<TestData>(fourth));
sensor_collator.AddSensorData(0, 200, second.frame_id,
common::make_unique<TestData>(second));
sensor_collator.AddSensorData(0, 500, fifth.frame_id,
common::make_unique<TestData>(fifth));
sensor_collator.AddSensorData(0, 300, third.frame_id,
common::make_unique<TestData>(third));
collator.AddSensorData(0, 100, first.frame_id,
common::make_unique<TestData>(first));
collator.AddSensorData(0, 600, sixth.frame_id,
common::make_unique<TestData>(sixth));
collator.AddSensorData(0, 400, fourth.frame_id,
common::make_unique<TestData>(fourth));
collator.AddSensorData(0, 200, second.frame_id,
common::make_unique<TestData>(second));
collator.AddSensorData(0, 500, fifth.frame_id,
common::make_unique<TestData>(fifth));
collator.AddSensorData(0, 300, third.frame_id,
common::make_unique<TestData>(third));
EXPECT_EQ(3, received.size());
EXPECT_EQ(100, received[0].first);
@ -71,7 +71,7 @@ TEST(SensorCollator, Ordering) {
EXPECT_EQ(300, received[2].first);
EXPECT_EQ("imu", received[2].second.frame_id);
sensor_collator.Flush();
collator.Flush();
ASSERT_EQ(6, received.size());
EXPECT_EQ("horizontal_laser", received[3].second.frame_id);
@ -82,5 +82,5 @@ TEST(SensorCollator, Ordering) {
}
} // namespace
} // namespace mapping
} // namespace sensor
} // namespace cartographer