355 lines
17 KiB
TeX
355 lines
17 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)$ with vertices $v_i \in V$ and undirected edges $e_{i,j} \in E$
|
|
\commentByFrank{notation geaendert. so ok?}
|
|
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
|
|
node's centre $\fPos{v} = (x,y,z)^T$.
|
|
|
|
|
|
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.
|
|
|
|
New states $\mStateVec_{t}$ may now be sampled by starting at the vertex for
|
|
position $\fPos{\mStateVec_{t-1}} = (x,y,z)^T$
|
|
\commentByFrank{eingefuehrt}
|
|
and walking along adjacent nodes into a given walking-direction $\gHead$ until a distance $\gDist$ is
|
|
reached \cite{Ebner-15}.
|
|
Both, heading and distance, are supplied by the current sensor readings $\mObsVec_{t}$
|
|
and thus reflect the pedestrian's real heading and walking speed including uncertainty.
|
|
Working with relative sensor readings, the state's heading is updated during each transition:
|
|
%
|
|
\begin{align}
|
|
\gHead &= \mState_{t}^{\mStateHeading} = \mState_{t-1}^{\mStateHeading} + \mObs_t^{\mObsHeading} + \mathcal{N}(0, \sigma_{\gHead}^2) \\
|
|
\gDist &= \mObs_t^{\mObsSteps} \cdot \mStepSize + \mathcal{N}(0, \sigma_{\gDist}^2)
|
|
.
|
|
\end{align}
|
|
\commentByFrank{fixed. war das falsche makro in (2) und dem satz darunter. das delta musste weg. der state hat ein absolutes heading. step-size als variable}
|
|
%
|
|
During the random walk, each edge has its own probability $p(\mEdgeAB)$
|
|
which e.g. depends on the edge's direction $\angle \mEdgeAB$ and the
|
|
pedestrian's current heading $\gHead$.
|
|
Furthermore, section \ref{sec:nav} uses $p(\mEdgeAB)$ to incorporate prior path knowledge to
|
|
favour edges leading towards the pedestrian's desired target $\mVertexDest$.
|
|
\commentByFrank{fixed}
|
|
|
|
For each single movement on the graph, we calculate $p(\mEdgeAB)$ for all edges
|
|
connected to a vertex $\mVertexA$, and, hereafter, randomly draw the to-be-walked edge
|
|
depending on those probabilities. This step is repeated until the sum
|
|
of the length of all used edges exceeds $d$. The latter depends on the number of
|
|
detected steps $\mObs_t^{\mObsSteps}$ and the pedestrian's step-size $\mStepSize$.
|
|
\commentByFrank{step-size als variable}
|
|
|
|
To quantify the improvement prior knowledge is able to provide,
|
|
we define a simple reference for $p(\mEdgeAB)$ that assigns a probability to each edge
|
|
just based on its deviation from the currently estimated heading $\gHead$:
|
|
%
|
|
\begin{equation}
|
|
p(\mEdgeAB) = p(\mEdgeAB \mid \gHead) = \mathcal{N} (\angle \mEdgeAB \mid \gHead, \sigma_\text{dev}^2).
|
|
\label{eq:transSimple}
|
|
\end{equation}
|
|
|
|
|
|
|
|
|
|
|
|
\section{Navigational Knowledge}
|
|
\label{sec:nav}
|
|
|
|
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 walls and
|
|
%walking many diagonals. Pedestrians however, will probably keep a small gap between themselves and
|
|
%nearby walls.
|
|
%To calculate paths that resemble this behaviour, an importance-factor is derived for
|
|
%each vertex. Those will be used to modify the weight $\fDistance{v}{v'}$ between two vertices
|
|
%$v,v'$, examined by the shortest-path algorithm.
|
|
|
|
Shortest-path algorithms such as Dijkstra use a scalar weight $\fDistance{v_1}{v_2}$ between two vertices
|
|
to determine the path with the lowest overall weight.
|
|
As discussed in section \ref{sec:relatedWork}, simply using the distance
|
|
$\fDistance{\mVertexA}{\mVertexB} = \| \mVertexA - \mVertexB \|$ as weighting function, would obviously lead to non-realistic paths
|
|
sticking to walls and walking many diagonals. Pedestrians however, will probably keep a small gap between
|
|
themselves and nearby walls. To calculate paths that resemble this behaviour, an importance-factor is derived for
|
|
each vertex. Those will be used to scale the distance between two nodes, just like navigation systems use
|
|
the speed-limit as scaling-factor.
|
|
\commentByFrank{so besser? der ganze absatz.}
|
|
To downvote vertices near walls, we need to determine the distance of each vertex from its nearest wall.
|
|
We therefore derive an inverted version $G' = (V', E')$ of the graph $G$, just describing walls and
|
|
obstacles. A nearest-neighbour search \cite{Cover1967} $\fNN{\mVertexA}{V'}$ within $V'$ provides the vertex
|
|
nearest to $\mVertexA$.
|
|
%\begin{equation}
|
|
% v' = \fNN{v}{V'} \enskip .
|
|
%\end{equation}
|
|
To get a smooth gradient around walls, the avoidance-factor
|
|
is derived using a normal distribution with a deviation of \SI{0.5}{\meter}:
|
|
%
|
|
\begin{equation}
|
|
\fWA{\mVertexA} = \mathcal{N}( \| \mVertexA - \fNN{\mVertexA}{V'} \| \mid 0.0, 0.5^2)
|
|
\enskip .
|
|
\label{eq:wallAvoidance}
|
|
\end{equation}
|
|
\commentByFrank{fixed. WA war WallAvoidance. hatte statt ll immer $\|$ gelesen und deshalb nicht verstanden}
|
|
%
|
|
%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 rendering wall-regions less likely, \refeq{eq:wallAvoidance}
|
|
will obviously have the same effect on doors as they are just a small gap between
|
|
consecutive walls. 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 thus 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$ belongs to a door, we use \mbox{$k$-NN} to fetch its $k$ nearest neighbours
|
|
$\{v'_1, v'_2, \dots, v'_k\} \in V'$ within the inverted grid $G'$, denoting nearby walls.
|
|
%Hereafter we determine the centroid $\vec{c} \in \R^3$
|
|
%and 2D covariance $(x,y)$ for those vertices.
|
|
Hereafter we determine their centroid $\vec{c}$ and covariance $\mat{\Sigma}$:
|
|
\begin{equation}
|
|
\vec{c} = \frac{1}{k}\sum_{i=1}^{k} \fPos{v_i}
|
|
,\smallskip
|
|
\mat{\Sigma} = \frac{1}{k} \sum_{i=1}^{k} \varphi \cdot \varphi^T
|
|
,\smallskip
|
|
\varphi = \fPos{v_i}-\vec{c} \enskip.
|
|
\end{equation}
|
|
%
|
|
For $\mat{\Sigma}$, the two largest eigenvalues $\lambda_1, \lambda_2$ with $\lambda_1 > \lambda_2$
|
|
\commentByFrank{fixed}
|
|
are calculated. If their ratio $^{\lambda_1}/_{\lambda_2}$ is above a certain
|
|
threshold, the neighbourhood describes a flat ellipse and thus either a door or a straight wall.
|
|
%
|
|
To filter the latter, we enforce the euclidean distance \mbox{$\| \fPos{v} - \vec{c} \|$} between
|
|
the centroid and the vertex to be very small. Hereafter, only vertices located directly within a
|
|
door itself remain.
|
|
%
|
|
\begin{figure}
|
|
\includegraphics[width=\columnwidth]{door_pca}
|
|
\caption{Detect doors within the floorplan using $k$-NN to get the centroid $\vec{c}$ and covariance $\mat{\Sigma}$
|
|
of the wall-vertices (black) near $\mVertexA$. While the left version is fine, the $\mVertexA$ in the middle is too far
|
|
from $\vec{c}$ and the right one has an invalid eigenvalue-ratio.}
|
|
\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 ratio between $\lambda_1$ and $\lambda_2$ is below the threshold.
|
|
|
|
For smooth gradients around doors, we again use a distribution based on
|
|
the distance of a vertex $\mVertexA$ from its nearest door and a deviation
|
|
of \SI{1.0}{\meter}:
|
|
%
|
|
%\commentByFrank{distanzrechnung: formel ok?}
|
|
\begin{equation}
|
|
\fDD{\mVertexA} = \mathcal{N}( \| \fPos{\mVertexA} - \vec{c} \| \mid 0.0, 1.0^2 )
|
|
\label{eq:doorDetection}
|
|
\end{equation}
|
|
%
|
|
The final importance combines \refeq{eq:wallAvoidance}
|
|
and \refeq{eq:doorDetection}:
|
|
%
|
|
\begin{equation}
|
|
\fImp{\mVertexA} = 1.0 - \fWA{\mVertexA} + \fDD{\mVertexA} \enspace .
|
|
\label{eq:finalImp}
|
|
\end{equation}
|
|
%
|
|
While most nodes receive a neutral 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}
|
|
\input{gfx/floorplan_importance}
|
|
\caption{The calculated importance-factor \refeq{eq:finalImp} for each vertex.
|
|
While the dark wall-elements denote a small importance, the yellow areas around doors and narrow
|
|
passages depict a high importance.}
|
|
\label{fig:importance}
|
|
\end{figure}
|
|
|
|
\subsection{Path Estimation}
|
|
\label{sec:pathEstimation}
|
|
|
|
For routing the pedestrian towards his desired target, a modified version
|
|
of Dijkstra's algorithm is used. Instead of calculating the shortest path from the start to the end,
|
|
the direction is inverted and the calculated terminates as soon as every single node was evaluated.
|
|
Hereafter, every node in the grid knows the distance and shortest path to the pedestrian's target.
|
|
|
|
To get realistic path suggestions, we use the importance-factors to adjust the edge-weight
|
|
$\delta(\mEdgeAB)$ for the Dijkstra:
|
|
%
|
|
\begin{equation}
|
|
\begin{split}
|
|
\delta(\mEdgeAB) =
|
|
\delta(\mVertexA, \mVertexB) =
|
|
\frac
|
|
{ \| \mVertexA - \mVertexB \| }
|
|
{ \fImp{\mVertexB} }
|
|
\enskip.
|
|
\end{split}
|
|
\label{eq:edgeWeight}
|
|
\end{equation}
|
|
%
|
|
Eq. \eqref{eq:edgeWeight} artificially increases the euclidean distance between $\mVertexA, \mVertexB$ when
|
|
$\mVertexB$ approaches a wall and decreases it when encountering a door.
|
|
%
|
|
%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 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 considerations, we propose two approaches to utilize prior
|
|
knowledge within the transition.
|
|
|
|
\subsubsection{Shortest Path}
|
|
|
|
\newcommand{\pathCentroid}{{\vec{\overline{c}}_{t-1}}}
|
|
\newcommand{\pathDev}{\sigma_{t-1}}
|
|
\newcommand{\pathRef}{v_\text{ref}}
|
|
|
|
|
|
Before every transition, the centre-position $\pathCentroid = (x,y,z)^T$ of the current
|
|
\commentByFrank{reicht das so schon?}
|
|
sample-set, representing the posterior distribution at time $t-1$, is calculated.
|
|
%
|
|
%
|
|
%\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 centre serves as the starting point for the shortest-path calculation.
|
|
As it is not necessarily part of the grid, its nearest-grid-neighbour is determined and used instead.
|
|
This vertex is located somewhere within the sample-set and already knows the way to the pedestrian's
|
|
destination $\mVertexDest$.
|
|
%
|
|
As new states $\mStateVec_{t}$ should approach the pedestrian's destination
|
|
we use a reference $\pathRef$ all states try to reach. This references must
|
|
be both part of the shortest path and located somewhere outside of the sample-set.
|
|
%
|
|
We thus calculate the standard deviation of the distance of all sample-positions
|
|
$\fPos{\mStateVec_{t-1}}$ from aforementioned centre $\pathCentroid$.
|
|
\commentByFrank{so klarer? platz fuer groese Eq. fehlt und Notation zum ansprechen jedes einzelnen Particles vermeide ich lieber...}
|
|
|
|
%\begin{equation}
|
|
% d_\text{cen} = \| pos(q_{t-1}) - \pathCentroid \|
|
|
% \sigma_\text{cen} = stdDev(distance)
|
|
%\end{equation}
|
|
|
|
After advancing the starting-vertex by three times this deviation
|
|
we get the new point $\pathRef$ that is: part of the shortest path, outside of the sample-set
|
|
and closer (but not too close) to the desired destination.
|
|
%
|
|
Hereafter, the simple transition \refeq{eq:transSimple} is combined with a second probability,
|
|
downvoting all grid-steps that depart from $\pathRef$.
|
|
To still allow leaving the shortest path, the intensity of the downvoting is controlled via $\mUsePath$,
|
|
with $0 < \mUsePath < 1$.
|
|
Finally, \refeq{eq:transShortestPath} provides a metric tending towards the reference while
|
|
still allowing the pedestrian to leave the shortest path:
|
|
%
|
|
\begin{equation}
|
|
\begin{split}
|
|
p(\mEdgeAB) &=
|
|
p(\mVertexB \mid \mVertexA) =
|
|
\mathcal{N} (\angle \mEdgeAB \mid \gHead, \sigma_\text{dev}^2) \cdot \alpha \\
|
|
\alpha &=
|
|
\begin{cases}
|
|
\mUsePath & \| \mVertexB - \pathRef \| < \| \mVertexA - \pathRef \| \\
|
|
(1-\mUsePath) & \text{else}
|
|
\end{cases}
|
|
\end{split}
|
|
\enskip .
|
|
\label{eq:transShortestPath}
|
|
\end{equation}
|
|
\commentByFrank{$\mUsePath$ als variable}
|
|
%
|
|
|
|
|
|
|
|
\subsubsection{Multipath}
|
|
|
|
The shortest-path algorithm mentioned in \ref{sec:pathEstimation} already calculated the distance
|
|
$\fLength{\mVertexA}{\mVertexDest}$ % = \sum_{i=s}^{e-1} \| v_{i} - v_{i+1} \| $
|
|
for the path from $\mVertexA$ to the pedestrian's destination $\mVertexDest$.
|
|
We thus apply the same assumption as \refeq{eq:transShortestPath} and downvote edges
|
|
not decreasing the distance to the destination:
|
|
%
|
|
\begin{equation}
|
|
\begin{split}
|
|
p(\mEdgeAB) &=
|
|
p(\mVertexB \mid \mVertexA) =
|
|
\mathcal{N} (\angle \mEdgeAB \mid \gHead, \sigma_\text{dev}^2) \cdot \alpha \\
|
|
\alpha &=
|
|
\begin{cases}
|
|
\mUsePath & \fLength{\mVertexB}{\mVertexDest} < \fLength{\mVertexA}{\mVertexDest} \\
|
|
(1-\mUsePath) & \text{else}
|
|
\end{cases}
|
|
\end{split}
|
|
\enskip .
|
|
\label{eq:transMultiPath}
|
|
\end{equation}
|
|
%
|
|
Fig. \ref{fig:multiHeatMap} shows a heat-map of how often vertices were visited after several
|
|
\SI{125}{\meter} walks. The colours from cold to hot indicate that both possible paths
|
|
are covered and slight deviations from the shortest version are possible.
|
|
%
|
|
\begin{figure}
|
|
%\includegraphics[width=\columnwidth, trim=4 8 4 4]{floorplan_dijkstra_heatmap}
|
|
\input{gfx/floorplan_dijkstra_heatmap}
|
|
\caption{Heat-Map showing the number of visits per vertex after $30.000$ walks using \refeq{eq:transMultiPath}.
|
|
Both possible paths are covered and slight deviations are possible.
|
|
Additionally shows the shortest-path calculation without (dashed) and with (solid) importance-factors
|
|
used for edge-weight-adjustment.
|
|
\commentByFrank{so besser?}}
|
|
\label{fig:multiHeatMap}
|
|
\end{figure}
|
|
|