From 67776542024861443be0420524da1d588050996d Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Wed, 16 Nov 2016 05:16:40 -0800 Subject: [PATCH] Adds a heuristic to detect floors in a trajectory. (#125) --- cartographer/common/CMakeLists.txt | 5 + cartographer/common/interval.h | 32 ++++ cartographer/mapping/CMakeLists.txt | 14 ++ cartographer/mapping/detect_floors.cc | 213 ++++++++++++++++++++++++++ cartographer/mapping/detect_floors.h | 47 ++++++ 5 files changed, 311 insertions(+) create mode 100644 cartographer/common/interval.h create mode 100644 cartographer/mapping/detect_floors.cc create mode 100644 cartographer/mapping/detect_floors.h diff --git a/cartographer/common/CMakeLists.txt b/cartographer/common/CMakeLists.txt index 7d43d9d..d2f215c 100644 --- a/cartographer/common/CMakeLists.txt +++ b/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 diff --git a/cartographer/common/interval.h b/cartographer/common/interval.h new file mode 100644 index 0000000..4c5a81c --- /dev/null +++ b/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 +struct Interval { + T start; + T end; +}; + +} // namespace common +} // namespace cartographer + +#endif // CARTOGRAPHER_COMMON_INTERVAL_H_ diff --git a/cartographer/mapping/CMakeLists.txt b/cartographer/mapping/CMakeLists.txt index 9d7fa95..ec4138f 100644 --- a/cartographer/mapping/CMakeLists.txt +++ b/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 diff --git a/cartographer/mapping/detect_floors.cc b/cartographer/mapping/detect_floors.cc new file mode 100644 index 0000000..3551eb9 --- /dev/null +++ b/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 +#include +#include + +#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; + +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 index; + std::vector 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* vals) { + vals->insert(std::upper_bound(vals->begin(), vals->end(), val), val); +} + +double Median(const std::vector& 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 SliceByAltitudeChange(const proto::Trajectory& trajectory) { + CHECK_GT(trajectory.node_size(), 0); + + std::vector 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& 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 FindFloors(const proto::Trajectory& trajectory, + const std::vector& spans, + const Levels& levels) { + std::map> 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 floors; + for (auto& level : level_spans) { + if (level.second.empty()) { + continue; + } + + std::vector 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::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 DetectFloors(const proto::Trajectory& trajectory) { + const std::vector spans = SliceByAltitudeChange(trajectory); + Levels levels; + for (size_t i = 0; i < spans.size(); ++i) { + levels[i] = i; + } + GroupSegmentsByAltitude(trajectory, spans, &levels); + + std::vector 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 diff --git a/cartographer/mapping/detect_floors.h b/cartographer/mapping/detect_floors.h new file mode 100644 index 0000000..0f98843 --- /dev/null +++ b/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> 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 DetectFloors(const proto::Trajectory& trajectory); + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_DETECT_FLOORS_H_