300 lines
14 KiB
TeX
300 lines
14 KiB
TeX
\section{Transition Model}
|
|
\label{sec:trans}
|
|
|
|
\newcommand{\spoint}{l}
|
|
\newcommand{\gHead}{\theta}
|
|
\newcommand{\gDist}{d}
|
|
|
|
To sample only transitions 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 section \ref{sec:relatedWork}.
|
|
However, we add improved $z$-transitions by also modelling realistic
|
|
stairwells using nodes and edges, depicted in fig. \ref{fig:gridStairs}.
|
|
|
|
\begin{figure}
|
|
\centering
|
|
\input{gfx/grid/grid}
|
|
\caption{
|
|
Besides the nodes and edges defined by the distinct floors, we add realistic stairs to interconnect them.
|
|
Stairs are given by three points $\vec{\spoint}_1, \vec{\spoint}_2, \vec{\spoint}_3$, defining the
|
|
starting-edge and the direction.
|
|
}
|
|
\label{fig:gridStairs}
|
|
\end{figure}
|
|
|
|
|
|
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
|
|
(see fig. \ref{fig:gridStairs}). The corresponding grid-vertices are determined using an intersection of
|
|
those segments with the bounding-box for each vertex.
|
|
|
|
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.
|
|
|
|
Walking the grid is now possible by moving along adjacent nodes into a given walking-direction
|
|
until a desired distance $\gDist$ is reached \cite{Ebner-15}.
|
|
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:
|
|
%
|
|
\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}
|
|
%
|
|
During a walk, each edge has an assigned probability $p(e)$ which depends on a chosen implementation.
|
|
This probability describes aspects such as the likelihood for walking into the edge's direction $\angle e$
|
|
given the current heading heading $\gHead$. Furthermore, we will incorporate additional prior knowledge to
|
|
favour some vertices/edges. For each single step on the graph, we calculate $p(e)$ for all available edges,
|
|
and, hereafter, randomly draw the to-be-walked edge depending on those probabilities. The random walk ends,
|
|
as soon as the distance $d$ is reached. The latter depends on the number of detected steps
|
|
$\mObsSteps$ and assumes an average step-size of \SI{0.7}{\meter}.
|
|
|
|
For comparison purpose we define a simple weighting method that assigns a probability to each edge
|
|
just based on the deviation from the currently estimated heading $\gHead$:
|
|
|
|
\commentByFrank{das erste $=$ ist komisch. ideen?}
|
|
\commentByToni{Find ich jetzt nicht tragisch. Eher notwendig fuers Verstaendnis.}
|
|
\begin{equation}
|
|
p(e) = p(e \mid \gHead) = N(\angle e \mid \gHead, \sigma_\text{dev}^2).
|
|
\label{eq:transSimple}
|
|
\end{equation}
|
|
|
|
|
|
|
|
|
|
|
|
\section{Navigational Knowledge}
|
|
|
|
Considering navigation, a pedestrian wants to reach a well-known destination which represents additional
|
|
prior knowledge. Most probably, the user will stick to the path presented by
|
|
a navigation system. However, some deviations like chatting to someone or taking another route
|
|
cannot be strictly ruled out. We will therefore describe a system that is able to deal with such
|
|
variations as well as present an algorithm to calculate realistic routes based on the aforementioned grid.
|
|
|
|
\subsection{Wall Avoidance}
|
|
\label{sec:wallAvoidance}
|
|
|
|
As discussed in section \ref{sec:relatedWork}, simply applying a shortest-path algorithm such as Dijkstra or
|
|
A* using the previously created graph would obviously lead to non-realistic paths sticking to the walls and
|
|
walking many diagonals. Pedestrian's however, walk either somewhere near (but not close to) a wall or, for
|
|
larger open spaces, somewhere far from the walls. In order to calculate paths that resemble such a walking
|
|
behaviour, an importance factor is derived for each vertex within the graph. Those will be used to
|
|
adjust weight between two vertices, needed by the shortest-path algorithm.
|
|
|
|
To downvote vertices near walls, we need to get the distance of each vertex from its nearest wall.
|
|
We therefore build an inverted version $G' = (V', E')$ of the graph $G$, just containing walls and other obstacles.
|
|
A nearest-neighbour search $\mNN(v_{x,y,z}, G')$ will then provide the nearest wall-vertex
|
|
$v'_{x,y,z} \in V'$ from the inverted graph \cite{Cover1967}. The wall avoidance can now be calculated as follows:
|
|
%
|
|
\begin{equation}
|
|
d_{v, v'} = d(v_{x,y,z}, v'_{x,y,z}), \enskip 0.0 < d_{v, v'} < 2.2 \\
|
|
\end{equation}
|
|
\begin{equation}
|
|
\begin{array}{ll}
|
|
\text{wa}_{x,y,z} = & - 0.30 \cdot \mathcal{N}(d_{v, v'} \mid 0.0, 0.5^2) \\
|
|
& + 0.15 \cdot \mathcal{N}(d_{v, v'} \mid 0.9, 0.5^2) \\
|
|
& + 0.15 \cdot \mathcal{N}(d_{v, v'} \mid 2.2, 0.5^2)
|
|
\end{array}
|
|
\label{eq:wallAvoidance}
|
|
\end{equation}
|
|
%
|
|
The parameters of the normal distribution and the scaling-factors were chosen empirically.
|
|
While this approach provides good results for most areas, doors are downvoted by
|
|
\refeq{eq:wallAvoidance}, as they have only vertices that are close to walls.
|
|
Door detection and upvoting thus is the next conducted step.
|
|
|
|
|
|
|
|
|
|
\subsection{Door Detection}
|
|
\label{sec:doorDetection}
|
|
|
|
Doors are usually anchored between two 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.
|
|
|
|
To decide whether a vertex $v_{x,y,z}$ within the (non-inverted) grid $G$ belongs to a door, we use $k$-NN to fetch its
|
|
$k$ nearest neighbours $N'$ within the inverted grid $G'$. For this neighbourhood 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 a certain threshold,
|
|
the node does not belong to a door.
|
|
\todo{Distanzformel centroid/vertex. ideen? $\|v1 - v2\|$ oder $d(v1, v2)$?}
|
|
|
|
Assuming the distance is fine, we compare the two eigenvalues $\{\lambda_1, \lambda_2 \mid \lambda_1 > \lambda_2\}$,
|
|
determined by the PCA. If their ratio $\frac{\lambda_1}{\lambda_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 $\lambda_1$ and $\lambda_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:
|
|
%
|
|
\commentByFrank{distanzrechnung: formel}
|
|
\begin{equation}
|
|
\text{dd}_{x,y,z} = 0.8 \cdot \mathcal{N}( \| \vec{c} - v_{x,y,z} \| \mid 0.0, 1.0 )
|
|
\end{equation}
|
|
|
|
|
|
|
|
|
|
\subsection{Path Estimation}
|
|
\label{sec:pathEstimation}
|
|
|
|
Based on aforementioned assumptions, the final importance for each node is given by
|
|
%
|
|
\begin{equation}
|
|
\text{imp}_{x,y,z} = 1.0 + \text{wa}_{x,y,z} + \text{dd}_{x,y,z} \enspace .
|
|
\end{equation}
|
|
%
|
|
A good visualization of the resulting importance-factors 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. 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
|
|
{ \| 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:multiHeatMap} depicts the difference between the path calculated without (dashed) and
|
|
with importance-factors (solid), 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 utilize the prior
|
|
knowledge within the transition model.
|
|
|
|
\subsubsection{Shortest Path}
|
|
|
|
Before every transition, the centroid $\vec{c}$ of the current sample-set $\Upsilon_{t-1}$,
|
|
representing the posterior distribution at time $t-1$, is calculated:
|
|
%
|
|
\begin{equation}
|
|
\vec{c} = \frac
|
|
%{ \sum_{\mStateVec_{t-1}} (\mState_{t-1}^x, \mState_{t-1}^y, \mState_{t-1}^z)^T }
|
|
{ \sum_{i=1}^N \Upsilon_{t-1}^{x,y,z} }
|
|
{N}
|
|
\end{equation}
|
|
%
|
|
oder
|
|
%
|
|
\begin{equation}
|
|
\vec{c} = \frac
|
|
{ \sum_{i=1}^N \{(\mState_{t-1}^x, \mState_{t-1}^y, \mState_{t-1}^z)^T\}^i }
|
|
{N}
|
|
\end{equation}
|
|
%
|
|
oder: the centroid $\vec{c}$ of the current sample-set's 3D positions $\Upsilon_{t-1}^i = \{(\mState_{t-1}^x, \mState_{t-1}^y, \mState_{t-1}^z)^T\}_{i=0}^N$ is calculated:
|
|
%
|
|
\begin{equation}
|
|
\vec{c} = \frac
|
|
%{ \sum_{\mStateVec_{t-1}} (\mState_{t-1}^x, \mState_{t-1}^y, \mState_{t-1}^z)^T }
|
|
{ \sum_{i=1}^N \Upsilon_{t-1}^{i} }
|
|
{N}
|
|
\end{equation}
|
|
|
|
\newcommand{\pathRef}{v_{\hat{x},\hat{y},\hat{z}}}
|
|
\commentByFrank{avg-state vom sample-set. frank d. meinte ja hier muessen wir aufpassen. bin noch unschluessig wie.}
|
|
\commentByToni{Das ist gar nicht so einfach... wir haben nie ein Sample Set eingefuehrt. Nicht mal einen Sample. Wir haben immer nur diesen State... Man könnte natuerlich einfach sagen das $\Upsilon_t$ an set of random samples representing the posterior distribution ist oder einfach nur ein set von partikeln. habs mal eingefuegt wie ich denke}
|
|
|
|
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 determined and used instead.
|
|
The resulting vertex already knows its way to the pedestrian's destination, but is located somewhere
|
|
within the sample-set. We thus calculate the standard deviation for the distance
|
|
of all samples from the centre. After advancing the starting-vertex by three times this deviation
|
|
we get a new point that is: part of the shortest path, outside of the sample-set and closer to the
|
|
desired destination.
|
|
This new reference node $\pathRef$ serves as a comparison base:
|
|
\commentByToni{Allgemein mal zur Schreibweise der Vertices. Irgendwie finde ich dieses $v_{x,y,z}$ nicht so gut. Ich denke jeder sieht das wir 3D haben und deswegen könntem man doch schlicht $v$, $v'$, $\hat{v}$ ... nutzen, oder was denkst du?}
|
|
\commentByFrank{war der vorschlag von frank d. letztes mal, weil man an vertices nicht einfach attribute (x,y,z) anhaengen kann wie wir es bei $\mObsVec$, $\mStateVec$ haben.}
|
|
|
|
\begin{equation}
|
|
\begin{split}
|
|
p(v_{x',y',z'} \mid v_{x,y,z})
|
|
= N(\angle [ v_{x,y,z} v_{x',y',z'} ] \mid \gHead, \sigma_\text{dev}^2) \cdot \alpha \\
|
|
\alpha =
|
|
\begin{cases}
|
|
1.0 & \| v_{x',y',z'} - \pathRef \| < \| v_{x,y,z} - \pathRef \| \\
|
|
0.1 & \text{else}
|
|
\end{cases}
|
|
\end{split}
|
|
\label{eq:transShortestPath}
|
|
\end{equation}
|
|
|
|
Eq. \eqref{eq:transShortestPath} combines the simple transition \refeq{eq:transSimple} with
|
|
a second probability, downvoting all nodes that are farther away from the reference $\pathRef$
|
|
than the previous step. Put another way: grid-steps increasing the distance to the reference
|
|
are unlikely but not impossible.
|
|
|
|
|
|
\subsubsection{Multipath}
|
|
|
|
The Dijkstra calculation mentioned in \ref{sec:pathEstimation} already calculated the
|
|
cumulative distance $\text{cdist}_{x,y,z}$ to the pedestrian's target for each vertex.
|
|
We thus apply the same assumption as above and downvote grid-steps not decreasing
|
|
the distance to the destination:
|
|
|
|
\begin{equation}
|
|
\begin{split}
|
|
p(v_{x',y',z'} \mid v_{x,y,z})
|
|
= N(\angle [ v_{x,y,z} v_{x',y',z'} ] \mid \gHead, \sigma_\text{dev}^2) \cdot \alpha \\
|
|
\alpha =
|
|
\begin{cases}
|
|
1.0 & \text{cdist}_{x',y',z'} < \text{cdist}_{x,y,z} \\
|
|
0.1 & \text{else}
|
|
\end{cases}
|
|
\end{split}
|
|
\label{eq:transMultiPath}
|
|
\end{equation}
|
|
|
|
Fig. \ref{fig:multiHeatMap} shows the heat-map of visited vertices after several \SI{125}{\meter}
|
|
walks simulating slight, random heading changes.
|
|
|
|
\begin{figure}
|
|
\includegraphics[angle=-90, width=\columnwidth, trim=20 19 17 9, clip]{floorplan_dijkstra_heatmap}
|
|
\caption{Heat-Map of visited vertices after several walks using \refeq{eq:transMultiPath}.
|
|
Additionally shows the shortest-path calculation without (dashed) and with (solid) importance-factors
|
|
used for edge-weight-adjustment.}
|
|
\label{fig:multiHeatMap}
|
|
\end{figure}
|
|
|