231 lines
10 KiB
TeX
231 lines
10 KiB
TeX
\section{Transition Model}
|
|
|
|
To sample only transitions $p(\mStateVec_{t} \mid \mStateVec_{t-1})$ that are actually feasible
|
|
within the environment, we utilize a \SI{20}{\centimeter}-gridded graph
|
|
$G = (V,E)$, $v_{x,y,z} \in V$, $e_{v_{x,y,z}}^{v_{x',y',z'}} \in E$
|
|
derived from the buildings floorplan as described in \cite{ipin2015}.
|
|
However, we add improved $z$-transitions by also modelling realistic
|
|
stairwells using nodes and edges as can be seen in fig. \ref{fig:gridStairs}.
|
|
|
|
\begin{figure}
|
|
\includegraphics[trim=45 60 45 30]{grid/grid}
|
|
\caption{Besides the nodes and edges defined by the distinct floors, we add realistic stairs to interconnect them.}
|
|
\label{fig:gridStairs}
|
|
\end{figure}
|
|
|
|
\newcommand{\spoint}{l}
|
|
Stairs are defined using three points $\vec{\spoint}_1, \vec{\spoint}_2, \vec{\spoint}_3 \in \R^3$ whereby the segment
|
|
$[ \vec{\spoint}_1 \vec{\spoint}_2 ]$ describes the starting-edge, and $[ \vec{\spoint}_2 \vec{\spoint}_3 ]$ the stair's direction.
|
|
The corresponding vertices are determined using intersections of the segments with the bounding-box
|
|
for each vertex.
|
|
|
|
\commentByFrank{mention?: clean z-transitions, remove x/y nodes by adding bounding boxes}
|
|
|
|
To reduce the system's memory footprint, we search for the largest connected region within the graph and
|
|
remove all nodes and edges that are not connected to this region.
|
|
|
|
\newcommand{\gHead}{\theta}
|
|
\newcommand{\gDist}{d}
|
|
Walking the grid is now possible by moving along adjacent nodes into a given walking-direction
|
|
until a desired distance is reached \cite{ipin2015}.
|
|
In order to use meaningful headings $\gHead$ and distances $\gDist$
|
|
(matching the pedestrian's real heading and walking speed) for each transition,
|
|
we use the current sensor-readings $\mObsVec_{t}$ for hinted instead of truly random adjustments.
|
|
|
|
During a walk, each edge has an assigned probability $p(e)$ which depends on a chosen implementation.
|
|
Usually, this probability describes aspects like a comparison of the edge's angle $\angle e$ with the
|
|
current heading $\gHead$. However, it is also possible to incorporate additional prior knowledge to favor
|
|
some vertices/edges
|
|
|
|
|
|
\commentByFrank{im system-teil anmerken: $\mObsVec_t^{\mObsSteps} \in \N$}
|
|
|
|
\begin{align}
|
|
\mStateVec_{t}^{\mStateHeading} = \gHead &= \mStateVec_{t-1}^{\mStateHeading} + \mObsVec_t^{\mObsHeading} + \mathcal{N}(0, \sigma_{\gHead}^2) \\
|
|
\gDist &= \mObsVec_t^{\mObsSteps} \cdot \SI{0.7}{\meter} + \mathcal{N}(0, \sigma_{\gDist}^2)
|
|
\end{align}
|
|
|
|
|
|
For comparison purpose we define a simple weighting method that assigns a probability to each edge
|
|
based on the deviation from the currently estimated heading $\gHead$:
|
|
|
|
\commentByFrank{das erste $=$ ist komisch. bessere option?}
|
|
\begin{equation}
|
|
p(e) = p(e \mid \gHead) = N(\angle e \mid \gHead, \sigma_\text{dev}^2).
|
|
\label{eq:transSimple}
|
|
\end{equation}
|
|
|
|
|
|
|
|
|
|
|
|
\section{TITLE?}
|
|
|
|
Assuming navigation, the pedestrian wants to reach a well-known destination and represents additional
|
|
prior knowledge. Most probabily, the pedestrian will stick to the path presented by
|
|
a navigation system. However, some deviations like chatting to someone or taking another router
|
|
cannot be strictly ruled out. We will therefor describe a system that is able to deal with such variants
|
|
as well as present an algorithm to calculate realistic routes based on aforemention grid.
|
|
|
|
Simply running a shortest-path algorithm as Dijkstra or A* \todo{cite} using the previously created floorplan
|
|
would oviously lead to non-realistic paths sticking to the walls and walking many diagonals. In order
|
|
to calculate paths the resemble pedestrian walking behaviour we thus need some adjustments to the
|
|
route calculation.
|
|
|
|
\subsection{wall avoidance}
|
|
|
|
As already mentioned, shortest-path calculation usually sticks close to walls to reduce the path's length.
|
|
Pedestrian's however, walk either somewhere near (but not close to) a wall or, for larger hallways/rooms,
|
|
somewhere far from the walls. Based on those assumptions, an importance factor is derived for each vertex
|
|
within the graph.
|
|
|
|
To get the distance of each vertex from the nearest wall, an inverted version $G' = (V', E')$ of the graph $G$
|
|
is built. A nearest-neighbor search \todo{cite} $\mNN(v_{x,y,z}, G')$ will then provide the nearest wall-vertex
|
|
$v'_{x,y,z} \in V'$ from the inverted graph. The wall avoidance is calculated as follows:
|
|
|
|
\begin{align}
|
|
d &= \text{dist}(v, v'), \enskip 0.0 < d < 2.2 \\
|
|
\text{wa}_{x,y,z} = & - 0.30 \enspace \mathcal{N}(d \mid 0.0, 0.5^2) \label{eq:wallAvoidanceDownvote} \\
|
|
& + 0.15 \enspace \mathcal{N}(d \mid 0.9, 0.5^2) \\
|
|
& + 0.15 \enspace \mathcal{N}(d \mid 2.2, 0.5^2)
|
|
\label{eq:wallAvoidance}
|
|
\end{align}
|
|
|
|
The $\mu$, $\sigma$ and scaling-factors were chosen empirically.
|
|
While this approach provides good results for most areas, doors are downvoted by
|
|
\refeq{eq:wallAvoidanceDownvote}, as they have only vertices that are close to walls.
|
|
Door detection thus is the next conducted step.
|
|
|
|
|
|
|
|
|
|
\subsection{door detection}
|
|
|
|
Doors are usually anchored between two (thin) walls and have a normed width. Examining only a limited region
|
|
around the door, its surrounding walls describe a flat ellipse with the same center as the door itself. It is thus
|
|
possible to detect doors within the floorplan using a PCA \todo{cite}.
|
|
|
|
To decide whether a vertex $v_{x,y,z}$ within the (non-inverted) grid $G$ belongs to a door, we use $k$-NN \todo{cite} to fetch its
|
|
$k$ nearest neighbors $N'$ within the inverted grid $G'$. For this neighborhood the centroid $\vec{c} \in \R^3$ is calculated.
|
|
If the distance $\| \vec{c} - v_{x,y,z} \|$ between the centroid and the vertex-in-question is above certain threshold,
|
|
the node does not belong to a door.
|
|
\todo{diese distanzformel oder dist(a,b)?}
|
|
%ugly...
|
|
%\begin{equation}
|
|
% \vec{c} = \frac{ \sum_{v_{x,y,z} \in N'} v_{x,y,z} }{k}
|
|
%\end{equation}
|
|
|
|
Assuming the distance was fine, we compare the two eigenvalues $\{e_1, e_2 \mid e_1 > e_2\}$ , determined by the PCA.
|
|
If their ratio $\frac{e_1}{e_2}$ is above a certain threshold (flat ellipse)
|
|
the node-in-question belongs to a door or some kind of narrow passage.
|
|
|
|
\begin{figure}
|
|
\includegraphics[width=\columnwidth]{door_pca}
|
|
\caption{Detect doors within the floorplan using k-NN and PCA. While the white nodes are walkable, the black ones represent walls. The grey node is the one in question.}
|
|
\label{fig:doorPCA}
|
|
\end{figure}
|
|
|
|
Fig. \ref{fig:doorPCA} depicts all three cases where
|
|
(left) the node is part of a door,
|
|
(middle) the distance between node and k-NN centroid is above the threshold and
|
|
(right) the ration between $e_1$ and $e_2$ is below the threshold.
|
|
|
|
Like before, we apply a distribution based on the distance from the nearest door to determine
|
|
an importance-factor for each node:
|
|
|
|
\begin{equation}
|
|
\text{dd}_{x,y,z} = 0.8 \enspace \mathcal{N}( \text{dist}(\vec{c}, v_{x,y,z}) \mid, 0.0, 1.0 )
|
|
\end{equation}
|
|
|
|
|
|
|
|
|
|
\subsection{path estimation}
|
|
|
|
Based on aforementioned assumptions, the final importance for each node is
|
|
|
|
\begin{equation}
|
|
\text{imp}_{x,y,z} = 1.0 + \text{wa}_{x,y,z} + \text{dd}_{x,y,z} ,
|
|
\end{equation}
|
|
|
|
and can be seen in fig. \ref{fig:importance}.
|
|
|
|
\begin{figure}
|
|
\includegraphics[angle=-90, width=\columnwidth, trim=20 19 17 9, clip]{floorplan_importance}
|
|
\caption{The calculated importance factor for each vertex. While the black elements denote an importance-factor
|
|
of about \SI{0.8}{}, the yellow door-regions denote a high importance of about \SI{1.2}{}.}
|
|
\label{fig:importance}
|
|
\end{figure}
|
|
|
|
To estimate the shortest path to the pedestrian's desired target, we use a modified version
|
|
of Dijkstra's algorithm \cite{todo}. Instead of calculating the shortest path from the start to the end,
|
|
we swap start/end and do not terminate the calculation until every single node was evaluated.
|
|
Thus, every node in the grid knows the shortest path to the pedestrian's target.
|
|
|
|
As weighting-function we use
|
|
|
|
\begin{equation}
|
|
\begin{split}
|
|
\text{weight}(v_{x,y,z}, v_{x',y',z'}) =
|
|
\frac
|
|
{ \text{dist}(v_{x,y,z}, v_{x',y',z'}) }
|
|
{ \text{stretch}(\text{imp}_{x',y',z'}) }
|
|
,
|
|
\end{split}
|
|
\end{equation}
|
|
|
|
whereby $\text{stretch}(\cdots)$ is a scaling function (linear or non-linear) used to adjust
|
|
the impact of the previously calculated importance-factors.
|
|
%
|
|
Fig. \ref{fig:shortestPath} depicts the difference between the path calculated without and
|
|
with importance-factors, where the latter version is clearly more realistic.
|
|
|
|
\begin{figure}
|
|
\includegraphics[angle=-90, width=\columnwidth, trim=20 19 17 9, clip]{floorplan_paths}
|
|
\caption{Comparision of shortest-path calculation without (dotted) and with (solid) importance-factors
|
|
use for edge-weight-adjustment.}
|
|
\label{fig:shortestPath}
|
|
\end{figure}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
\subsection{guidance}
|
|
|
|
Based on the previous calculations, we propose two approaches to incorporate the prior
|
|
knowledge into the transiton model.
|
|
|
|
During every transition, the first algorithm calculates the centroid $\vec{c}$ of the current sample-set:
|
|
\begin{equation}
|
|
\vec{c} = \frac
|
|
{ \sum_{\mStateVec_{t-1}} (\mState_{t-1}^x, \mState_{t-1}^y, \mState_{t-1}^z)^T }
|
|
{N}
|
|
\end{equation}
|
|
This center is used as starting-point for the shortest path. As it is not necessarily part of
|
|
the grid, its nearest-grid-neighbor is used instead.
|
|
The resulting node already knows its way to the pedestrian's destination, but is located somewhere
|
|
within the deviation of the sample set. After slightly advancing it by a fixed value of about \SI{5}{\meter}
|
|
we get a new point outside of the sample-set and closer to the desired destination.
|
|
This new reference node serves as a comparison base
|
|
|
|
\begin{equation}
|
|
p(e) =
|
|
\end{equation}
|
|
|
|
|
|
\begin{figure}
|
|
\includegraphics[angle=-90, width=\columnwidth, trim=20 19 17 9, clip]{floorplan_dijkstra_heatmap}
|
|
\end{figure}
|
|
|
|
|
|
|
|
\commentByFrank{angular-change probability as polar-plot}
|
|
|
|
\commentByFrank{describe the multi-path version}
|
|
|
|
\commentByFrank{describe the single-path version}
|
|
\commentByFrank{exp-dist for distance to the path. more distance = less-likely}
|
|
\commentByFrank{lambda-factor controls the allowed deviation from the shortest-path}
|