% Chapter 1

\chapter{Design of Algorithms for Body-SLAM} % Main chapter title

\label{Chapter3} % For referencing the chapter elsewhere, use \ref{Chapter1} 

\lhead{Chapter 3. \emph{Design of Algorithms for Body-SLAM}} % This is for the header on each page - perhaps a shortened title

%----------------------------------------------------------------------------------------

Since the RF signal suffers from the noisy characteristics of wireless channel and multi-path distortions, it is natural to resort to other techniques to improve the overall performance of the localization system. One way to enhance the performance of RF localization is to combine the motion information of the capsule by employing a data fusion algorithm such as Kalman filter \cite{jetto1999development} or particle filter \cite{howard2006multi, duda2007vq}. In our previous work \cite{pahlavan2010taking}, we have used both filters to integrate the RSS-based Wi-Fi localization and the movement models from inertial sensors including accelerometers and gyroscopes for cooperative robotic localization in indoor areas. The results were promising since this method shown the potential to enhance the localization accuracy by combining data from various sensor sources. However, as we mentioned previously, inertial sensors that meet the accuracy requirement for the WCE applications are too large to be embedded inside a video capsule and even if they can be embedded as the assembly technology improves, the cost of the capsule will be increased dramatically. In the localization literature, there has been a trend to extract motion parameters from consecutive image sequence to improve the accuracy of RF localization, which is known as visual based Simultaneous Localization and Mapping (V-SLAM) algorithm \cite{silveira2008efficient, castellanos2001multisensor, davison2002simultaneous}. In the WCE application, since the endoscopic capsule continuously takes pictures with very short time interval (up to 6 frames / sec), it is possible to extract the motion information of the capsule by processing the video stream captured by the embedded vision sensor. This motion information can be used as an alternative of inertial sensors to smooth the RF localization results and meanwhile to reconstruct the trajectory that the capsule has traveled in the same manner of V-SLAM for the indoor geo-location.


\section{Formulation of Body-SLAM}

For every location aware application, higher positioning accuracy can be achieved by employing hybrid techniques which take advantage of data fusion of different sensors \cite{jetto1999development}. Since the only two data sources come with the endoscopic capsule are video stream captured by the embedded vision sensor and wireless signal received by the body mounted RF sensors, an intuitive idea to enhance the localization accuracy of the capsule is through combination of the two. As we mentioned before, the endoscopic capsule continuously takes pictures at short time interval (2 - 6 frames / sec) as it travels. Thus, it's possible to obtain information such as how quick the capsule moves and the direction of moving to track the position of the capsule. In this chapter, we present a novel motion tracking algorithm for the endoscopic capsule by analyzing the displacements of unique portion of the scene, which referred as feature points (FPs), between consecutive image frames. The proposed motion tracking algorithm consists of 3 steps: feature points matching, image unrolling and quantitative calculation of motion parameters. Detailed procedures of each step are explained in the upcoming subsections.
\\
\\

\begin{figure}[h]
\centering
\includegraphics[width=0.95 \textwidth]{./Figures/hybrid.eps}
\caption{Overall flow chart of Body-SLAM}
\label{fig:hybrid}
\end{figure}

\clearpage
%----------------------------------------------------------------------------------------
\section{Motion Tracking using Endoscopic Images}

The movement of the endoscopic capsule is highly unpredictable. It may move fast, slow, rotate and with any combination of the movements stated above. This complicated movement of the wireless capsule creates great errors to the localization accuracy since the Received Signal Strength (RSS) [5] various a lots due to fast fading and sudden change of antenna gain caused by flipping and rotating. Thus, knowing how the capsule moves will help us to better understand the radio propagation channel inside human body and therefore enhance the accuracy of the existing localization methods.

\subsection{Analyzing the Content of Endoscopic Images}

\subsubsection{Image segmentation using SRM}

To model the pattern of movements of the endoscopic capsule, we need to categorize the endoscopic images first to get a conceptual idea how the capsule moves \cite{guanqunsvm}. Based on our observation, the received endoscopic images can be briefly categorized into two basic categories: ``facing the tunnel'' (FT) and ``facing the lumen'' (FL). Two sets of typical FT and FL images are shown in Figure~\ref{fig:imageclassification}. FT is the case when the focal axis of the camera is parallel to the center of the intestinal tube. The major feature of this set of images is always there would be a black hole (we call it ``tunnel'' here) somewhere in the picture representing the vanishing line of the intestinal tube. Through a sequence of consecutive FT images, we can clearly see the capsule either moves propelled by the intestinal motility. On the contrast, FL is the case where the capsule tends to stop or moves not as fast as in the FT. The reason why we do such classification is we are trying to develop a geometric model for the FT images to quantitatively calculate the speed of the capsule. 

\begin{figure}[h]
\centering
\includegraphics[width=0.80 \textwidth]{./Figures/imageclassification.eps}
\caption{Two basic categories of endoscopic images}
\label{fig:imageclassification}
\end{figure}

