\section{Transition Model}
\label{sec:trans}

	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}
	\label{sec:wallAvoidance}
	
		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}
	\label{sec:doorDetection}
				
		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 $\{\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 $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}
	\label{sec:pathEstimation}
		
		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
						{ \| 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.
		
		\subsubsection{Shortest Path}
		
			Before every transition, the centroid $\vec{c}$ of the current sample-set is calculated:
			\todo{summe laesst sich so nicht schreiben. ideen?}
			\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}
			
			\newcommand{\pathRef}{v_{\hat{x},\hat{y},\hat{z}}}
			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 states from the center. After advancing the starting-vertex by three times the deviation
			we get a new point outside of the sample-set and closer to the desired destination.
			This new reference node $\pathRef$ serves as a comparison base:
			
			\todo{bessere ideen fuer die schreibweise?}
			\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.2 & \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{cdst}_{x,y,z}$ to the pedestrian's target for each vertex.
			We thus apply the same assumption as above and downvote 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{cdst}_{x',y',z'} < \text{cdst}_{x,y,z}		\\
							0.2 & \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}}
				\label{fig:multiHeatMap}
			\end{figure}



	\commentByFrank{angular-change probability as polar-plot}

	\commentByFrank{describe the multi-path version}