A Non-Rigid Map Fusion-Based RGB-Depth SLAM Method for Endoscopic Capsule Robots

TL;DR

This paper introduces a novel RGB-Depth SLAM technique tailored for endoscopic capsule robots, enabling real-time dense 3D mapping of the gastrointestinal tract with non-rigid surface adaptation.

Contribution

It presents the first visual SLAM method specifically designed for endoscopic capsule robots, incorporating non-rigid map fusion for accurate 3D reconstruction.

Findings

Real-time dense 3D maps of GI tract achieved

Effective non-rigid surface deformation handling

Enhanced localization accuracy in endoscopic navigation

Abstract

In the gastrointestinal (GI) tract endoscopy field, ingestible wireless capsule endoscopy is considered as a minimally invasive novel diagnostic technology to inspect the entire GI tract and to diagnose various diseases and pathologies. Since the development of this technology, medical device companies and many groups have made significant progress to turn such passive capsule endoscopes into robotic active capsule endoscopes to achieve almost all functions of current active flexible endoscopes. However, the use of robotic capsule endoscopy still has some challenges. One such challenge is the precise localization of such active devices in 3D world, which is essential for a precise three-dimensional (3D) mapping of the inner organ. A reliable 3D map of the explored inner organ could assist the doctors to make more intuitive and correct diagnosis. In this paper, we propose to our knowledge for the first time in literature a visual simultaneous localization and mapping (SLAM) method specifically developed for endoscopic capsule robots. The proposed RGB-Depth SLAM method is capable of capturing comprehensive dense globally consistent surfel-based maps of the inner organs explored by an endoscopic capsule robot in real time. This is achieved by using dense frame-to-model camera tracking and windowed surfelbased fusion coupled with frequent model refinement through non-rigid surface deformations.

Figures

Tables

Table 1. Table 1 : AWAIBA NANEYE MONOCULAR CAMERA SPECIFICATIONS

Resolution	250 x 250 pixels
Footprint	2.2 x 1.0 x 1.7 mm
Pixel size	3 x 3 $\mu m^2$
Pixel depth	10 bit
Frame rate	44 fps

Table 2. Table 3 : VOYAGER MINI CAMERA SPECIFICATIONS

Resolution	720 x 480 pixels
Footprint	5.2 x 5.0 x 2.7 mm
Pixel size	10 x 10 $\mu m^2$
Pixel depth	10 bit
Frame rate	15 fps

Table 3. Table 4 : Trajectory lengths and RMSE results in meters

Trajectory ID	POTENSIC	VOYAGER	AWAIBA	LENGTH
1	0.015	0.018	0.020	0.414
2	0.018	0.021	0.023	0.513
3	0.017	0.019	0.025	0.432
4	0.032	0.035	0.042	0.478
5	0.035	0.038	0.045	0.462
6	0.038	0.041	0.048	0.481
7	0.041	0.043	0.049	0.468

Table 4. Table 5 : Trajectory length and 3D surface reconstruction RMSE results in meters

Trajectory ID	POTENSIC	VOYAGER	AWAIBA	Length
1	0.023	0.025	0.028	0.414
2	0.025	0.027	0.032	0.513
3	0.026	0.029	0.034	0.432
4	0.029	0.032	0.035	0.478
5	0.032	0.034	0.038	0.462
6	0.034	0.036	0.041	0.481
7	0.035	0.041	0.044	0.468

Equations

$${\text{P}}_t=\left[\begin{array}{cccc}& {\text{R}}_t& & {\text{t}}_t\\ 0& 0& 0& 1\end{array}\right]\in {\mathbb{SE}}_3.$$

$$E_{icp}=\sum _k(({\text{v}}^k-\exp (\hat{\xi })\text{T}{\text{v}}_k^t)\cdot {\text{n}}^k{)}^2$$

$$E_{rgb}=\sum _{\text{u}\in \Omega }{\left(I(\text{u},{\mathcal{C}}_t^l)-I(\pi (\text{K}\exp (\hat{\xi })\text{T}\text{p}(\text{u},{\mathcal{D}}_t^l)),{\hat{\mathcal{C}}}_{t-1}^a)\right)}^2$$

$$E_{\text{track}}=E_{\text{icp}}+w_{\text{rgb}}{E}_{\text{rgb}},$$

