map_by_time.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #ifndef CARTOGRAPHER_SENSOR_MAP_BY_TIME_H_
00018 #define CARTOGRAPHER_SENSOR_MAP_BY_TIME_H_
00019 
00020 #include <algorithm>
00021 #include <iterator>
00022 #include <map>
00023 #include <memory>
00024 #include <vector>
00025 
00026 #include "cartographer/common/port.h"
00027 #include "cartographer/common/time.h"
00028 #include "cartographer/mapping/id.h"
00029 #include "glog/logging.h"
00030 
00031 namespace cartographer {
00032 namespace sensor {
00033 
00034 // 'DataType' must contain a 'time' member of type common::Time.
00035 template <typename DataType>
00036 class MapByTime {
00037  public:
00038   // Appends data to a 'trajectory_id', creating trajectories as needed.
00039   void Append(const int trajectory_id, const DataType& data) {
00040     CHECK_GE(trajectory_id, 0);
00041     auto& trajectory = data_[trajectory_id];
00042     if (!trajectory.empty()) {
00043       CHECK_GT(data.time, std::prev(trajectory.end())->first);
00044     }
00045     trajectory.emplace(data.time, data);
00046   }
00047 
00048   // Removes data no longer needed once 'node_id' gets removed from 'nodes'.
00049   // 'NodeType' must contain a 'time' member of type common::Time.
00050   template <typename NodeType>
00051   void Trim(const mapping::MapById<mapping::NodeId, NodeType>& nodes,
00052             const mapping::NodeId& node_id) {
00053     const int trajectory_id = node_id.trajectory_id;
00054     CHECK_GE(trajectory_id, 0);
00055     if (data_.count(trajectory_id) == 0) {
00056       return;
00057     }
00058 
00059     // Data only important between 'gap_start' and 'gap_end' is no longer
00060     // needed. We retain the first and last data of the gap so that
00061     // interpolation with the adjacent data outside the gap is still possible.
00062     const auto node_it = nodes.find(node_id);
00063     CHECK(node_it != nodes.end());
00064     const common::Time gap_start =
00065         node_it != nodes.BeginOfTrajectory(trajectory_id)
00066             ? std::prev(node_it)->data.time
00067             : common::Time::min();
00068     const auto next_it = std::next(node_it);
00069     const common::Time gap_end = next_it != nodes.EndOfTrajectory(trajectory_id)
00070                                      ? next_it->data.time
00071                                      : common::Time::max();
00072     CHECK_LT(gap_start, gap_end);
00073 
00074     auto& trajectory = data_.at(trajectory_id);
00075     auto data_it = trajectory.lower_bound(gap_start);
00076     auto data_end = trajectory.upper_bound(gap_end);
00077     if (data_it == data_end) {
00078       return;
00079     }
00080     if (gap_end != common::Time::max()) {
00081       // Retain the last data inside the gap.
00082       data_end = std::prev(data_end);
00083       if (data_it == data_end) {
00084         return;
00085       }
00086     }
00087     if (gap_start != common::Time::min()) {
00088       // Retain the first data inside the gap.
00089       data_it = std::next(data_it);
00090     }
00091     while (data_it != data_end) {
00092       data_it = trajectory.erase(data_it);
00093     }
00094     if (trajectory.empty()) {
00095       data_.erase(trajectory_id);
00096     }
00097   }
00098 
00099   bool HasTrajectory(const int trajectory_id) const {
00100     return data_.count(trajectory_id) != 0;
00101   }
00102 
00103   class ConstIterator {
00104    public:
00105     using iterator_category = std::bidirectional_iterator_tag;
00106     using value_type = DataType;
00107     using difference_type = int64;
00108     using pointer = const DataType*;
00109     using reference = const DataType&;
00110 
00111     explicit ConstIterator(
00112         typename std::map<common::Time, DataType>::const_iterator iterator)
00113         : iterator_(iterator) {}
00114 
00115     const DataType& operator*() const { return iterator_->second; }
00116 
00117     const DataType* operator->() const { return &iterator_->second; }
00118 
00119     ConstIterator& operator++() {
00120       ++iterator_;
00121       return *this;
00122     }
00123 
00124     ConstIterator& operator--() {
00125       --iterator_;
00126       return *this;
00127     }
00128 
00129     bool operator==(const ConstIterator& it) const {
00130       return iterator_ == it.iterator_;
00131     }
00132 
00133     bool operator!=(const ConstIterator& it) const { return !operator==(it); }
00134 
00135    private:
00136     typename std::map<common::Time, DataType>::const_iterator iterator_;
00137   };
00138 
00139   class ConstTrajectoryIterator {
00140    public:
00141     using iterator_category = std::bidirectional_iterator_tag;
00142     using value_type = int;
00143     using difference_type = int64;
00144     using pointer = const int*;
00145     using reference = const int&;
00146 
00147     explicit ConstTrajectoryIterator(
00148         typename std::map<int, std::map<common::Time, DataType>>::const_iterator
00149             current_trajectory)
00150         : current_trajectory_(current_trajectory) {}
00151 
00152     int operator*() const { return current_trajectory_->first; }
00153 
00154     ConstTrajectoryIterator& operator++() {
00155       ++current_trajectory_;
00156       return *this;
00157     }
00158 
00159     ConstTrajectoryIterator& operator--() {
00160       --current_trajectory_;
00161       return *this;
00162     }
00163 
00164     bool operator==(const ConstTrajectoryIterator& it) const {
00165       return current_trajectory_ == it.current_trajectory_;
00166     }
00167 
00168     bool operator!=(const ConstTrajectoryIterator& it) const {
00169       return !operator==(it);
00170     }
00171 
00172    private:
00173     typename std::map<int, std::map<common::Time, DataType>>::const_iterator
00174         current_trajectory_;
00175   };
00176 
00177   ConstIterator BeginOfTrajectory(const int trajectory_id) const {
00178     return ConstIterator(data_.at(trajectory_id).begin());
00179   }
00180 
00181   ConstIterator EndOfTrajectory(const int trajectory_id) const {
00182     return ConstIterator(data_.at(trajectory_id).end());
00183   }
00184 
00185   // Returns Range object for range-based loops over the trajectory IDs.
00186   mapping::Range<ConstTrajectoryIterator> trajectory_ids() const {
00187     return mapping::Range<ConstTrajectoryIterator>(
00188         ConstTrajectoryIterator(data_.begin()),
00189         ConstTrajectoryIterator(data_.end()));
00190   }
00191 
00192   mapping::Range<ConstIterator> trajectory(const int trajectory_id) const {
00193     return mapping::Range<ConstIterator>(BeginOfTrajectory(trajectory_id),
00194                                          EndOfTrajectory(trajectory_id));
00195   }
00196 
00197   // Returns an iterator to the first element in the container belonging to
00198   // trajectory 'trajectory_id' whose time is not considered to go before
00199   // 'time', or EndOfTrajectory(trajectory_id) if all keys are considered to go
00200   // before 'time'. 'trajectory_id' must refer to an existing trajectory.
00201   ConstIterator lower_bound(const int trajectory_id,
00202                             const common::Time time) const {
00203     return ConstIterator(data_.at(trajectory_id).lower_bound(time));
00204   }
00205 
00206  private:
00207   std::map<int, std::map<common::Time, DataType>> data_;
00208 };
00209 
00210 }  // namespace sensor
00211 }  // namespace cartographer
00212 
00213 #endif  // CARTOGRAPHER_SENSOR_MAP_BY_TIME_H_


cartographer
Author(s): The Cartographer Authors
autogenerated on Thu May 9 2019 02:27:35