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

master
Holger Rapp 2016-11-16 05:16:40 -08:00 committed by GitHub
parent a74319cf1a
commit 6777654202
5 changed files with 311 additions and 0 deletions

View File

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

View File

@ -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_

View File

@ -32,6 +32,20 @@ google_library(mapping_collated_trajectory_builder
sensor_data 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 google_library(mapping_global_trajectory_builder_interface
HDRS HDRS
global_trajectory_builder_interface.h global_trajectory_builder_interface.h

View File

@ -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

View File

@ -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_