$${\hat{\mathcal{M}}}_{\text{p}}^s=\varphi ({\mathcal{M}}^s)=\sum _{n\in \mathcal{I}({\mathcal{M}}^s,\mathcal{G})}{w}^n({\mathcal{M}}^s)[{\mathcal{G}}_{\text{R}}^n({\mathcal{M}}_{\text{p}}^s-{\mathcal{G}}_{\text{g}}^n)+{\mathcal{G}}_{\text{g}}^n+{\mathcal{G}}_{\text{t}}^n]$$

$${\hat{\mathcal{M}}}_{\text{p}}^s=\sum _{n\in \mathcal{I}({\mathcal{M}}^s,\mathcal{G})}{w}^n({\mathcal{M}}^s){\mathcal{G}}_{\text{R}}^{{n-1}^T}{\mathcal{M}}_{\text{n}}^s,$$

$$w^n({\mathcal{M}}^s)=(1-||{\mathcal{M}}_{\text{p}}^s-{\mathcal{G}}_{\text{g}}^n|{|}_2/d_{\text{max}}{)}^2.$$

Full text

1 Introduction

In the past decade, the advances in microsensors and microelectronics have enabled small, low cost devices in a variety of high impact applications. Following these advances, untethered pill-size, swallowable capsule endoscopes with an on-board camera and wireless image transmission device have been developed and used in hospitals for screening the gastrointestinal (GI) tract and diagnosing diseases such as the inflammatory bowel disease, the ulcerative colitis, and the colorectal cancer. Unlike standard endoscopy, endoscopic capsule robots are non-invasive, painless, and more appropriate to be employed for long-duration screening purposes. Moreover, they can access difficult body parts that were not possible to reach before with standard endoscopy (e.g., small intestines). Such advantages make pill-size capsule endoscopes a significant alternative screening method over standard endoscopy [1, 2, 3, 4].

However, current capsule endoscopes used in hospitals are passive devices controlled by peristaltic motions of the inner organs. The control over the capsule’s position, orientation, and functions would give the doctor a more precise reachability of targeted body parts and more intuitive and correct diagnosis opportunity. Therefore, several groups have recently proposed active, remotely controllable robotic capsule endoscope prototypes equipped with additional functionalities, such as local drug delivery, biopsy, and other medical functions [5, 6, 7, 8, 9, 2, 10, 11, 12, 13, 14, 15]. An active motion control is, on the other hand, heavily dependent on a precise and reliable real-time pose estimation capability, which makes the robot localization and mapping the key capability for a successful endoscopic capsule robot operation. In the last decade, several localization methods [4, 16, 17, 18, 19] were proposed to calculate the 3D position and orientation of the endoscopic capsule robot such as fluoroscopy [4], ultrasonic imaging [16, 17, 18, 19], positron emission tomography (PET) [4, 19], magnetic resonance imaging (MRI) [4], radio transmitter based techniques, and magnetic field-based techniques [19, 20]. The common drawback of these localization methods is that they require extra sensors and hardware to be integrated to the robotic capsule system. Such extra sensors have their own drawbacks and limitations if it comes to their application in small-scale medical devices, such as space limitations, cost aspects, design incompatibilities, biocompatibility issue, accuracy, and the interference of the sensors with the activation system of the device.

2 Visual SLAM Structure and Survey

As a solution of these issues, a trend of vision-based localization methods have attracted the attention for the localization of such small-scale medical devices. With their low cost and small size, cameras are frequently used in localization applications where weight and power consumption are limiting factors, such as in the case of small-scale robots. However, many challenges posed by the GI tract and low quality cameras of the endoscopic capsule robots are causing further difficulties in front of a vision based SLAM technique to be applied in that field. One of the most important challenges is the non-rigid structure of the GI tract, which causes deformations of the organ tissue making robotic mapping and localization even more difficult. Self-repetitiveness of the GI tract texture, heavy reflections caused by the organ fluids, peristaltic motions of the inner organs, and lack of distinctive feature points on the GI tract tissue are further challenges in front of a reliable robotic operation. Moreover, the low frame rate and resolution of the current capsule camera system also restrict the applicability of computer vision methods inside the GI tract. Especially visual localization methods based on feature point detection and tracking have less performance in the abdomen region than other application areas such as outdoor or indoor large scale environments.

Such a modern visual SLAM method is expected to be equipped with an accurate camera pose estimation module that is not affected by sudden movements, blur, noise, illumination changes, occlusions and large depth variations. Such a SLAM method should have a precise map reconstruction module that is capable of generating efficient dense scene representations in regions of little texture. A map maintenance method improving the map quality with resilience against dynamic changing small and large scale environments and a failure recovery procedure reviving the system from significantly large changes in camera viewpoints are further expected capabilities of such a modern SLAM system.

