detect_floors.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
18 
19 #include <algorithm>
20 #include <fstream>
21 #include <vector>
22 
23 #include "Eigen/Core"
25 #include "glog/logging.h"
26 
27 namespace cartographer {
28 namespace mapping {
29 
30 namespace {
31 
32 // A union-find structure for assigning levels to 'spans' in the trajectory
33 // implemented as a disjoint-set forest:
34 // https://en.wikipedia.org/wiki/Disjoint-set_data_structure#Disjoint-set_forests
35 // TODO(hrapp): We use this elsewhere. Pull out into a common place.
36 using Levels = std::map<int, int>;
37 
38 constexpr double kMaxShortSpanLengthMeters = 25.;
39 constexpr double kLevelHeightMeters = 2.5;
40 constexpr double kMinLevelSeparationMeters = 1.;
41 
42 // Indices into 'trajectory.node', so that 'start_index' <= i < 'end_index'.
43 struct Span {
45  int end_index;
46  std::vector<double> z_values;
47 
48  bool operator<(const Span& other) const {
49  return std::forward_as_tuple(start_index, end_index) <
50  std::forward_as_tuple(other.start_index, other.end_index);
51  }
52 };
53 
54 // Union-find implementation for classifying spans into levels.
55 int LevelFind(const int i, const Levels& levels) {
56  auto it = levels.find(i);
57  CHECK(it != levels.end());
58  if (it->first == it->second) {
59  return it->second;
60  }
61  return LevelFind(it->second, levels);
62 }
63 
64 void LevelUnion(int i, int j, Levels* levels) {
65  const int repr_i = LevelFind(i, *levels);
66  const int repr_j = LevelFind(j, *levels);
67  (*levels)[repr_i] = repr_j;
68 }
69 
70 void InsertSorted(const double val, std::vector<double>* vals) {
71  vals->insert(std::upper_bound(vals->begin(), vals->end(), val), val);
72 }
73 
74 double Median(const std::vector<double>& sorted) {
75  CHECK(!sorted.empty());
76  return sorted.at(sorted.size() / 2);
77 }
78 
79 // Cut the trajectory at jumps in z. A new span is started when the current
80 // node's z differes by more than kLevelHeightMeters from the median z values.
81 std::vector<Span> SliceByAltitudeChange(const proto::Trajectory& trajectory) {
82  CHECK_GT(trajectory.node_size(), 0);
83 
84  std::vector<Span> spans;
85  spans.push_back(Span{0, 0, {trajectory.node(0).pose().translation().z()}});
86  for (int i = 1; i < trajectory.node_size(); ++i) {
87  const auto& node = trajectory.node(i);
88  const double z = node.pose().translation().z();
89  if (std::abs(Median(spans.back().z_values) - z) > kLevelHeightMeters) {
90  spans.push_back(Span{i, i, {}});
91  }
92  InsertSorted(z, &spans.back().z_values);
93  spans.back().end_index = i + 1;
94  }
95  return spans;
96 }
97 
98 // Returns the length of 'span' in meters.
99 double SpanLength(const proto::Trajectory& trajectory, const Span& span) {
100  double length = 0;
101  for (int i = span.start_index + 1; i < span.end_index; ++i) {
102  const auto a =
103  transform::ToEigen(trajectory.node(i - 1).pose().translation());
104  const auto b = transform::ToEigen(trajectory.node(i).pose().translation());
105  length += (a - b).head<2>().norm();
106  }
107  return length;
108 }
109 
110 // True if 'span' is considered to be short, i.e. not interesting on its own,
111 // but should be folded into the levels before and after entering it.
112 bool IsShort(const proto::Trajectory& trajectory, const Span& span) {
113  return SpanLength(trajectory, span) < kMaxShortSpanLengthMeters;
114 }
115 
116 // Merges all 'spans' that have similar median z value into the same level.
117 void GroupSegmentsByAltitude(const proto::Trajectory& trajectory,
118  const std::vector<Span>& spans, Levels* levels) {
119  for (size_t i = 0; i < spans.size(); ++i) {
120  for (size_t j = i + 1; j < spans.size(); ++j) {
121  if (std::abs(Median(spans[i].z_values) - Median(spans[j].z_values)) <
122  kMinLevelSeparationMeters) {
123  LevelUnion(i, j, levels);
124  }
125  }
126  }
127 }
128 
129 std::vector<Floor> FindFloors(const proto::Trajectory& trajectory,
130  const std::vector<Span>& spans,
131  const Levels& levels) {
132  std::map<int, std::vector<Span>> level_spans;
133 
134  // Initialize the levels to start out with only long spans.
135  for (size_t i = 0; i < spans.size(); ++i) {
136  const Span& span = spans[i];
137  if (!IsShort(trajectory, span)) {
138  level_spans[LevelFind(i, levels)].push_back(span);
139  }
140  }
141 
142  for (size_t i = 0; i < spans.size(); ++i) {
143  const Span& span = spans[i];
144  if (!IsShort(trajectory, span)) {
145  continue;
146  }
147 
148  // If we have a long piece on this floor already, merge this short piece
149  // into it.
150  int level = LevelFind(i, levels);
151  if (!level_spans[level].empty()) {
152  level_spans[level].push_back(span);
153  continue;
154  }
155 
156  // Otherwise, add this short piece to the level before and after it. It is
157  // likely some intermediate level on stairs.
158  size_t index = i - 1;
159  if (index < spans.size()) {
160  level_spans[LevelFind(index, levels)].push_back(span);
161  }
162  index = i + 1;
163  if (index < spans.size()) {
164  level_spans[LevelFind(index, levels)].push_back(span);
165  }
166  }
167 
168  // Convert the level_spans structure to 'Floor'.
169  std::vector<Floor> floors;
170  for (auto& level : level_spans) {
171  if (level.second.empty()) {
172  continue;
173  }
174 
175  std::vector<double> z_values;
176  std::sort(level.second.begin(), level.second.end());
177  floors.emplace_back();
178  for (const auto& span : level.second) {
179  if (!IsShort(trajectory, span)) {
180  // To figure out the median height of this floor, we only care for the
181  // long pieces that are guaranteed to be in the structure. This is a
182  // heuristic to leave out intermediate (short) levels.
183  z_values.insert(z_values.end(), span.z_values.begin(),
184  span.z_values.end());
185  }
186  floors.back().timespans.push_back(Timespan{
187  common::FromUniversal(trajectory.node(span.start_index).timestamp()),
189  trajectory.node(span.end_index - 1).timestamp())});
190  }
191  std::sort(z_values.begin(), z_values.end());
192  floors.back().z = Median(z_values);
193  }
194  return floors;
195 }
196 
197 } // namespace
198 
199 std::vector<Floor> DetectFloors(const proto::Trajectory& trajectory) {
200  const std::vector<Span> spans = SliceByAltitudeChange(trajectory);
201  Levels levels;
202  for (size_t i = 0; i < spans.size(); ++i) {
203  levels[i] = i;
204  }
205  GroupSegmentsByAltitude(trajectory, spans, &levels);
206 
207  std::vector<Floor> floors = FindFloors(trajectory, spans, levels);
208  std::sort(floors.begin(), floors.end(),
209  [](const Floor& a, const Floor& b) { return a.z < b.z; });
210  return floors;
211 }
212 
213 } // namespace mapping
214 } // namespace cartographer
int start_index
Time FromUniversal(const int64 ticks)
Definition: time.cc:34
std::vector< Floor > DetectFloors(const proto::Trajectory &trajectory)
Eigen::Transform< T, 2, Eigen::Affine > ToEigen(const Rigid2< T > &rigid2)
std::vector< double > z_values
int end_index


cartographer
Author(s): The Cartographer Authors
autogenerated on Mon Feb 28 2022 22:00:58