321 lines
15 KiB
TeX
321 lines
15 KiB
TeX
\section{Transition Model}
|
|
\label{sec:trans}
|
|
|
|
\newcommand{\spoint}{l}
|
|
\newcommand{\gHead}{\theta_\text{walk}}
|
|
\newcommand{\gDist}{d_\text{walk}}
|
|
|
|
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 $[\vec{\spoint}_1 \vec{\spoint}_2]$ and the direction $[\vec{\spoint}_2 \vec{\spoint}_3]$.
|
|
}
|
|
\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 grid-vertices corresponding to the starting-edge are determined using an intersection of
|
|
the segment $[ \vec{\spoint}_1 \vec{\spoint}_2 ]$ with the \SI{20}{\centimeter} bounding-box around 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 randomized adjustments:
|
|
%
|
|
\begin{align}
|
|
\mState_{t}^{\mStateHeading} = \gHead &= \mState_{t-1}^{\mStateHeading} + \mObs_t^{\mObsHeading} + \mathcal{N}(0, \sigma_{\gHead}^2) \\
|
|
\gDist &= \mObs_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
|
|
the its direction $\angle e$ and the current heading $\gHead$.
|
|
We will use $p(e)$ to incorporate prior path knowledge to
|
|
favour some vertices/edges. For each single movement on the graph,
|
|
we calculate $p(e)$ for all adjacent 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. $d$ depends on the number of detected steps
|
|
$\mObsVec_t^{\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$ omitting additional prior knowledge:
|
|
|
|
\commentByFrank{das erste $=$ ist komisch. ideen?}
|
|
\commentByToni{Find ich jetzt nicht tragisch. Eher notwendig fuers Verstaendnis.}
|
|
\begin{equation}
|
|
p(e) = p(e \mid \gHead) = \mathcal{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 the distance-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 \cite{Cover1967}
|
|
%$\mNN(v_{x,y,z}, G')$
|
|
will then provide the nearest wall-vertex
|
|
$v'_{x,y,z} \in V'$ from the inverted graph. To get a smooth gradient, the wall avoidance
|
|
is calculated using a normal distribution with the distance from the nearest wall
|
|
and a deviation of \SI{0.5}{\meter}:
|
|
%
|
|
%\begin{equation}
|
|
% d_{v, v'} = \| 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}
|
|
\begin{equation}
|
|
\text{wa}_{x,y,z} = \mathcal{N}( \| v_{x,y,z} - v'_{x,y,z} \| \mid 0.0, 0.5^2) \\
|
|
\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.
|
|
While effectively rendering wall-regions less likely, \refeq{eq:wallAvoidance}
|
|
will obviously have the same effect on all doors located within the building.
|
|
Therefore, a door-detection is necessary, to upvote them again.
|
|
|
|
|
|
|
|
\subsection{Door Detection}
|
|
\label{sec:doorDetection}
|
|
|
|
To automatically detect doors within the floorplan, we utilize the fact that doors are usually
|
|
anchored between two (straight) walls and have a normed width. Examining the region directly
|
|
around it, the door and its surrounding walls describe a flat ellipse with the door as its
|
|
centre.
|
|
%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 $\hat{V}$ within the inverted grid $G'$,
|
|
describing the walls nearby. Hereafter we determine the centroid $\vec{c} \in \R^3$
|
|
and 2D covariance $(x,y)$ for those vertices.
|
|
%
|
|
Using the PCA, we examine the two eigenvalues $\{\lambda_1, \lambda_2 \mid \lambda_1 > \lambda_2\}$,
|
|
for the covariance matrix. If their ratio $\frac{\lambda_1}{\lambda_2}$ is above a certain
|
|
threshold, the neighbourhood describes a flat ellipse and thus either a straight wall or door.
|
|
%
|
|
To prevent a vertex $v_{x,y,z}$ adjacent to such straight walls from also being detected,
|
|
we ensure the distance $\| \vec{c} - v_{x,y,z} \|$ between the centroid and the vertex is
|
|
below a certain threshold. Hereafter, only vertices located within the door itself remain.
|
|
%
|
|
\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 vertex is part of a door,
|
|
(middle) the distance between node and centroid is above the threshold and
|
|
(right) the ration between $\lambda_1$ and $\lambda_2$ is below the threshold.
|
|
|
|
For smooth importance-gradients around doors, we again use a distribution based on
|
|
the distance of a vertex $v_{x,y,z}$ from its nearest door and a deviation
|
|
of \SI{1.0}{\meter} to determine its importance-factor:
|
|
%
|
|
%\commentByFrank{distanzrechnung: formel ok?}
|
|
\begin{equation}
|
|
\text{dd}_{x,y,z} = \mathcal{N}( \| \vec{c} - v_{x,y,z} \| \mid 0.0, 1.0^2 )
|
|
\label{eq:doorDetection}
|
|
\end{equation}
|
|
|
|
The final importance for each node is now calculated using \refeq{eq:wallAvoidance}
|
|
and \refeq{eq:doorDetection}:
|
|
%
|
|
\begin{equation}
|
|
\text{imp}_{x,y,z} = 1.0 - \text{wa}_{x,y,z} + \text{dd}_{x,y,z} \enspace .
|
|
\end{equation}
|
|
%
|
|
While most vertices receive a factor of $1.0$, wall-regions get lower and
|
|
door-regions higher values, depicted in fig. \ref{fig:importance}.
|
|
%
|
|
\begin{figure}
|
|
\includegraphics[width=\columnwidth]{floorplan_importance}
|
|
\caption{The calculated importance-factor for each vertex. While the black wall-elements denote
|
|
a small importance, the yellow door-regions receive much higher values.}
|
|
\label{fig:importance}
|
|
\end{figure}
|
|
|
|
\subsection{Path Estimation}
|
|
\label{sec:pathEstimation}
|
|
|
|
|
|
\commentByFrank{ueberleitung}
|
|
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{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 shortest 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(e) &=
|
|
p(v_{x',y',z'} \mid v_{x,y,z})\\
|
|
&= \mathcal{N} (\angle [ v_{x,y,z} v_{x',y',z'} ] \mid \gHead, \sigma_\text{dev}^2) \cdot \alpha \\
|
|
\alpha &=
|
|
\begin{cases}
|
|
0.9 & \| 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 shortest-path algorithm 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(e) &=
|
|
p(v_{x',y',z'} \mid v_{x,y,z})\\
|
|
& = \mathcal{N} (\angle [ v_{x,y,z} v_{x',y',z'} ] \mid \gHead, \sigma_\text{dev}^2) \cdot \alpha \\
|
|
\alpha &=
|
|
\begin{cases}
|
|
0.9 & \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[width=\columnwidth]{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}
|
|
|