Modern visual SLAM methods are categorized as direct and indirect. Direct methods -also known as dense or semi-dense methods- exploit the information available at every pixel to estimate camera pose and to reconstruct the 3D map [21]. Therefore, they are more persistent than indirect methods in regions with poor texture. Nevertheless, direct methods are susceptible to failure when scene illumination changes occur as the minimization of the photometric error assumes brightness consistency constraint between frames.

Indirect methods were introduced to reduce the computational complexity and achieve real time processing time. This is achieved by using sparse feature points instead all pixel information [22]. Features are expected to be distinctive and invariant to viewpoint and illumination changes, as well as resilient to blur, occlusion and noise. On the other hand, it is desirable for feature extractors to be computationally efficient and fast. Unfortunately, such objectives are hard to achieve at the same time and a trade-off between computational speed and feature quality is required.

Dense RGB Depth SLAM maps a space by fusing the RGB and depth data into a representation of the continuous surfaces it contains, permitting accurate viewpoint-invariant localization as well as offering the potential for detailed semantic scene understanding [12]. However, existing dense SLAM methods suitable for incremental, real-time operation struggle when the sensor makes movements which are both of extended duration and often criss-cross loop back on themselves. Such a trajectory is typical of an endoscopic capsule robot aiming to explore and densely map the GI tract organs of a patient.

Despite there exists no work done for vision-based endoscopic capsule robot localization and mapping in the literature, some works exist for hand-held standard endoscopic SLAM. Visual SLAM for Hand-held Monocular Endoscope [23] is one approach which uses visual features to localize hand-held monocular endoscopes. This paper claims to achieve good results but since the results were achieved in a very controlled environment with rigid and well textured surface and perfect lighting without reflections, the results are not comparable for challenge real scenes. ORB-SLAM-based Endoscope Tracking and 3D Reconstruction [24] is another approach addressing the issue of endoscopic localization. This algorithm uses ORB descriptors as detected landmarks and using these descriptors the images are tracked in real time and the endoscope is localized using an already available 3D map information. As mentioned earlier, the nature of endoscopic images makes it rather difficult to maintain the tracking until operation executes.

3 System Overview and Analysis

Our architecture follows the traditional approach in real-time dense visual SLAM systems that alternates between localization and mapping [25, 26, 27, 28, 29, 30, 31]. Making significant use of GPU programming with CUDA library, robot localization module was implemented. OpenGL Shading Language was utilized to manage map reconstruction and maintenance. Our approach is based on estimating a fully dense 3D map of the inner organ tissue explored with an endoscopic capsule robot in real-time. In the following, key points of the proposed system are summarized (1):

Detect and remove specular reflections using an authentic reflection suppression method; 2. 2.

Correct vignetting distortions; 3. 3.

Create 3D maps of consecutive endoscopic images using the Tsai-Shah surface reconstruction method [32]; 4. 4.

Estimate a fused surfel-based model of the observed parts of the inner organ inspired by the surfel-based fusion system [27]; 5. 5.

Define the environment into active and inactive areas inspired by the frame-to-model fusion approach [31]. Areas are getting inactive if they have not appeared for a certain period of time $\delta t$. Tracking and fusing modules use data in the active area and do not use inactive areas. 6. 6.

Every frame, search for intersecting parts of active and inactive areas of the model. In case there exists an intersection, fuse the portion of the active model within the current estimated camera frame with the portion of the inactive model in the same frame. A successful fusion realizes the loop-closure to the previously inactive model by deforming the entire model in a non-rigid fashion. The previous inactive part is reactivated and served for a tracking and surface fusion between the registered areas.

At the time of writing, besides being the first visual SLAM method specifically developed for endoscopic capsule robots, we believe our proposed system to be the first of its kind for hand-held endoscopic SLAM methods with many proposed novelties such as the use of photometric and geometric frame-to-model predictive tracking in a fused surfel-based dense map, use of dense model-to-model local surface loop closures with a non-rigid space deformation.

3.1 Preprocessing

The proposed system starts with a preprocessing module that suppresses reflections caused by inner organ fluids. The reflection detection is done by combining the gradient map of the input image with the peak values detected by an adaptive threshold operation. For the suppression of the detected reflections, inpainting method is applied onto the detected reflection distorted areas.