To distinguish the FT images from the FL images seems to be a fairly easy task for the human eye, however, it has been proved to be extremely difficult for the machines. Major sources of difficulties include highly complicated shape of the scene, various lightning conditions and uncontrolled noise include liquid and bubbles inside the GI tract. Given a set of labeled images, finding what is in common among each set and what is difference between different set can provide inductive clues for classifier design. Some normally used feature descriptors such like Histogram of Oriented Gradients (HOG) and Local Binary Pattern (LBP) \cite{poh2010multi} doesn’t work well for our application since no distinguish difference can be found between the two image sets. Thus, in terms of image representation, our approach is a region-based method. We used a Statistical Region Merging (SRM) techniques described in \cite{nock2004statistical} to segment the original image into several sub-regions with each region represent an object. The basic idea of this technique is to grow the major regions iteratively by combining smaller regions or pixels with homogeneous properties. A typical example of segmented FT image is shown in Figure~\ref{fig:2category} with segmented regions shown in their representative colors. From Figure~\ref{fig:2category} we can clearly see that after the segmentation, the image preserved the tunnel shape (the darkest component in the center) while the complex textures around the tunnel are get rid of. This will effectively reduce the variance in the feature space. 

\begin{figure}[h]
\centering
\includegraphics[width=0.9 \textwidth]{./Figures/2category.eps}
\caption{Two sequences of segmented image with Q = 16 Top 2 rows: FT, Bottom 2 rows: FL}
\label{fig:2category}
\end{figure}

The region merging rule is following:

\begin{equation} \label{merge}
P(R,R')=\left\{\begin{matrix}Merge \; \; \; \; if |\bar{R}-\bar{R'}|\leqslant \sqrt{s^2(R,Q)+s^2(R',Q)}
\\ 
Not\; merge \; \; \; \; \; \; Otherwise \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; 
\end{matrix}\right.
\end{equation}

where $\bar{R}$ is the average value of a certain color channel inside region $R$, $s(R,Q)$ is a threshold function whose value is controlled by $Q$. Detailed expression of $s(.)$ can be found in \cite{nock2004statistical}. A good threshold is to find balance between preserving the major components of the scene and the risk of over merging. The choice $Q$ control the coarseness of the segmentation: a large $Q$ will keep more detailed regions while a small $Q$ tends to merge the small regions. From the experimental point of view, we set the value of $Q$ to be 16.

After merging, pixels inside each isolated region share a common color expectation while the expectations between adjacent regions are different for at least one color channel. Then, we can extract features out of the segmented images. To reach a good classification performance, the feature selection must obey the following rule: choose the feature that is more likely to appear in one set other than in the other set. This can be measured by calculating the co-occurrence of similar instances from different sets with the same label. Features that are more distinguishable may increase the precision of classification. Nine features are selected to classify the images. They are the size of the darkest region of the segmented image, length of the darkest region, length of the darkest region, RGB value for the darkest region and RGB value for the remaining regions. 

\subsubsection{Image classification using SVM classifier}

After feature extraction, the segmented images are classified using a Kernel Support Vector Machine (K-SVM). The training data set are labeled in the following format $\begin{Bmatrix} \boldsymbol{x}_i, y_i \end{Bmatrix}$, where $\boldsymbol{x}_i$ is a $n\times 1$ feature vector, each element of the feature vector is composed by the feature we extracted from the previous section, since we extracted 9 features to represent an image, here $n = 9$, and $y_i\in \begin{Bmatrix} +1,-1 \end{Bmatrix}$ is the label of the image. If the endoscopic image is FT, $y_i=+1$, otherwise, $y_i=-1$. Suppose we have some hyperplanes which separates the positive from the negative examples, the points $\boldsymbol{x}$ that lie on the hyperplane must satisfy

\begin{equation}  \label{eq:margin}
\boldsymbol{w}^T \boldsymbol{x}+b = 0
\end{equation}

where $\boldsymbol{w}$ is a weight vector with the same dimension of $\boldsymbol{x}$ and $b$ is a bias term, which is a real number. The distance between a training sample $\boldsymbol{x}_i$ and the boundary, usually called ``geometric margin'', can be expressed as follows:

\begin{equation} \label{eq:max}
\frac{\boldsymbol{w}_i^T+b}{\left \| \boldsymbol{w} \right \|}
\end{equation}

Since the hyperplane expressed by Eq.~\ref{eq:margin} are identical after $\boldsymbol{w}$ and $b$ are scaled by a common constant, we can add a normalized restriction to this expression:

\begin{equation} \label{eq:restrict}
min|\boldsymbol{w}_i^T+b|=1
\end{equation}

Then, the optimal solution is the boundary that maximize the minimum distance which expressed by Eq.~\ref{eq:max}. By restriction of Eq.~\ref{eq:restrict}, this can be reduced to maximization of $\frac{1}{\left \| \boldsymbol{w} \right \|}$.

The above equations are only applicable for the linear separable case. However, for our application, since the content of endoscopic images from different sets sometimes share similar features, the two set of images are not always linear separable. In another word, a hyperplane that can perfectly classify the two image sets does not exist. Thus, we need an approach that able to achieve nonlinear boundaries. Kernel mapping \cite{suykens1999least} is a technique which is used to solve nonlinear separation data set. The basic concept of the Kernel method is to map the vector $\boldsymbol{x}_i$ to a higher dimensional space (possibly infinite dimensional) and do the SVM in this higher dimensional space. Figure~\ref{fig:SVMplot} shows a one dimensional example of kernel mapping. The transformed space should satisfy that the distance is defined in the transformed space and the distance has a relationship to the distance in the original space.

\begin{figure}[h]
\centering
\includegraphics[width=0.9 \textwidth]{./Figures/SVMplot.eps}
\caption{Illustration of feature mapping using Kernel function}
\label{fig:SVMplot}
\end{figure}

\begin{equation} \label{eq:mapping}
\boldsymbol{x}\in \vec{\boldsymbol{R}^n} \overset{mapping}{\rightarrow}\boldsymbol{\phi(x)}\in \vec{\boldsymbol{R}^m}
\end{equation}

where $\boldsymbol{\phi}$ is the mapping function. Since $\boldsymbol{\phi(x)}$ is very high dimensional, it would not be very easy to work with $\boldsymbol{\phi(x)}$ explicitly. If we measure the margin by the kernel function and perform the optimization, a nonlinear boundary can be obtained. 

\subsection{Feature Points Matching}

For the FT images, the translation of the endoscopic capsule inside the small intestine can be modeled as a tiny camera passing through a elastic cylindrical tube as shown in Fig.~\ref{fig:WCEmove}. Since the WCE continuously takes pictures at a rate up to 6 frames/sec, common portions of the scene may present between consecutive images \cite{bell2013image}. These portions of the images are called ``feature points'' (FP). The pattern and magnitudes of the displacements of these feature points can be used as a hint to reveal the speed of the endoscopic capsule. 

\begin{figure}[hb]
\begin{center}
\begin{tabular}{c}
\scalebox{0.5}{\includegraphics[]{./Figures/WCEmove.eps}}
\end{tabular}
\caption{A WCE moving inside the small intestine}
\label{fig:WCEmove}
\end{center}
\end{figure}

To make an accurate estimation of the capsule's speed, it's very important that the FPs extracted from the reference (first) frame can be accurately located in the following frames.


The Affine Scale-invariant Feature Transform (ASIFT) defined by the affine camera model in Eq.~\ref{eq1}, is a perfect matching tool for the WCE images due to its immune property to viewpoint changes, blur, noise and spatial deformations.

$$A=H_{\lambda}R_1(\Psi)T_t R_2(\Phi) \hspace*{4.1cm}$$
\begin{equation} \label{eq1}
= \lambda\begin{bmatrix}
cos\Psi & -sin\Psi\\ 
 sin\Psi& cos\Psi
\end{bmatrix}\begin{bmatrix}
t & 0\\ 
t & 1
\end{bmatrix}\begin{bmatrix}
cos\Phi & -sin\Phi\\ 
 sin\Phi& cos\Phi
 \end{bmatrix}
\end{equation}
where $R$ represents rotation and $T$ represents tilt. $\Psi$ is rotation angle of camera around optical axis. $\Phi$ is longitude angle between optical axis and a fixed vertical plane. $\lambda$ is zoom parameter. Detailed procedure of FPs matching using ASIFT can be found in \cite{morel2009asift}. An example of feature points matching is given in Fig.~\ref{fig:feature point} (a), in which blue ``o'' represents the coordinates of detected FPs in the reference frame, red ``o'' represent the coordinates of matched FPs on the second frame. If we connect the corresponded FP pairs on the same frame (as shown in Fig.~\ref{fig:feature point} (b)), a bunch of motion vectors will be generated representing the displacements of FPs between frames. 


\begin{figure*}[h]
\centering
%\hfill
\begin{minipage}[]{.6\textwidth}
\begin{tabular}{c}
\scalebox{0.5}{\includegraphics[]{./Figures/FPa.eps}}
%\par\vspace{0pt}
%\scalebox{0.3}{\includegraphics[]{fig1b.eps}}\\
\\
(a) Corresponding feature points between\\ two consecutive frames
\end{tabular}
\end{minipage}
%\hfill
\begin{minipage}[]{.3\textwidth}
\begin{tabular}{c}
\scalebox{0.512}{\includegraphics[]{./Figures/FPb.eps}}
%\par\vspace{0pt}
%\scalebox{0.3}{\includegraphics[]{fig1b.eps}}\\
\\
(b) Formation of\\ motion vectors 
\end{tabular}
\end{minipage}
%\hfill
%\caption{Measurement Settings$. \\(a) Shielded Room, (b) Antenna Position on the Test Subject$.}
\caption{Feature matching between two consecutive images using A-SIFT}
\label{fig:feature point}
\end{figure*}


\subsection{Image Unrolling}


To standardize the displacement of each FP pair and facilitate the quantitive calculations of motion parameters that are useful for localization, we need to perform an inverse cylindrical projection \cite{tillo2010inverse} (also referred as ``image unrolling'' in \cite{sathyanarayana2007real}) to project the original cylindrical image onto an flatten view coordinate system, which we called ``unrolled'' image domain. As shown in Fig.~\ref{fig:imageacquire}, given a point $P$ at distance $d$ away from the camera, the angler depth of $P$ is defined as:

\begin{figure*}[h]
\begin{center}
\begin{tabular}{c}
\scalebox{0.7}{\includegraphics[]{./Figures/imageacquire.eps}}
\end{tabular}
\caption{Image acquisition system of WCE.}
\label{fig:imageacquire}
\end{center}
\end{figure*}


\begin{equation} \label{eq1}
 \theta= tan^{-1}\left(\frac{R}{d}\right)
\end{equation}
where $R$ represents the radius of the intestinal tube. It can be seen from Eq.~\ref{eq1} that a smaller angler depth indicates a larger distance away from the camera. To facilitate the derivation of angler depth, we map the coordinate $(x, y)$ of any point on the cylindrical image plane to the unrolled image plane $(x', y')$ by: 

\begin{equation} \label{eq2}
 x'=\frac{L\phi}{2\pi}  \quad \quad y'=r
\end{equation}
where $\phi$ is the angle between point $P$ and the horizontal axis in the cylindrical image plane (shown in Fig.~\ref{fig:imageunroll} (a)).
\begin{equation} \label{eq3}
 \phi =tan^{-1}\left(\frac{y-y_0}{x-x_0}\right)
\end{equation}
$r$ is the radius of the circular ring associated with point $P$ that can be calculated by:
\begin{equation} \label{eq4} 
 r= \sqrt {(x-x_0)^2+(y-y_0)^2}.
\end{equation}
$L$ and $H$ are length and height of the unrolled image plane respectively. Fig.~\ref{fig:imageunroll} illustrates the procedure of image unrolling. 


In this unrolled image plane, $x'$ axis represents the radian angle $\phi$ whose value ranges from $0$ (when $x'=0$) to $2\pi$ (when $x'=L$). $y'$ axis represents angular depth which reflect the distance away from the camera. $y'=0$ represents a $0$ angular depth and $y'=H$ gives the maximum field of view $\eta$ of the camera. As can be seen in Fig.~\ref{fig:imageunroll}(a), after the mapping, the circular rings in the cylindrical image plane are stacked up vertically in the unrolled image plane. Under this new coordinate system, the angular depth of any point $P$ can be calculated directly through its $y'$ value by:

\begin{equation} \label{eq5}
 \theta\cong\left(\frac{y'}{H}\right)\eta
\end{equation}

The angular depth obtained from Eq.~\ref{eq5} would facilitate the calculation on the speed of the capsule and values changes in $x'$ direction would facilitate the calculation of rotation of the capsule. Detailed calculation based on this new coordinate will be presented in the upcoming subsection.

\begin{figure}[h]
\begin{center}
\begin{tabular}{c}
\scalebox{0.5}{\includegraphics[]{./Figures/unroll.eps}}
\end{tabular}
\caption{The process of "unrolling" the cylindrical image}
\label{fig:imageunroll}
\end{center}
\end{figure}


%----------------------------------------------------------------------------------------
\subsection{Speed Estimation}

As mentioned in previous sections, motions of a video capsule can be detected by measuring the displacements of the FPs. To explain better, we use Fig.~\ref{fig:Speed}  to illustrate the procedure of calculating the transition speed of a capsule traveling through the intestinal tube.

In Fig.~\ref{fig:Geographic} (a), point $P$ is a FP detected at a distance $D$ from the initial position of the camera $C$ with its angular depth equals to $\theta_1$. After the camera has moved forward by a distance $d$ to a new position $C'$, the angular depth of $P$ changes to $\theta_2$. The changes in angular depth can be used to calculate the transition speed of the capsule. 

\begin{equation} \label{eq6}
 \theta_1=tan^{-1}\frac{R}{D} \quad \Longrightarrow \quad D=\frac{R}{tan\theta_1}
\end{equation}

\begin{equation} \label{eq7}
 \theta_2=tan^{-1}\frac{R}{D-d}
\end{equation}
Replacing $D$ in Eq.~\ref{eq7} with Eq.~\ref{eq6}, we get:

\begin{equation} \label{eq8}
 d=\frac{R}{tan\theta_2}\left(1-\frac{tan\theta_2}{tan\theta_1}\right)
\end{equation}
since the time interval for this distance $d$ is half a second, the speed of the capsule can be calculated by:

\begin{equation} \label{eq9}
 v=\frac{\frac{1}{N}\sum_{i=0}^N d_i}{0.5}=\frac{2}{N}\sum_{i=0}^N\frac{R}{tan\theta_{2i}}\left(1-\frac{tan\theta_{2i}}{tan\theta_{1i}}\right)
\end{equation}
where $N$ equals to the total number of all detected FPs.

From Eq.~\ref{eq9} it can be seen that information on depth of FP is factored into the final expression of distance moved by the capsule. In this way, the actual displacement $d$ of the camera is independent of the location of the FP chosen in the image. To reemphasize, the unrolling process facilitates the deriving of $\theta_1$ and $\theta_2$ in Eq.~\ref{eq5} and therefore facilitates the deduction of $v$. Similarly, if the capsule moves backward, the speed can be calculated in the same manner as well.

\begin{figure}[h]
\begin{center}
\begin{tabular}{c}
\scalebox{0.8}{\includegraphics[]{./Figures/speed.eps}}
\end{tabular}
\caption{Speed estimation}
\label{fig:Speed}
\end{center}
\end{figure}


%----------------------------------------------------------------------------------------
\subsection{Direction of Moving Estimation}

\begin{figure}[!t]
\begin{center}
\begin{tabular}{c}
\scalebox{0.8}{\includegraphics[]{./Figures/motiontracking.eps}}
\end{tabular}
\caption{Direction of moving of the capsule}
\label{fig:motiontracking}
\end{center}
\end{figure}

Another important aspect for motion tracking is estimating the direction of moving of the capsule. As illustrated in Fig.\ref{fig:motiontracking}. If we define the world coordinate as $(X, Y, Z)$ and capsule's coordinate as $(X', Y', Z')$. The moving direction of the capsule is given by a norm vector $(n_x, n_y, n_z)^T$ in the world coordinate.  After the capsule rotated with angle $\alpha$ around its $X'$ axis (pitch), angle $\beta$ around its $Y'$ axis (yaw) and angle $\gamma$ around its $Z'$ axis (roll), the new direction of the capsule $(n_x', n_y', n_z')^T$ can be calculated by:

\begin{equation} \label{eq10}
\begin{bmatrix}n_x' \\n_y' \\n_z' \end{bmatrix}=\mathbb{R}\cdot\begin{bmatrix}n_x \\n_y \\n_z \end{bmatrix}
\end{equation}
where $\mathbb{R}$ is an accumulative rotation matrix which relates the camera's coordinate system $(X', Y', Z')$ to the world coordinate system $(X, Y, Z)$. If we assume the camera's coordinate system was initially aligned with the world coordinate system with it's focal axis pointed to the $Z$ axis, then, the initial value of $\mathbb{R}$ equals to a $3\times3$ identical matrix. As the capsule moves away from the original position, $\mathbb{R}$ is updated at each time step by: 


\begin{figure}[h]
\begin{center}
\begin{tabular}{c}
\scalebox{0.8}{\includegraphics[]{./Figures/tilt.eps}}
\end{tabular}
\caption{Direction of moving estimation}
\label{fig:direction}
\end{center}
\end{figure}


\begin{equation} \label{eq11}
\mathbb{R}=\mathbb{R}\cdot\mathbb{R}_t\cdot\mathbb{R}^{-1}
\end{equation}
where $\mathbb{R}_t$ is an direction updating matrix that has a following expression:\\\\
\begin{equation} \label{eq12}
\mathbb{R}_t = \begin{bmatrix}cos\alpha cos\gamma & cos\gamma sin\alpha sin\beta - cos\alpha sin\gamma & cos\alpha cos\gamma sin\beta-sin\alpha sin\gamma \\cos\beta sin\gamma & cos\alpha cos\gamma + sin\alpha sin\beta sin\gamma & -cos\gamma sin\alpha + cos\alpha sin\beta sin\gamma \\ -sin\beta & cos\beta sin\alpha & cos\alpha cos\beta \end{bmatrix}
\end{equation}
where $\alpha$, $\beta$ and $\gamma$ are the pitch, yaw and roll angles about the capsule's $X'$, $Y'$ and $Z'$ axises, respectively, during the elapsed time interval. Again, these angles can be obtained without complicated computation in the unrolled image domain.

\begin{itemize}
	\item pitch ($\alpha$) and yaw ($\beta$) estimation
\end{itemize}

During the transition of the capsule, the capsule will tilt toward the direction of the curly intestinal tube. As illustrated in Fig.~\ref{fig:Geographic} (b), point $P$ and point $Q$ are of the same distance from the initial position of the camera $C$. After the camera moves to $C'$ and tilted with angle $\varphi$ towards $Q$, the angular depths of the two FPs changes with different amount of magnitudes. 

\begin{equation} \label{eq13}
 \triangle P=\theta_{P2}-\theta_{P1} \quad \triangle Q=\theta_{Q2}-\theta_{Q1}
\end{equation}

Fig.~\ref{fig:Geographic} (b) shows that angular displacement $\triangle P$ is obviously larger than the angular displacement $\triangle Q$. The magnitude of tilting can be roughly estimated by:
\begin{equation} \label{eq14}
 \varphi \cong \frac{\triangle Q-\triangle P}{max(\triangle P, \triangle Q)} 
\end{equation}

The direction of tilting can be obtained by finding the group of with smallest displacement in $y'$ in the unrolled image domain. Therefore, this tilting angle $\phi$ can be further decomposed into pitch angle $\alpha$ and yaw angle $\beta$ by:

\begin{equation} \label{eq15}
 \alpha = \varphi \cdot cos\phi \quad  \beta = \varphi \cdot sin\phi
\end{equation}

\begin{itemize}
	\item roll ($\gamma$) estimation
\end{itemize}

The calculation of roll angle $\gamma$ is even easier in the unrolled image domain by measuring the horizontal displacements of FPs in the $x'$ axis:

\begin{equation} \label{eq16}
 \gamma=\frac{1}{N}\sum_{i=0}^N\frac{\triangle x_i'}{L}2\pi
\end{equation}
where $\triangle x'$ denotes the horizontal displacement of a FP in the unrolled domain. $L$ is the length of the unrolled image. It can be seen from Eq.~\ref{eq16}, a greater $\triangle x'$ reflects a greater rolling angle $\gamma$ and vice versa. 


\section{Data Fusion of Visual and RF Information}

The two data sources that come with the endoscopic capsule provide complementary characteristics: visual motion tracking is very accurate at low-velocities but suffers from accumulative estimation errors which leads to drifting away, while RF localization provides absolute localization results that is independent from the previous measurements but with certain amount of error for estimation. In this section, we talk about how to fuse the data from both sensors to improve the reliability and accuracy of WCE localization inside small intestine. The proposed hybrid solution utilizes a Kalman filter to predict the position of the capsule based on the motion model extracted from images and obtains feedback from the RF measurements to correct the position estimations. 

\subsection{Kalman Filter}

The state of the mobile robot (in this case the capsule) in the 3D space can be modeled as its $x, y, z$ coordinates and orientation. These parameters can be combined into a state variable vector. As we introduced above, the capsule continuously takes pictures as it moves along, by processing the video stream, we are able to extract the motion information about how far it has moved and its orientation. However, due to the low resolution and low frame rate, these estimations include errors and these errors accumulate frame by frame. This will cause the capsule drifting away from the correct path. The Kalman Filter (KF), which has been widely used for mobile robot navigation, is a smarter way to optimally estimate the state by integrating all available data from various sensor sources. Since the only two data sources come with the endoscopic capsule are video stream captured by the embedded vision sensor and wireless signal received by the body mounted RF sensors, an intuitive idea to enhance the localization accuracy
of the capsule is through combination of the two.

The main idea of KF is to estimate the conditional probability of being in state $\boldsymbol{m}_t$ given available measurements $\boldsymbol{z}_1,\boldsymbol{z}_2...\boldsymbol{z}_{t}$. We call the probability of being in state $\boldsymbol{m}_t$ given measurements $\boldsymbol{z}_1,\boldsymbol{z}_2...\boldsymbol{z}_{t}$ the belief,

\begin{equation} 
Bel(\boldsymbol{m}_t)=P(\boldsymbol{m}_t|\boldsymbol{z}_1,\boldsymbol{z}_2...\boldsymbol{z}_{t})
\end{equation}

We can split this belief definition into the prior belief $Bel^-(\boldsymbol{m_t})$ and the posterior belief $Bel^+(\boldsymbol{m_t})$ using Bayesian rule and Markov assumption.

\begin{equation} 
Bel^-(\boldsymbol{m}_t)=P(\boldsymbol{m}_t|\boldsymbol{z}_1,\boldsymbol{z}_2...\boldsymbol{z}_{t-1})
\end{equation}
$$=\int P(\boldsymbol{m}_t|\boldsymbol{m}_{t-1})Bel^+(\boldsymbol{m}_{t-1})d\boldsymbol{m}_{t-1}$$

\begin{equation} \label{eq:probability}
Bel^+(\boldsymbol{m}_t)=P(\boldsymbol{m}_t|\boldsymbol{z}_1,\boldsymbol{z}_2...\boldsymbol{z}_{t})
\end{equation}
$$=\frac{P(\boldsymbol{z}_t|\boldsymbol{m}_t)Bel^-(\boldsymbol{m}_t)}{P(\boldsymbol{z}_t|\boldsymbol{z}_1,\boldsymbol{z}_2...\boldsymbol{z}_{t-1})} $$

The prior belief is the conditional probability of being at state $\boldsymbol{m}_t$ given all the measurements $\boldsymbol{z}$ up to step $t$. The posterior belief is the conditional probability of being at state $\boldsymbol{m}_t$ given all the measurements $\boldsymbol{z}$ up to and include step $t$. In order to compute the beliefs, we need to find expressions for the system model $P(\boldsymbol{m}_t|\boldsymbol{m}_{t-1})$ and the measurement model $P(\boldsymbol{z}_t|\boldsymbol{m}_t)$.

In order to predict and correct the belief, the KF needs a model for the system and a model for the measurements. The KF assumes a Linear Dynamic System description of the system of which it is estimating the state. The dynamic system may be corrupted by noise sources, which the KF assumes can adequately be modeled by independent, white, zero-mean, Gaussian distributions.

\begin{itemize}
  \item Assumptions
  
  The KF assumes that the system state and measurements can be described by a linear dynamic system. This is a set of linear equations that models the evolution of the state of the system over time and that describes how measurements are related to the state. The KF assumes a linear model, since it simplifies the computations and since often a linear approach is adequate for the problem to be modeled. When the problem is not linear, then we can use linearizing techniques to transform a non-linear problem into a linear. A linear dynamic system consists of a system and a measurement model.
  
  \item System Model
  
  The system model describes how the true state of the system evolves over time. The KF needs this model in order to make predictions about the state. The KF assumes that the state of the system evolves according to the linear equation.
  
\begin{equation} \label{eq:systemmodel}
\boldsymbol{m}_t=\boldsymbol{A}\boldsymbol{m}_{t-1}+\boldsymbol{\omega}_{t-1}
\end{equation}
  
  The true state $\boldsymbol{m}_t \in \boldsymbol{R}^n$ of the system at time $t$ depends on the state of the system one step earlier $\boldsymbol{m}_{t-1}$ and some noise. Matrix $\boldsymbol{A}$ is an $n \times n$ matrix that, without taking into account possible noise in the system, relates the state of the previous time step $t-1$ to the state at the current step $t$. The vector $\boldsymbol{\omega}\in \boldsymbol{R}^n$ represents the noise in the system. 
  
  \item Measurement Model
  
  The measurement model describes how measurements are related to states. The KF needs a model of the measurements in order to correct the state prediction when a measurement is available. If it has a model that given the true state of the system describes what the measurement will be, then it can compare the real measurement with the measurement that the model gives to correct the state prediction. The KF assumes that the measurements can be modeled by an equation that linearly relates the state of the system to a measurement,
  
\begin{equation} \label{eq:measurementmodel}
\boldsymbol{z}_t=\boldsymbol{H}\boldsymbol{m}_{t}+\boldsymbol{\psi}_{t}
\end{equation}

The true measurement $\boldsymbol{z}_t\in \boldsymbol{R}^m$ at time $t$ depends linearly on the state of the system $\boldsymbol{m}_{t}$. The $m \times n$ matrix $H$ relates the current state $\boldsymbol{m}_{t}$ to the measurement $\boldsymbol{z}_{t}$. The vector $\boldsymbol{\psi}\in \boldsymbol{R}^m$ represents the noise during the measurements. 

\item Markov process

Notice in Eq.~\ref{eq:systemmodel} that the state $\boldsymbol{m}_{t}$ at time $t$ does not depend on all other states and measurements given $\boldsymbol{m}_{t-1}$. Also notice in Eq.~\ref{eq:measurementmodel} that, given $\boldsymbol{m}_{t}$, the measurement $\boldsymbol{z}_{t}$ does not depend on all other states and measurements. These properties make the system a Markov process.
 
\end{itemize}


\subsection{Relative Position Predictions using Images}

Given the motion model derived from the previous section, the priori motion state ${\widehat{\boldsymbol{m}}^{-}}_t$ at time step $t$ (without any knowledge of RF measurement) is given by:

\begin{equation} \label{eq17}
{\widehat{\boldsymbol{m}}^{-}}_t=\boldsymbol{A_{t-1} \cdot {\widehat{\boldsymbol{m}}}_{t-1}+\boldsymbol{\omega_{t-1}}}
\end{equation}
motion state vector $\boldsymbol{m_t}$ is defined as $[x,y,z,n_x,n_y,n_z]^T$, where $(x,y,z)$ is the 3D position of the capsule in the world coordinate system and $[n_x,n_y,n_z]$ is a norm vector that indicates the direction of moving of the capsule. $\boldsymbol{\omega_t}$ is a noise term caused by inaccurate motion estimation which follows a normal probability distributions with covariance equal to $\boldsymbol{Q_t}$. $\boldsymbol{A}$ is a $6 \times 6$ transition matrix that relates the previous motion state at time $t-1$ to the current motion state at time $t$. Given that the sum of two Gaussian random variables results in another Gaussian random variable, we derive the probabilistic system model as

\begin{equation} 
P(\boldsymbol{m}_t|\boldsymbol{m}_{t-1})= N(\boldsymbol{A}\boldsymbol{m}_{t-1},\boldsymbol{Q}_t)
\end{equation}

If we plug in all the parameters, Eq.~\ref{eq17} can be rewritten as:  

\begin{equation} \label{eq18}
\begin{bmatrix}x_{t} \\y_{t} \\z_{t}\\n_{x|t}\\n_{y|t}\\n_{z|t} \end{bmatrix}=\begin{bmatrix}1 & 0 & 0 & v \triangle t & 0 & 0 \\ 0 & 1 & 0 & 0 & v \triangle t & 0 \\ 0 & 0 & 1 & 0 & 0 & v \triangle t \\ 0 & 0 & 0 &  &  &  \\ 0 & 0 & 0 &  &  \begin{bmatrix} \mathbb{R} \end{bmatrix}  &  &\\ 0 & 0 & 0 &  &  &  \end{bmatrix} \cdot\begin{bmatrix}x_{t-1} \\y_{t-1} \\z_{t-1}\\n_{x|t-1}\\n_{y|t-1}\\n_{z|t-1} \end{bmatrix} 
\end{equation}
where $v$ is the transition speed of the capsule derived from Eq.~\ref{eq9}. $\triangle t$ is the time interval between frames (half a second).  $\mathbb{R}$ is the same rotation matrix introduced in Eq.~\ref{eq10}. 

In the same way we want to find the probabilistic characteristics of the RF measurement model from Eq.~\ref{eq:measurementmodel}, since this is the distribution $P(\boldsymbol{z}_t|\boldsymbol{m}_{t})$ needed to compute the posterior belief from equation Eq.~\ref{eq:probability}. The RF measurements are modeled according to the measurement model

\begin{equation} \label{eq19}
\boldsymbol{\widehat{z}_t}=\boldsymbol{H_t}\cdot\boldsymbol{\widehat{m}}^{-}_t+ \boldsymbol{\nu_t} 
\end{equation}
where $\boldsymbol{\nu_t}$ is a measurements noise term. Similar to $\boldsymbol{\omega_t}$, $\boldsymbol{\nu_t}$ also followed a normal distribution with covariance equal to $\boldsymbol{R}_t$. In this, we again look at the term $boldsymbol{H_t}\cdot\boldsymbol{\widehat{m}}^{-}_t$ as a Gaussian distribution with mean $N(\boldsymbol{H_t}\cdot\boldsymbol{\widehat{m}}^{-}_t,0)$. Given these two Gaussian distributions, we see that the conditional probability of observing $\boldsymbol{z}_t$ given $\boldsymbol{m}_t$ is Gaussian distributed as

\begin{equation} 
P(\boldsymbol{z}_t|\boldsymbol{m}_{t})= N(\boldsymbol{H}\boldsymbol{m}_{t},\boldsymbol{R}_t)
\end{equation}

$\boldsymbol{H}$ is a $3 \times 6$ matrix which predicts the RF localization based on the prior motion state at time $t$.
\begin{equation} \label{eq20}
\boldsymbol{H}=\begin{bmatrix}1 & 0 & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \end{bmatrix} 
\end{equation}

Once the actual RF localization result $\boldsymbol{z_t}$ is available, we can use it to correct the predicted position of the capsule. The covariance gives an indication of how precise the KF thinks the state estimate elements are estimated. The smaller the variances are, the more precise the state estimate is.

%\begin{figure}[!t]
%\begin{center}
%\begin{tabular}{c}
%\scalebox{0.35}{\includegraphics[]{Fig1.eps}}
%\end{tabular}
%\caption{A typical RF localization infrastructure for VCE.( d1, d2, and d3 are the RSS ranging distances between the capsule and three body mounted sensors respectively. Using $d1$ $d2$ and $d3$ to draw red circles around these sensors, the intersection of these circles should be the position of the capsule.)}
%\label{fig:RFlocalization}
%\end{center}
%\end{figure}

\subsection{Absolute Position Measurements by RF Localization}

To obtain the actual RF measurement $\boldsymbol{z_t}$, a bunch of calibrated external sensors are attached to the anterior abdominal wall of the human body as shown in Figure~\ref{fig:RFlocalization} to detect the wireless signal emitted by the wireless capsule \cite{de2009intestinal}. As we mentioned in Chapter 2, the power of received signal (RSS) is used to identify the distance between the capsule and body mounted sensors using statistical channel models. 

\begin{figure}[h]
\centering
\includegraphics[width=0.99 \textwidth]{./Figures/RFlocalization.eps}
\caption{A typical RF localization system}
\label{fig:RFlocalization}
\end{figure}

\clearpage

\begin{equation} \label{eq20}
 L_{p}(d)=L_{p}(d_0)+10 \alpha  log_{10}  \big( \frac{d}{d_0} \big) + S (d>d_0)
\end{equation}
where $L_{p}(d)$ represents the path loss in dB at some distance $d$ between the transmitter and receiver, $d_0$ is a threshold distance and $\alpha$ is the path loss gradient which is determined by the propagation environment. The parameters of the path loss model developed by National Institute of Standards and Technology (NIST) at MICS band \cite{sayrafian2009statistical} was summarized in Table ~\ref{table:NISTtable}.

Let $(x,y,z)$ be the potential position of the capsule and $(x_i,y_i,z_i)$ be the position of body mounted sensor $i$. The ranging distance between the capsule and sensors can be expressed as:

\begin{equation} \label{eq21}
d_i=10 \left( \frac{L_p(d)-L_p(d_0)}{10\alpha} \right) d_0
\end{equation}

Given 3 or more estimated distances between the capsule and body mounted sensors, the 3D position of the capsule $\boldsymbol{z_t}$ can be calculated using a least square algorithm introduced in Chapter 2 by minimizing the function below:
\begin{equation} \label{eq22}
f(x,y,z)= \sum_{i=1}^N { \left(\sqrt{(x-x_i)^2+(y-y_i)^2+(z-z_i)^2}-d_i^2 \right)^2} 
\end{equation}

\subsection{Correction using RF Localization}

After the actual RF localization $\boldsymbol{z}_t$ is obtained, we can use the priori motion estimate ${\widehat{\boldsymbol{m}}^{-}}_t$ and a weighted difference between the actual RF measurement $\boldsymbol{z_t}$ and the predicted RF measurement $\boldsymbol{\widehat{z}_t}$ to correct the localization results.

\begin{equation} \label{eq23}
{\widehat{\boldsymbol{m}}}_t={\widehat{\boldsymbol{m}}^{-}}_t+\boldsymbol{K}_t\left(\boldsymbol{z}_t-\boldsymbol{\widehat{z}_t}\right)
\end{equation}
where ${\widehat{\boldsymbol{m}}}_t$ is defined as a posteriori motion state estimate given the RF measurement $\boldsymbol{z}_t$. The $3 \times 6$ matrix $\boldsymbol{K}$ in Eq.~\ref{eq23} is called Kalman gain. If we define the priori estimate errors covariance as $\boldsymbol{P}^{-}_t=E[(\boldsymbol{m}_t-\widehat{\boldsymbol{m}}^{-}_t)(\boldsymbol{m}_t-\widehat{\boldsymbol{m}}^{-}_t)^T]$ and a posteriori estimate errors covariance as $\boldsymbol{P}_t=E[(\boldsymbol{m}_t-\widehat{\boldsymbol{m}}_t)(\boldsymbol{m}_t-\widehat{\boldsymbol{m}}_t)^T]$, the Kalman Gain can be expressed as:

\begin{equation} \label{}
\boldsymbol{K}_t=\boldsymbol{P}^{-}_t \boldsymbol{H}^T\left( \boldsymbol{H}\boldsymbol{P}^{-}_t\boldsymbol{H}^T +\boldsymbol{R}\right)^{-1}
\end{equation}

The Kalman Gain controls the weighs of both sensors on the final position estimation: if RF measurement noise is low, then the final estimation is more dependent on the RF measurement. Otherwise, the final estimation is more dependent on the motion model. The whole process of data fusion is illustrated in Fig.~\ref{fig:flowchart}


\begin{figure}
\begin{center}
\begin{tabular}{c}
\scalebox{0.6}{\includegraphics[]{./Figures/kalman.eps}}
\end{tabular}
\caption{A complete flowchart of data fusion of images and RF measurements using a Kalman filter}
\label{fig:flowchart}
\end{center}
\end{figure}