\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}