3.2 Shapes from shading-based 3D map reconstruction

Since the light source direction is known both in endoscopic capsule robot applications and standard hand-held endoscope systems, shape from shading technique is an effective and powerful method to reconstruct the 3D surface of the GI tract. In our system, we implemented the Tsai and Shah surface reconstruction method [32]. This method uses a linear approximation to calculate depth map in an iterative manner using the estimated slant, tilt and albedo values. For further details of the method, the reader is suggested to refer to the original paper of the method. Figure 2 demonstrates the results after applying the reflection suppression and shape from shading-based 3D surface reconstruction module.

3.3 Fused predicted tracking

Our scene representation is an unordered list of surfels $\mathcal{M}$ inspired by the representation used in [12]. Each surfel $\mathcal{M}^{s}$ has a position $\textbf{p}\in\mathbb{R}^{3}$, normal $\textbf{n}\in\mathbb{R}^{3}$, colour $\textbf{c}\in\mathbb{N}^{3}$, weight $w\in\mathbb{R}$, radius $r\in\mathbb{R}$, initialisation timestamp $t_{0}$ and last updated timestamp $t$. The radius of each surfel is intended to represent the local surface area around a given point while minimising visible holes [33]. Surfel initialisation and depth map fusion follow the same scheme and surfel colours follow the same moving average approach as in [12]. Pose estimation is realized by estimating a full colour splatted rendering of the surfels and performing a photometric frame-to-model tracking [31]. A time window threshold $\delta t$ is predefined which labels the surfels in $\mathcal{M}$ as active and inactive. Only active surfels are taken into account for robot trajectory estimation and depth map fusion. A surfel in $\mathcal{M}$ is declared as inactive when the time since that surfel was last updated is greater than $\delta t$ [31].

3.3.1 Joint photometric and geometric pose estimation from a splatted surfel prediction

We define the image space domain as $\Omega\subset\mathbb{N}^{2}$, where an RGB-D frame is composed of a depth map $\mathcal{D}$ of depth pixels $d:\Omega\rightarrow\mathbb{R}$ acquired by shape from shading technique and a colour image $\mathcal{C}$ of colour pixels acquired by the RGB camera of the endoscopic capsule robot $\textbf{c}:\Omega\rightarrow\mathbb{N}^{3}$. A normal map is extracted for every depth map. Given a depth map $\mathcal{D}$, the 3D back-projection of a point $\textbf{u}\in\Omega$ is defined as $\textbf{p}(\textbf{u},\mathcal{D})=\textbf{K}^{-1}\textbf{{u}}d(\textbf{u})$, where K is the camera intrinsics matrix and u is the homogeneous form of u. The intensity value of a pixel $\textbf{u}\in\Omega$ given a colour image $\mathcal{C}$ with colour $\textbf{c}(\textbf{u})=[c_{1},c_{2},c_{3}]^{T}$ is defined as $I(\textbf{u},\mathcal{C})=(0.2989\times c_{1}+0.5870\times c_{2}+0.1140\times c_{3})$ as predefined NTSC standards. Current depth map and colour image will be fused with the surfel-splatted predicted depth map and colour image of the current active model to determine the global pose $\textbf{P}_{t}$ of the camera. This is called joint photometric and geometric pose estimation approach. All camera poses are represented with a transformation matrix where

[TABLE]

The geometric pose $\xi$ between the current depth map $\mathcal{D}^{l}_{t}$ and the predicted active depth map from the last frame $\hat{\mathcal{D}}_{t-1}^{a}$ is calculated by minimizing the cost function over the point-to-plane error between 3D back-projected vertices:

[TABLE]

where $\textbf{v}^{k}_{t}$ is the back-projection of the $k$-th vertex in $\mathcal{D}^{l}_{t}$, $\textbf{v}^{k}$ and $\textbf{n}^{k}$ are the corresponding vertex and normal represented in the previous camera coordinate frame. T is the current estimate of the transformation from the previous camera pose to the current one and $\exp{(\hat{\xi})}$ is the matrix exponential that maps a member of the Lie algebra $\mathfrak{se}_{3}$ to a member of the corresponding Lie group $\mathbb{SE}_{3}$. Vertices are associated using projective data association [30].

The photometric pose $\xi$ between the current colour image $\mathcal{C}^{l}_{t}$ and the predicted active model colour from the last frame $\hat{\mathcal{C}}^{a}_{t-1}$ is calculated by minimising the cost function over the photometric error between pixels which is defined as the intensity difference:

