From 77fb50fd765e3145b6544bcf01d2693e34ba5bc9 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 6 Nov 2017 13:36:59 +0100 Subject: [PATCH] Introduce sensor::MapByTime<>. (#631) This data structure is used for keeping IMU data. This allows trimming IMU data in the middle which is needed for life-long mapping. --- .../sparse_pose_graph/optimization_problem.cc | 17 +- .../sparse_pose_graph/optimization_problem.h | 3 +- .../sparse_pose_graph/optimization_problem.cc | 22 +-- .../sparse_pose_graph/optimization_problem.h | 3 +- cartographer/sensor/map_by_time.h | 156 ++++++++++++++++++ cartographer/sensor/map_by_time_test.cc | 93 +++++++++++ 6 files changed, 260 insertions(+), 34 deletions(-) create mode 100644 cartographer/sensor/map_by_time.h create mode 100644 cartographer/sensor/map_by_time_test.cc diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index de07d57..3844c7c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -64,10 +64,7 @@ OptimizationProblem::~OptimizationProblem() {} void OptimizationProblem::AddImuData(const int trajectory_id, const sensor::ImuData& imu_data) { - CHECK_GE(trajectory_id, 0); - imu_data_.resize( - std::max(imu_data_.size(), static_cast(trajectory_id) + 1)); - imu_data_[trajectory_id].push_back(imu_data); + imu_data_.Append(trajectory_id, imu_data); } void OptimizationProblem::AddOdometerData( @@ -95,18 +92,8 @@ void OptimizationProblem::InsertTrajectoryNode( } void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { + imu_data_.Trim(node_data_, node_id); node_data_.Trim(node_id); - - const int trajectory_id = node_id.trajectory_id; - if (node_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 && - trajectory_id < static_cast(imu_data_.size())) { - const common::Time node_time = - node_data_.BeginOfTrajectory(trajectory_id)->data.time; - auto& imu_data = imu_data_.at(trajectory_id); - while (imu_data.size() > 1 && imu_data[1].time <= node_time) { - imu_data.pop_front(); - } - } } void OptimizationProblem::AddSubmap(const int trajectory_id, diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index 2a286f4..9a3e3e2 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -31,6 +31,7 @@ #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/map_by_time.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/transform/transform_interpolation_buffer.h" @@ -90,7 +91,7 @@ class OptimizationProblem { private: mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; - std::vector> imu_data_; + sensor::MapByTime imu_data_; mapping::MapById node_data_; std::vector odometry_data_; mapping::MapById submap_data_; diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index ee39c3e..082b09f 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -56,10 +56,7 @@ OptimizationProblem::~OptimizationProblem() {} void OptimizationProblem::AddImuData(const int trajectory_id, const sensor::ImuData& imu_data) { - CHECK_GE(trajectory_id, 0); - imu_data_.resize( - std::max(imu_data_.size(), static_cast(trajectory_id) + 1)); - imu_data_[trajectory_id].push_back(imu_data); + imu_data_.Append(trajectory_id, imu_data); } void OptimizationProblem::AddOdometerData( @@ -101,18 +98,8 @@ void OptimizationProblem::InsertTrajectoryNode( } void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { + imu_data_.Trim(node_data_, node_id); node_data_.Trim(node_id); - - const int trajectory_id = node_id.trajectory_id; - if (node_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 && - trajectory_id < static_cast(imu_data_.size())) { - const common::Time node_time = - node_data_.BeginOfTrajectory(trajectory_id)->data.time; - auto& imu_data = imu_data_.at(trajectory_id); - while (imu_data.size() > 1 && imu_data[1].time <= node_time) { - imu_data.pop_front(); - } - } } void OptimizationProblem::AddSubmap(const int trajectory_id, @@ -233,8 +220,9 @@ void OptimizationProblem::Solve(const std::vector& constraints, problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4, new ceres::QuaternionParameterization()); - const std::deque& imu_data = imu_data_.at(trajectory_id); - CHECK(!imu_data.empty()); + CHECK(imu_data_.HasTrajectory(trajectory_id)); + const auto imu_data = imu_data_.trajectory(trajectory_id); + CHECK(imu_data.begin() != imu_data.end()); auto imu_it = imu_data.begin(); auto prev_node_it = node_it; diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index 9e444b7..c3a9d77 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -32,6 +32,7 @@ #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/map_by_time.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/transform/transform_interpolation_buffer.h" @@ -100,7 +101,7 @@ class OptimizationProblem { mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; FixZ fix_z_; - std::vector> imu_data_; + sensor::MapByTime imu_data_; mapping::MapById node_data_; std::vector odometry_data_; mapping::MapById submap_data_; diff --git a/cartographer/sensor/map_by_time.h b/cartographer/sensor/map_by_time.h new file mode 100644 index 0000000..6c0b297 --- /dev/null +++ b/cartographer/sensor/map_by_time.h @@ -0,0 +1,156 @@ +/* + * Copyright 2017 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_MAP_BY_TIME_H_ +#define CARTOGRAPHER_SENSOR_MAP_BY_TIME_H_ + +#include +#include +#include +#include +#include + +#include "cartographer/common/port.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/id.h" +#include "glog/logging.h" + +namespace cartographer { +namespace sensor { + +// 'DataType' must contain a 'time' member of type common::Time. +template +class MapByTime { + public: + // Appends data to a 'trajectory_id', creating trajectories as needed. + void Append(const int trajectory_id, const DataType& data) { + CHECK_GE(trajectory_id, 0); + auto& trajectory = data_[trajectory_id]; + if (!trajectory.empty()) { + CHECK_GT(data.time, std::prev(trajectory.end())->first); + } + trajectory.emplace(data.time, data); + } + + // Removes data no longer needed once 'node_id' gets removed from 'nodes'. + // 'NodeType' must contain a 'time' member of type common::Time. + template + void Trim(const mapping::MapById& nodes, + const mapping::NodeId& node_id) { + const int trajectory_id = node_id.trajectory_id; + CHECK_GE(trajectory_id, 0); + + // Data only important between 'gap_start' and 'gap_end' is no longer + // needed. We retain the first and last data of the gap so that + // interpolation with the adjacent data outside the gap is still possible. + const auto node_it = nodes.find(node_id); + CHECK(node_it != nodes.end()); + const common::Time gap_start = + node_it != nodes.BeginOfTrajectory(trajectory_id) + ? std::prev(node_it)->data.time + : common::Time::min(); + const auto next_it = std::next(node_it); + const common::Time gap_end = next_it != nodes.EndOfTrajectory(trajectory_id) + ? next_it->data.time + : common::Time::max(); + CHECK_LT(gap_start, gap_end); + + auto& trajectory = data_[trajectory_id]; + auto data_it = trajectory.lower_bound(gap_start); + auto data_end = trajectory.upper_bound(gap_end); + if (data_it == data_end) { + return; + } + if (gap_end != common::Time::max()) { + // Retain the last data inside the gap. + data_end = std::prev(data_end); + if (data_it == data_end) { + return; + } + } + if (gap_start != common::Time::min()) { + // Retain the first data inside the gap. + data_it = std::next(data_it); + } + while (data_it != data_end) { + data_it = trajectory.erase(data_it); + } + if (trajectory.empty()) { + data_.erase(trajectory_id); + } + } + + bool HasTrajectory(const int trajectory_id) const { + return data_.count(trajectory_id) != 0; + } + + class ConstIterator { + public: + using iterator_category = std::bidirectional_iterator_tag; + using value_type = DataType; + using difference_type = int64; + using pointer = const DataType*; + using reference = const DataType&; + + explicit ConstIterator( + typename std::map::const_iterator iterator) + : iterator_(iterator) {} + + const DataType& operator*() const { return iterator_->second; } + + const DataType* operator->() const { return &iterator_->second; } + + ConstIterator& operator++() { + ++iterator_; + return *this; + } + + ConstIterator& operator--() { + --iterator_; + return *this; + } + + bool operator==(const ConstIterator& it) const { + return iterator_ == it.iterator_; + } + + bool operator!=(const ConstIterator& it) const { return !operator==(it); } + + private: + typename std::map::const_iterator iterator_; + }; + + ConstIterator BeginOfTrajectory(const int trajectory_id) const { + return ConstIterator(data_.at(trajectory_id).begin()); + } + + ConstIterator EndOfTrajectory(const int trajectory_id) const { + return ConstIterator(data_.at(trajectory_id).end()); + } + + mapping::Range trajectory(const int trajectory_id) const { + return mapping::Range(BeginOfTrajectory(trajectory_id), + EndOfTrajectory(trajectory_id)); + } + + private: + std::map> data_; +}; + +} // namespace sensor +} // namespace cartographer + +#endif // CARTOGRAPHER_SENSOR_MAP_BY_TIME_H_ diff --git a/cartographer/sensor/map_by_time_test.cc b/cartographer/sensor/map_by_time_test.cc new file mode 100644 index 0000000..10d1060 --- /dev/null +++ b/cartographer/sensor/map_by_time_test.cc @@ -0,0 +1,93 @@ +/* + * Copyright 2017 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/map_by_time.h" + +#include + +#include "cartographer/common/time.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace sensor { +namespace { + +common::Time CreateTime(const int milliseconds) { + return common::Time(common::FromMilliseconds(milliseconds)); +} + +struct Data { + common::Time time; +}; + +struct NodeData { + common::Time time; +}; + +TEST(MapByTimeTest, AppendAndViewTrajectory) { + MapByTime map_by_time; + map_by_time.Append(0, Data{CreateTime(10)}); + map_by_time.Append(42, Data{CreateTime(42)}); + map_by_time.Append(42, Data{CreateTime(43)}); + std::deque expected_data = {Data{CreateTime(42)}, Data{CreateTime(43)}}; + for (const Data& data : map_by_time.trajectory(42)) { + ASSERT_FALSE(expected_data.empty()); + EXPECT_EQ(expected_data.front().time, data.time); + expected_data.pop_front(); + } + EXPECT_TRUE(expected_data.empty()); +} + +TEST(MapByTimeTest, Trimming) { + MapByTime map_by_time; + EXPECT_FALSE(map_by_time.HasTrajectory(42)); + map_by_time.Append(42, Data{CreateTime(1)}); + map_by_time.Append(42, Data{CreateTime(41)}); + map_by_time.Append(42, Data{CreateTime(42)}); + map_by_time.Append(42, Data{CreateTime(43)}); + map_by_time.Append(42, Data{CreateTime(47)}); + map_by_time.Append(42, Data{CreateTime(48)}); + map_by_time.Append(42, Data{CreateTime(49)}); + map_by_time.Append(42, Data{CreateTime(5000)}); + EXPECT_TRUE(map_by_time.HasTrajectory(42)); + // Trim one node. + mapping::MapById map_by_id; + map_by_id.Append(42, NodeData{CreateTime(42)}); + map_by_id.Append(42, NodeData{CreateTime(46)}); + map_by_id.Append(42, NodeData{CreateTime(48)}); + map_by_time.Trim(map_by_id, mapping::NodeId{42, 1}); + map_by_id.Trim(mapping::NodeId{42, 1}); + ASSERT_TRUE(map_by_time.HasTrajectory(42)); + std::deque expected_data = { + Data{CreateTime(1)}, Data{CreateTime(41)}, Data{CreateTime(42)}, + Data{CreateTime(48)}, Data{CreateTime(49)}, Data{CreateTime(5000)}}; + for (const Data& data : map_by_time.trajectory(42)) { + ASSERT_FALSE(expected_data.empty()); + EXPECT_EQ(expected_data.front().time, data.time); + expected_data.pop_front(); + } + EXPECT_TRUE(expected_data.empty()); + // Trim everything. + map_by_time.Trim(map_by_id, mapping::NodeId{42, 2}); + map_by_id.Trim(mapping::NodeId{42, 2}); + map_by_time.Trim(map_by_id, mapping::NodeId{42, 0}); + map_by_id.Trim(mapping::NodeId{42, 0}); + EXPECT_FALSE(map_by_time.HasTrajectory(42)); +} + +} // namespace +} // namespace sensor +} // namespace cartographer