Adds a heuristic to detect floors in a trajectory. (#125)

cartographer/common/CMakeLists.txt

@@ -75,6 +75,11 @@ google_library(common_histogram
     common_port
 )
 
+google_library(common_interval
+  HDRS
+    interval.h
+)
+
 google_library(common_lua
   USES_LUA
   HDRS

cartographer/common/interval.h

@@ -0,0 +1,32 @@
+/*
+ * Copyright 2016 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_COMMON_INTERVAL_H_
+#define CARTOGRAPHER_COMMON_INTERVAL_H_
+
+namespace cartographer {
+namespace common {
+
+template <typename T>
+struct Interval {
+  T start;
+  T end;
+};
+
+}  // namespace common
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_COMMON_INTERVAL_H_

cartographer/mapping/CMakeLists.txt

@@ -32,6 +32,20 @@ google_library(mapping_collated_trajectory_builder
     sensor_data
 )
 
+google_library(mapping_detect_floors
+  USES_EIGEN
+  USES_GLOG
+  SRCS
+    detect_floors.cc
+  HDRS
+    detect_floors.h
+  DEPENDS
+    common_interval
+    common_time
+    mapping_proto_trajectory
+    transform_transform
+)
+
 google_library(mapping_global_trajectory_builder_interface
   HDRS
     global_trajectory_builder_interface.h

cartographer/mapping/detect_floors.cc

@@ -0,0 +1,213 @@
+/*
+ * Copyright 2016 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/mapping/detect_floors.h"
+
+#include <algorithm>
+#include <fstream>
+#include <vector>
+
+#include "Eigen/Core"
+#include "cartographer/transform/transform.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace mapping {
+
+namespace {
+
+// A union-find structure for assigning levels to 'spans' in the trajectory
+// implemented as a disjoint-set forest:
+// https://en.wikipedia.org/wiki/Disjoint-set_data_structure#Disjoint-set_forests
+// TODO(hrapp): We use this elsewhere. Pull out into a common place.
+using Levels = std::map<int, int>;
+
+constexpr double kMaxShortSpanLengthMeters = 25.;
+constexpr double kLevelHeightMeters = 2.5;
+constexpr double kMinLevelSeparationMeters = 1.0;
+
+// Indices into 'trajectory.node', so that index.start <= i < index.end.
+struct Span {
+  common::Interval<int> index;
+  std::vector<double> z_values;
+
+  bool operator<(const Span& other) const {
+    return std::forward_as_tuple(index.start, index.end) <
+           std::forward_as_tuple(other.index.start, other.index.end);
+  }
+};
+
+// Union-find implementation for classifying spans into levels.
+int LevelFind(const int i, const Levels& levels) {
+  auto it = levels.find(i);
+  CHECK(it != levels.end());
+  if (it->first == it->second) {
+    return it->second;
+  }
+  return LevelFind(it->second, levels);
+}
+
+void LevelUnion(int i, int j, Levels* levels) {
+  const int repr_i = LevelFind(i, *levels);
+  const int repr_j = LevelFind(j, *levels);
+  (*levels)[repr_i] = repr_j;
+}
+
+void InsertSorted(const double val, std::vector<double>* vals) {
+  vals->insert(std::upper_bound(vals->begin(), vals->end(), val), val);
+}
+
+double Median(const std::vector<double>& sorted) {
+  CHECK(!sorted.empty());
+  return sorted.at(sorted.size() / 2);
+}
+
+// Cut the trajectory at jumps in z. A new span is started when the current
+// node's z differes by more than kLevelHeightMeters from the median z values.
+std::vector<Span> SliceByAltitudeChange(const proto::Trajectory& trajectory) {
+  CHECK_GT(trajectory.node_size(), 0);
+
+  std::vector<Span> spans;
+  spans.push_back(Span{{0, 0}, {trajectory.node(0).pose().translation().z()}});
+  for (int i = 1; i < trajectory.node_size(); ++i) {
+    const auto& node = trajectory.node(i);
+    const double z = node.pose().translation().z();
+    if (std::abs(Median(spans.back().z_values) - z) > kLevelHeightMeters) {
+      spans.push_back(Span{{i, i}, {}});
+    }
+    InsertSorted(z, &spans.back().z_values);
+    spans.back().index.end = i + 1;
+  }
+  return spans;
+}
+
+// Returns the length of 'span' in meters.
+double SpanLength(const proto::Trajectory& trajectory, const Span& span) {
+  double length = 0;
+  for (int i = span.index.start + 1; i < span.index.end; ++i) {
+    const auto a =
+        transform::ToEigen(trajectory.node(i - 1).pose().translation());
+    const auto b = transform::ToEigen(trajectory.node(i).pose().translation());
+    length += (a - b).head<2>().norm();
+  }
+  return length;
+}
+
+// True if 'span' is considered to be short, i.e. not interesting on its own,
+// but should be folded into the levels before and after entering it.
+bool IsShort(const proto::Trajectory& trajectory, const Span& span) {
+  return SpanLength(trajectory, span) < kMaxShortSpanLengthMeters;
+}
+
+// Merges all 'spans' that have similar median z value into the same level.
+void GroupSegmentsByAltitude(const proto::Trajectory& trajectory,
+                             const std::vector<Span>& spans, Levels* levels) {
+  for (size_t i = 0; i < spans.size(); ++i) {
+    for (size_t j = i + 1; j < spans.size(); ++j) {
+      if (std::abs(Median(spans[i].z_values) - Median(spans[j].z_values)) <
+          kMinLevelSeparationMeters) {
+        LevelUnion(i, j, levels);
+      }
+    }
+  }
+}
+
+std::vector<Floor> FindFloors(const proto::Trajectory& trajectory,
+                              const std::vector<Span>& spans,
+                              const Levels& levels) {
+  std::map<int, std::vector<Span>> level_spans;
+
+  // Initialize the levels to start out with only long spans.
+  for (size_t i = 0; i < spans.size(); ++i) {
+    const Span& span = spans[i];
+    if (!IsShort(trajectory, span)) {
+      level_spans[LevelFind(i, levels)].push_back(span);
+    }
+  }
+
+  for (size_t i = 0; i < spans.size(); ++i) {
+    const Span& span = spans[i];
+    if (!IsShort(trajectory, span)) {
+      continue;
+    }
+
+    // If we have a long piece on this floor already, merge this short piece
+    // into it.
+    int level = LevelFind(i, levels);
+    if (!level_spans[level].empty()) {
+      level_spans[level].push_back(span);
+      continue;
+    }
+
+    // Otherwise, add this short piece to the level before and after it. It is
+    // likely some intermediate level on stairs.
+    size_t index = i - 1;
+    if (index < spans.size()) {
+      level_spans[LevelFind(index, levels)].push_back(span);
+    }
+    index = i + 1;
+    if (index < spans.size()) {
+      level_spans[LevelFind(index, levels)].push_back(span);
+    }
+  }
+
+  // Convert the level_spans structure to 'Floor'.
+  std::vector<Floor> floors;
+  for (auto& level : level_spans) {
+    if (level.second.empty()) {
+      continue;
+    }
+
+    std::vector<double> z_values;
+    std::sort(level.second.begin(), level.second.end());
+    floors.emplace_back();
+    for (const auto& span : level.second) {
+      if (!IsShort(trajectory, span)) {
+        // To figure out the median height of this floor, we only care for the
+        // long pieces that are guaranteed to be in the structure. This is a
+        // heuristic to leave out intermediate (short) levels.
+        z_values.insert(z_values.end(), span.z_values.begin(),
+                        span.z_values.end());
+      }
+      floors.back().timespans.push_back(common::Interval<common::Time>{
+          common::FromUniversal(trajectory.node(span.index.start).timestamp()),
+          common::FromUniversal(
+              trajectory.node(span.index.end - 1).timestamp())});
+    }
+    std::sort(z_values.begin(), z_values.end());
+    floors.back().z = Median(z_values);
+  }
+  return floors;
+}
+
+}  // namespace
+
+std::vector<Floor> DetectFloors(const proto::Trajectory& trajectory) {
+  const std::vector<Span> spans = SliceByAltitudeChange(trajectory);
+  Levels levels;
+  for (size_t i = 0; i < spans.size(); ++i) {
+    levels[i] = i;
+  }
+  GroupSegmentsByAltitude(trajectory, spans, &levels);
+
+  std::vector<Floor> floors = FindFloors(trajectory, spans, levels);
+  std::sort(floors.begin(), floors.end(),
+            [](const Floor& a, const Floor& b) { return a.z < b.z; });
+  return floors;
+}
+
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/detect_floors.h

@@ -0,0 +1,47 @@
+/*
+ * Copyright 2016 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_MAPPING_DETECT_FLOORS_H_
+#define CARTOGRAPHER_MAPPING_DETECT_FLOORS_H_
+
+#include "cartographer/mapping/proto/trajectory.pb.h"
+
+#include "cartographer/common/interval.h"
+#include "cartographer/common/time.h"
+
+namespace cartographer {
+namespace mapping {
+
+struct Floor {
+  // The spans of time we spent on this floor. Since we might have walked up and
+  // down many times in this place, there can be many spans of time we spent on
+  // a particular floor.
+  std::vector<common::Interval<common::Time>> timespans;
+
+  // The median z-value of this floor.
+  double z;
+};
+
+// Uses a heuristic which looks at z-values of the poses to detect individual
+// floors of a building. This requires that floors are *mostly* on the same
+// z-height and that level changes happen *relatively* abrubtly, e.g. by taking
+// the stairs.
+std::vector<Floor> DetectFloors(const proto::Trajectory& trajectory);
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_DETECT_FLOORS_H_