[TABLE]

where as above T is the current estimate of the transformation from the previous camera pose to the current one. The joint photometric and geometric pose is defined by the cost function:

[TABLE]

with $w_{\textrm{rgb}}=0.1$ in line with related work [28, 34]. For the minimisation of this cost function, the Gauss-Newton non-linear least-squares method with a three-level coarse-to-fine pyramid scheme is applied.

3.4 Deformation graph

Loop closures are carried out in the set of surfels $\mathcal{M}$ by non rigid deformations according to the surface constraints provided by the local loop closure method in order to ensure surface consistency in the 3D reconstructed non rigid organ surface. For that purpose, a space deformation approach based on the embedded deformation technique of [35] is applied.

A deformation graph consists of a set of nodes and edges distributed throughout the model to be deformed. Each node $\mathcal{G}^{n}$ has a time-stamp $\mathcal{G}^{n}_{t_{0}}$, a position $\mathcal{G}_{g}^{n}\in\mathbb{R}^{3}$ and set of neighboring nodes $\mathcal{N}(\mathcal{G}^{n}$). The directed edges of the graph are the neighbors of each node. A graph is connected up to a neighbor count $k$ such that $\forall n,|\mathcal{N}(\mathcal{G}^{n})|=k$. Each node also stores an affine transformation in the form of a $3\times 3$ matrix $\mathcal{G}^{n}_{\textbf{R}}$ and a $3\times 1$ vector $\mathcal{G}_{\textbf{t}}^{n}$ , initialized by default to the identity and $(0,0,0)^{T}$ respectively. When deforming a surface, the $\mathcal{G}^{n}_{\textbf{R}}$ and $\mathcal{G}_{\textbf{t}}^{n}$ parameters of each node are optimized according to surface constraints.

In order to apply a deformation graph to the surface, each surfel $\mathcal{M}^{s}$ identifies a set of influencing nodes in the graph $\mathcal{I}(\mathcal{M}^{s},\mathcal{G})$. The deformed position of a surfel is given by [31]:

[TABLE]

while the deformed normal of a surfel is given by:

[TABLE]

where $w^{n}(\mathcal{M}^{s})$ is a scalar representing the influence node $\mathcal{G}^{n}$ has on surfel $\mathcal{M}^{s}$, summing to a total of $1$ when $n=k$:

[TABLE]

Here, $d_{\textrm{max}}$ is the Euclidean distance to the $k+1$-nearest node of $M^{s}$. For the construction, optimization and application of the deformation graph into the map, we followed the rules represented by [31].

3.5 Loop closure

To ensure local surface consistency throughout the map our system closes many small loops with the existing map as those areas are revisited. We fuse into the active area of the model while gradually labeling surfels that have not been seen in a period of time $\delta t$ as inactive. The inactive area of the map is not used for frame tracking and fusion until a loop is closed between the active model and inactive model, at which point the matched inactive area becomes active again. This has the advantage of continuous frame-to-model tracking and also model-to-model tracking which provides viewpoint-invariant local loop closures [31].

We divide the set of surfels in our map $\mathcal{M}$ into two disjoint active and inactive sets. A match between the active and inactive sets is tried to be established by registering the predicted surface renderings of the sets from the latest pose estimate. Our algorithm does not check global loop closures since the inner organ volumes are quite small.

Before the activation of the deformation functionality, the algorithm checks the final condition of the Gauss-Newton optimization used to align the two views. If a high quality alignment has been achieved, a set of surface constraints $\mathcal{Q}$ are generated and fed into the deformation graph to align active and inactive surfels.

4 EXPERIMENTS AND RESULTS

We evaluate the performance of our system both quantitatively and qualitatively in terms of trajectory estimation, surface reconstruction accuracy and computational performance.

4.1 Dataset, equipment and specifications

Since there is no publicly available dataset existing for 6-DoF endoscopic capsule robot localization and mapping to our knowledge, we created our authentic capsule robot dataset with ground truths. To make sure that our dataset was not narrowed down to just one specific endoscopic camera, three different endoscopic cameras were used to capture the endoscopic videos. The cameras used for recording the dataset were the AWAIBA Naneye, VOYAGER and POTENSIC endoscopic camera. Specifications of the cameras can be found in Table 2, Table 2 and Table 3, respectively. We mounted endoscopic cameras on our magnetically activated soft capsule endoscope (MASCE) systems as seen in Figure 5. The videos were recorded on an oiled non-rigid, realistic surgical stomach model Koken LM103 - EDG (EsophagoGastroDuodenoscopy) Simulator. To obtain 6-DoF localization ground truth for the captured videos, OptiTrack motion tracking system consisting of four infrared cameras and a tracking software was utilized. Figure 3 demonstrates our experimental setup as a visual reference. A total of 15 minutes of stomach imagery was recorded containing over 10000 frames. Some sample frames of the dataset are shown in Figure 4. Finally, we scanned the open surgical stomach model using the 3D Artec Space Spider image scanner. This 3D image scan served as the ground truth for the error calculation for our 3D map reconstruction system (6).

4.2 Trajectory Estimation

Table 4 demonstrates the results of the trajectory estimation for 7 different trajectories. Trajectory 1 is an uncomplicated path with slow incremental translations and rotations. Trajectory 2 follows a comprehensive scan of the stomach with many local loop closures. Trajectory 3 contains an extensive scan of the stomach with more complicated local loop closures. Trajectory 4 consists of more challenge motions including faster rotational and translational movements. Trajectory 5 consists of very loopy and complex motions with many loop closures. Trajectory 6 is the same of trajectory 5 but included synthetic noise to see the robustness of the system against noise effects. Before capturing trajectory 7, we added more paraffin oil into the simulator tissue to have heavier reflection conditions. Similar to the trajectory 6, trajectory 7 consists of very loopy and complex motions including very fast rotations, translations and drifting. Some qualitative tracking results of our proposed system and corresponding ground truth trajectory sections are demonstrated in figures 7(a),7(b),8(a), and 8(b). For the quantitative analysis, we used the absolute trajectory (ATE) root-mean-square error metric (RMSE), which measures the root-mean-square of the euclidean distances between all estimated camera poses and the ground truth poses associated by timestamp [36]. As seen in table 4, the system performs a very robust and accurate tracking in all of the challenge datasets and is not affected by sudden movements, blur, noise or heavy spectral reflections.

4.3 Surface Estimation

We evaluated the surface reconstruction results of our system on the same dataset we used for the trajectory estimation as well. We scanned the non-rigid EGD (Esophagogastroduodenoscopy) simulator to obtain the ground truth 3D data. Reconstructed 3D surface and ground truth 3D data were aligned using iterative closest point algorithm (ICP). RMSE for reconstructed surface was calculated using again the absolute trajectory (ATE) RMSE, which measured this time the root-mean-square of the euclidean distances between estimated depth values and the corresponding ground truth depth values [36] (Figure 9). The detailed RMSE results in Table 5 prove that even in very challenge trajectories such as trajectory 4, 5, 6, and 7 with sudden movements, heavy noise and reflections, our system is capable of providing a reliable and accurate 3D surface reconstruction of the explored inner organ tissue. A sample 3D reconstructed surface is shown in Figure 10 for visual reference.

4.4 Computational Performance

To analyze the computational performance of the system we observed the average frame processing time across the trajectory 2 and 4 sequences. The test platform was a desktop PC with an Intel Xeon E5-1660v3- CPU at 3.00, 8 cores, 32GB of RAM and an NVIDIA Quadro K1200 GPU with 4GB of memory. The execution time of the system is depended on the number of surfels in the map, with an overall average of 48 ms per frame scaling to a peak average of 53 ms implying a worst case processing frequency of 18 Hz.

5 CONCLUSION

Endoscopic capsule robots are promising novel, minimally invasive technological developments in the area of medical devices for the GI tract. In this paper, we have presented for the first time in the literature a visual SLAM method for endoscopic capsule robots. Our system makes use of shape from shading surface reconstruction technique to obtain the depth sense of the consecutive frames and performs time windowed surfel-based dense data fusion in combination with frame-to-model tracking and non-rigid deformation. The proposed system was able to produce a highly accurate 3D map of the explored inner organ tissue and was able to stay close to the ground truth endoscopic capsule robot trajectory even for very challenge datasets confronted with sharp rotations and fast translations, heavy specular reflections and noise. Our system proved qualitatively and quantitatively its effectiveness in occasionally looping capsule robot motions and comprehensive inner organ scanning tasks. In future, we aim to extend our work into the stereo capsule endoscopy applications to achieve even more accurate localization and mapping and demonstrate the accuracy of our results in animal experiments.