2.1. VSLAM and VO for endoscopy

VSLAM is a technique that uses camera vision for simultaneous robot self-locating and scene map construction [Reference Durrant-Whyte and Bailey41]. It enables autonomous robot exploration in unknown or partially unknown environments. The architecture of a classical VSLAM system typically includes a front-end visual odometer, backend optimization, loop closure detection, and finally mapping, as shown in Fig. 2.

Figure 2. Classic VSLAM framework.

VSLAM has the potential to estimate the relative pose of the endoscope camera and construct a viscera surface texture map, which is important for lesion localization and surgical navigation. However, complicated intraoperative scenes (e.g., deformable targets, surface texture, sparsity of visual features, viscera specular reflection, etc.) and strict accuracy requirements have posed challenges to the application of VSLAM to minimally invasive surgery. Recently, Lamarca et al. [Reference Bartoli, Montiel and Lamarca42] proposed a monocular non-rigid SLAM method that combines shape from template (SfT) and non-rigid structure from motion (NRSfM) methods for non-rigid environment scene map construction. However, this method is susceptible to variations in illumination and does not perform well under poor visual texture conditions, rendering it unsuitable for reconstruction of viscera with non-isometric deformations. Later, Gong et al. [Reference Gong, Chen and Li43] constructed an online tracking and relocation framework which employs a rotation invariant Haar-like descriptor and a simplified random forest discriminator to select and track the target region for gastrointestinal biopsy images. Song [Reference Song, Wang and Zhao44] constructed a real-time SLAM system to address scope-tracking problems through an improved localization technique. Much work has focused on adapting VSLAM to enable application to an endoscopic scene, addressing problems such as poor texture [Reference Wei, Feng and Li45, Reference Song, Zhu and Lin46], narrow field of view [Reference Seshamani, Lau and Hager11], and specular reflections [Reference Wei, Yang and Shi47]. Still, the variable illumination problem remains unaddressed. Intraoperative scenarios require accurate camera localization; complex viscera images can lead to mismatched point pairs and thus incorrect camera location. Data association also remains a challenging problem for VSLAM systems in MIS scenarios [Reference Yadav and Kala48]. This paper focuses on addressing the problems of variable illumination and data association for intraoperative scenes.

2.2. SLAM based on SuperPoint and SuperGlue

CNNs have made outstanding achievements in computer vision to aid lesion diagnosis or intraoperative scene reconstruction [Reference Yadav and Kala48–Reference Li, Shi and Long52]. Researchers have studied and improved many aspects of VSLAM with learning-based feature extraction techniques to address variable illumination and poor visceral surface texture in complex surgical scenarios [Reference Liu, Zheng and Killeen53, Reference Liu, Li and Ishii54]. Bruno et al. [Reference Bruno H.M. and Colombini49] presented a novel hybrid VSLAM algorithm based on a Learned Invariant Feature Transform network to perform feature extraction in a traditional backend based on an ORB-SLAM system. Li et al. [Reference Li, Shi and Long52] attempted to use an end-to-end deep CNN in VSLAM to extract local descriptors and global descriptors from endoscopic images for pose estimation. Schmidt et al. [Reference Schmidt and Salcudean50] proposed Real-Time Rotated descriptor (ReTRo), which was more effective than classical descriptors and allowed for the development of surgical tracking and mapping frameworks. However, the aforementioned methods are based on traditional Fast Library for Approximate Nearest Neighbors (FLANN) techniques to track keypoints and match extracted features. FLANN does not perform well at feature point matching of high-similarity images, resulting in mismatches between extracted new features and potential features. Its performance is even worse under variable illumination; therefore, FLANN is not always applicable for MIS [Reference Muja and Lowe55].

This paper proposes to apply a SuperPoint approach for keypoint detection and to utilize the SuperGlue technique to deal with complex data associations in intraoperative scenes. SuperPoint [Reference Detone, Malisiewicz and Rabinovich31] is a self-supervised framework for detecting features and describing points of interest, while SuperGlue [Reference Sarlin, Detone and Malisiewicz37] is a network that can simultaneously filter outliers and match features. Recently, researchers have studied the effectiveness of SuperPoint and SuperGlue in VSLAM systems for MIS [Reference Barbed, Chadebecq and Morlana56, Reference Sarlin, Cadena and Siegwart57]. Barbed et al. [Reference Barbed, Chadebecq and Morlana56] demonstrated that SuperPoint delivers better feature detection in VSLAM than using hand-crafted local features. Laura et al. [Reference Oliva Maza, Steidle and Klodmann58] applied SuperPoint to a monocular VSLAM system to estimate the pose of the ureteroscope tip. Sarlin et al. [Reference Sarlin, Cadena and Siegwart57] proposed a Hierarchical Feature Network (HF-Net) algorithm based on SuperPoint and SuperGlue to predict local features and global descriptors for a 6-DoF localization of the camera. However, existing algorithms require substantial computing power to run in real time, which presents a significant obstacle to building maps in real time. In this work, a SPSVO algorithm is proposed to accelerate the CNN to realize real-time endoscopic pose estimation and viscera surface map construction.

3.1. System overview

The proposed SPSVO approach consists of four main modules: feature extraction, stereo matching, keyframe selection, and pose graph optimization, as shown in Fig. 3. The SPSVO can perform feature matching and keypoint tracking between stereo images and images in different frames, and can avoid incorrect data associations by using matching results of relevant key points. For real-time performance, the SPSVO performs feature tracking of images from only the left eye to reduce computation time. Nvidia TensorRT Toolkit is used to accelerate feature extraction and matching. On the backend, the SPSVO uses a traditional pose graph optimization framework for map construction. The above modules are designed to enable real-time application of the SPSVO within human enterococci and achieve accurate tracking by combining the efficiency of traditional optimization methods and the robustness of learning-based techniques.

Figure 3. Structure of the SPSVO system.

3.2. Image pre-processing

For image pre-processing, the SPSVO uses Contrast-Constrained Adaptive Histogram Equalization (CLAHE) [Reference Zuiderveld59] to enhance contrast, brightness, details, and texture of the input image. Due to severe variability in illumination in optical colonoscopy, some parts of the L-channel color space of the image are overexposed, resulting in image specular reflections, while some images are underexposed and lead to dark areas. In this work, pixels with a luminance greater than 50 are marked as reflective regions, and pixel values in the reflective region are set to the average of surrounding pixels. Possible noise is eliminated by a morphological closure operation. Performance of the CLAHE is demonstrated in Fig. 4. The proposed CLAHE effectively improves the uniformity of illumination and improves the contrast of endoscopic images. Due to the proximity of the endoscope light source to the inner wall of the organ and rapid movement of the endoscope, this pre-processing step allows the system to eliminate the effects of mismatches caused by specular reflections.

Figure 4. Image pre-processing. (a) Original image and (b) image after using CLAHE.

3.3. Proposed SuperPoint model

The SuperPoint network consists of four parts: encoding network, feature point detection network, descriptor detection network, and loss function. The encoder network converts the input image into a high-dimensional tensor representation for the decoder, making it easier to detect and describe key points. The feature point detection network is a decoding structure that calculates keypoint probability for each pixel and embeds a sub-pixel convolution algorithm to reduce computational effort. The descriptor detection network is also a decoding structure that extracts semi-dense descriptors first, performs a bicubic interpolation algorithm to obtain full descriptors, and uses L2-normalization to obtain unit-length descriptors. The loss function is a measure of the difference between the network output and ground truth label, guiding the network to optimize and improve its performance in detecting and describing key points of the input image. This provides better performance for related applications such as VSLAM, 3D reconstruction, and autonomous navigation. The SuperPoint network is trained in PyTorch. The input of the SuperPoint network is a single image I with $I\in R^{H\times W}$ , where $H$ is the height and $W$ is the width of the image, in pixels. The output of the network is positions of key points extracted in each image and their corresponding descriptors.

Based on Barbed [Reference Barbed, Chadebecq and Morlana56], the loss function can be expressed as

(1) \begin{align} L_{SP}\left(X,X',D,D'\mathit{;}\,Y,Y',S\right)=L_{P}\left(X,Y\right)+L_{P}\left(X',Y'\right)+\lambda L_{d}\left(D,D',S\right) \end{align}

where the X and X´ are outputs of the original detection header of image I and warped image I´, respectively. The associated detection pseudo-labels are Y and Y.´; D and D´ are outputs of the raw description header. $S\in R^{H\mathit{/}8\times W\mathit{/}8\times H\mathit{/}8\times H\mathit{/}8}$ is the homography estimation matrix. L _P represents the loss of feature points during detection, which can be used to measure the difference between detected outputs and the pseudo-label. The L _d is the loss function of the descriptor; λ is a weight parameter used to balance the weight of L _p and L _d .

As shown in Fig. 1(b), there are generally multiple specular reflection areas (white spot areas) that exist in an endoscopic image. Most existing feature detection methods tend to detect many feature points around contour areas or specular reflection areas [Reference Barbed, Chadebecq and Morlana56]. For VSLAM, the more evenly the feature points are distributed in the image, the more accurately feature matching can estimate spatial pose relation. To make feature points extracted by SuperPoint evenly distributed in the region of interest, the specularity loss (L _S ), which reconsiders weights of all extracted key points in specular regions, is proposed. The revised loss function is defined as

(2) \begin{align} L_{ESP}\left(I,I',X,X',D,D';\,Y,Y',S\right)=L_{SP}\left(\ldots \right)+\lambda _{S}L_{S}\left(X,I\right)+\lambda _{S}L_{S}\left(X,I'\right), \end{align}

in which λ _S is a scale weighting factor determined by characteristics of the dataset and contribution of each objective function to the model performance. In this work, $\lambda _{s}=100$ . The L _S is defined as

(3) \begin{align} L_{S}\left(X,I\right)=\frac{\sum \begin{array}{l} H,W\\ h,w=1 \end{array}\left[m\left(I\right)_{hw}\cdot d2s\left(\textit{softmd}\left(X\right)\right)_{hw}\right]}{\varepsilon +\sum \begin{array}{l} H,W\\ h,w=1 \end{array}m\left(I\right)_{hw}}, \end{align}

where softmd() and d2s() are SoftMax functions. The $\varepsilon$ is a constant with $\varepsilon =10^{-10}$ [Reference Detone, Malisiewicz and Rabinovich31, Reference Barbed, Chadebecq and Morlana56]. The $m(I)_{hw}$ is a weighting mask, where $m(I)_{hw}\gt 0$ for pixels near a specularity and 0 otherwise. The value of L _S is close to zero when there is no key point at that location.

The default thresholds of the parameters of the ORB-SLAM2 and SPSVO are determined based on [Reference Campos, Elvira and Rodriguez30, Reference Xu, Hao and Wang51], as shown in Table 1. The algorithms were run with default thresholds at the beginning and calibrated by comparing with the results of the ground truth values through increasing or decreasing the thresholds. In this work, ±40% variations were made with respect to the default thresholds. Figure 5 shows the comparison of the number of keypoints matched per keyframe with feature points threshold of 1600, as can be observed that the proposed SPSVO outperforms the ORB-SLAM2 in terms of matched feature points (approximately 700 points versus 500 points).

Figure 5. Comparison of number of keypoints matched per keyframe with feature points threshold of 1600.

Table I. The parameters used in ORB-SLAM2 and SPSVO are presented. Default parameters indicate in italics, the optimal parameters indicate in bold. Parameters A, B, C, and D all use default values in order to follow the principle of variable control. A: Scale factor between levels in ORB scale pyramid. B: Number of levels in ORB scale pyramid. C: Initial response threshold of FAST detector. D: Minimum response threshold of FAST detector.

Comparison of the distribution of feature points extracted by the SPSVO algorithm and ORB-SLAM2 on the “colon_reconstruction_dataset” [Reference Zhang, Zhao and Huang38] is shown in Fig. 6; the image resolution is $480\times 640$ . According to the results of Table 1, the upper threshold of feature point extraction is set to 1600 to ensure that both algorithms have the potential to obtain perfect system performance in most scenarios. It can be seen that the SPSVO extracts more effective features than ORB-SLAM2. The large number and even distribution of feature points will provide more scene information, thus improving the accuracy of camera localization. Furthermore, the feature points extracted by SPSVO are evenly distributed and located in textured areas, which is beneficial for subsequent VSLAM tasks such as keypoint matching, camera localization, map construction, and path planning.

Figure 6. Comparison of feature extraction results of the ORB-SLAM2 and SPSVO methods. (a) ORB-SLAM2 and (b) SPSVO.

3.4. Feature matching

The SuperGlue algorithm is commonly applied to simultaneously address feature matching and outlier filtering for real-time pose estimation in indoor and outdoor environments [Reference Jang, Yoon and Kim60–Reference Su and Yu62]. SuperGlue needs to be trained on the true value of the trajectory in the abdominal cavity to achieve an adaptive intra-abdominal environment. A bi-directional brute force matching algorithm is utilized to establish correspondence between features in consecutive frames of an image sequence. Additionally, SPSVO uses the Random Sample Consensus algorithm to remove false matches of feature points for robust geometric estimation, see Algorithm 1. Figure 7 shows the results of the proposed algorithm for stereo matching. The successfully matched feature pairs are connected by lines. It can be seen that the SPSVO can accurately match a large number of key points. Moreover, the SPSVO has good consistency in feature matching between frames, where a feature point can be consistently matched across multiple frames. Consistent matching indicates that the proposed SPSVO can effectively estimate camera position.

Algorithm 1. SuperGlue Stereo Matching

Figure 7. Comparison of feature matching. (a) ORB_SLAM2 and (b) SPSVO.

3.5. Keyframe selection

Keyframe selection plays an important role in reducing computational cost, decreasing redundant information, and improving accuracy of VSLAM [Reference Klein and Murray22, Reference Mur-Artal, Montiel J.M. and Tardos25, Reference Strasdat, Montiel and Davison63]. The general criteria for keyframe selection are (1) distribution of the keyframes should not be too dense or too sparse; (2) the number of keyframes should generate sufficient local map points [Reference Liu, Li and Ishii54]. Unlike other SLAM or VO systems, SPSVO integrates a learning-based matching method that can effectively match frames with large differences in baseline length. Therefore, during feature-matching SPSVO only matches the current frame with keyframes, which can reduce tracking error. The keyframe selection criteria should take into account the movement between frames, information gain, tracking stability, and previous experience. Based on the key frame selection principle [Reference Campos, Elvira and Rodriguez30, Reference Xu, Hao and Wang51], the keyframe selection criteria corresponding to the matching process of SPSVO are defined as:

• • The distance between the current frame and the nearest keyframe ( $L$ ) satisfies the condition of $L\gt D_{f}$ ;

• • The angle between the current frame and the nearest keyframe ( $\theta$ ) satisfies the condition of $\theta \gt \theta _{f}$ ;

• • The number of map points ( $N_{A}$ ) tracked by the current frame satisfies the condition $N_{1}^{u}\lt N_{A}\lt N_{2}^{l}$ ;

• • The number of the map points ( $N_{B}$ ) tracked by the current frame satisfies the condition $N_{B}\lt N_{3}$ ;

• • The number of frames since the last keyframe inserted ( $N_{C}$ ) satisfies the condition of $N_{C}\gt N_{4}$ _.

in which, $D_{f},\theta _{f},N_{1}^{u},N_{2}^{l},N_{3},N_{4}$ are preset thresholds. A frame is selected as a keyframe if it meets any of the above conditions, see Algorithm 2. The proposed keyframe selection criteria consider both image quality and keypoint quality. These can play an important role in filtering useless or incorrect information and avoiding adverse impacts on endoscope localization and scene mapping.

Algorithm 2. The keyframe selection

3.6. Keyframe selection

The Levenberg Marquardt (LM) algorithm is used as the optimization solver in the backend of the proposed SPSVO to construct the Covisibility Graph. For each optimizing iterative loop, when LM optimization converges, both inputs and outputs of the optimization process are set as inputs of the loss function for decoding network training. The optimization variables are keyframes and map points, and the corresponding constraints are the monocular and stereo constraints.

3.6.1 The monocular constraint

If a 3D map point ${}^{w}{P}{_{i}^{}}$ is observed by the left eye camera, the reprojection error $e_{k,i}$ of the i-th point in the k-th frame is defined as

(4) \begin{align} e_{k,i}=\overset{\wedge }{p}_{i}-\pi _{i}\left({w}^{c}{R}{}{}^{w}{P}{_{i}^{}}+{w}^{c}{t}{}\right), \end{align}

where ${}^{w}{P}{_{i}^{}}$ is the i-th point observed by frame k, w is the world coordinate system and c is the camera coordinate system. R and t are the rotation and translation of the camera. $\overset{\wedge }{p}_{i}=(\overset{\wedge }{u}_{i},\overset{\wedge }{v}_{i})$ is the observation data of the map point on the frame, and $\pi _{i}(\cdot )$ is the camera projection model representing coordinates of the 3D map point projection on the left eye image, expressed as

(5) \begin{align} \pi _{i}\left(\left[\begin{array}{l} x_{i}\\ y_{i}\\ z_{i} \end{array}\right]\right)=\left[\begin{array}{l} f_{x}\frac{x_{i}}{z_{i}}+c_{x}\\ f_{y}\frac{y_{i}}{z_{i}}+c_{y} \end{array}\right], \end{align}

where $[\begin{array}{lll} x_{i} & y_{i} & z_{i} \end{array}]^{\mathrm{T}}$ are the world coordinates of point ${}^{w}{P}{_{i}^{}}$ , and $f_{x},f_{y},c_{x},c_{y}$ are the intrinsic parameters of camera.

3.6.2 The stereo constraint

If a 3D map point ${}^{w}{P}{_{j}^{}}$ is observed by both left and right cameras at the same time, the reprojection error is defined as

(6) \begin{align} e_{k,j}=\overset{\wedge }{p}_{j}-\pi _{j}\left({w}^{c}{R}{}{}^{w}{P}{_{j}^{}}+{w}^{c}{t}{}\right), \end{align}

where $\overset{\wedge }{p}_{j}=(\overset{\wedge }{u}_{j},\overset{\wedge }{v}_{j},\overset{\wedge }{r_{j}})$ is the observation data of the map point on the k-th frame of the right image, and $\overset{\wedge }{r}_{j}$ is the horizontal coordinate of the right image. $\pi _{j}(\cdot )$ is the camera projection model representing the 3D map point projection on the stereo image and defined as

(7) \begin{align} \pi _{j}\left(\left[\begin{array}{l} x_{j}\\ y_{j}\\ z_{j} \end{array}\right]\right)=\left[\begin{array}{l} f_{x}\frac{x_{j}}{z_{j}}+c_{x}\\ f_{y}\frac{y}{z_{j}}+c_{y}\\ f_{x}\frac{x_{j}-b}{z_{j}}+c_{x} \end{array}\right], \end{align}

where b represents the baseline of the stereo camera. $[\begin{array}{lll} x_{j} & y_{j} & z_{j} \end{array}]^{\mathrm{T}}$ are the world coordinates of point ${}^{w}{P}{_{j}^{}}$ .

3.6.3 Graph optimization

Assuming that the distribution of key points satisfies a Gaussian distribution [Reference Szeliski64], the final cost function of the proposed SPSVO can be defined as

(8) \begin{align} J=\sum _{k,i}\rho _{k,i}\left(\left(e_{k,i}\right)^{\mathrm{T}}\left(\Sigma _{k,i}\right)^{-1}\left(e_{k,i}\right)\right)+\sum _{k,j}\rho _{k,j}\left(\left(e_{k,j}\right)^{\mathrm{T}}\left(\Sigma _{k,j}\right)^{-1}\left(e_{k,j}\right)\right), \end{align}

where $\rho _{k,i}$ and $\rho _{k,j}$ are robust kernel functions to further reduce the impact of any possible outliers. $(e_{k,i})^{\mathrm{T}}$ and $(e_{k,j})^{\mathrm{T}}$ are the transpose of matrix $e_{k,i}$ and $e_{k,j}$ , respectively. $\Sigma _{k,i}$ and $\Sigma _{k,j}$ are covariance matrices, and $(\Sigma _{k,i})^{-1}, (\Sigma _{k,j})^{-1}$ are the inverse of these covariance matrices, respectively.

4.1. Dataset

The “colon_reconstruction_dataset” contains 16 stereo colonoscope sequences (named as Case 0–Case 15, there are total of 17,362 frames) with corresponding depth and ego-motion ground truth.

4.2. Implementation details

The proposed SPSVO algorithm runs in a C++ environment on a laptop with an i7-10750H CPU and NVIDIA GTX1650Ti. SPSVO uses Nvidia TensorRT Toolkit to accelerate feature extraction and matching networks and uses the LM algorithm of the g2o library for nonlinear squared optimization. OpenCV and the Ceres library are applied to implement computer vision functions and statistical estimation, respectively.

4.3. Results on the colon reconstruction dataset

The performances of the ORB-SLAM2 and SPSVO were tested with the “colon_reconstruction_dataset”; however, the ORB-SLAM2 could only successfully obtain the endoscope trajectories of “Case 0,” and results are shown in Figs. 8, 9 and Table 2. The data sequences of “Case 0” contain 4751 frames of images for each left and right camera and have slower camera motion speed and smaller translation and rotation amplitude compared to “Case 1” to “Case 10.” It can be observed from Fig. 8 that ORB-SLAM2 has larger drift error compared to the proposed SPSVO method.

Figure 8. Comparison between the endoscope trajectories estimated by ORB-SLAM2 and SPSVO, and the true trajectories. (a) ORB-SLAM2 and (b) SPSVO.

Figure 9. Variation of the absolute pose error (APE) between the estimated trajectories of ORB-SLAM2 and SPSVO and the true trajectories. (a) ORB-SLAM2 and (b) SPSVO.

Comparisons between estimated trajectories and true trajectories of the endoscope are shown in Fig. 10. Colored solid lines represent estimated trajectories of the SPSVO. gray dotted lines represent real motion trajectories of the endoscope corresponding to the “colon_reconstruction_dataset” [Reference Zhang, Zhao and Huang38]. Statistics for SPSVO are shown in Table 3. The average measurement error of SPSVO for the 10 cases is between 0.058 and 0.740 mm, with the RMSE between 0.278 and 0.690 mm. This indicates that the proposed SPSVO method can accurately track the true trajectory of the endoscope. Figure 11 shows the variation of the absolute pose error between estimated and true trajectories with respect to time. It can be observed that the proposed SPSVO method has high accuracy and reliability for endoscope trajectory estimation. ORB-SLAM2 cannot extract enough feature points to initialize the viscera scene map, resulting in a loss of feature tracking and failure to construct endoscopic trajectories. Therefore, quantitative results for ORB-SLAM2 on Case1-Case10 are not presented.

Table II. Statistical error analysis of estimated trajectories of ORB-SLAM2 and SPSVO on Case0 sequence (unit: mm).

Figure 10. Comparison between SPSVO-estimated and true trajectories of the endoscope. (a) Case 1, (b) Case 2, (c) Case 3, (d) Case 4, (e) Case 5, (f) Case 6, (g) Case 7, (h) Case 8, (i) Case 9, and (j) Case 10.

Figure 11. Variation of the absolute pose error (APE) between SPSVO-estimated and true trajectories. (a) Case 1, (b) Case 2, (c) Case 3, (d) Case 4, (e) Case 5, (f) Case 6, (g) Case 7, (h) Case 8, (i) Case 9, and (j) Case 10.

Table III. Statistical error analysis of SPSVO-estimated trajectories (unit: mm). RMSE is the Root Mean Square Error, STD stands for the Standard Deviation. SSE refers to the Sum of Squared Errors.

4.4. Computational cost

Computational time of the SPSVO and ORB-SLAM2 on Case 0 sequence for one frame of the “colon_reconstruction_dataset” [Reference Zhang, Zhao and Huang38] is shown in Table 4. For fair comparison, 1000 points were extracted in this experiment, loop closure, relocalization, and visualization parts were disabled. Keypoint detection takes 25 ms for keypoint extraction of one stereo image. 29 ms are required for stereo matching and feature tracking between frames. Pose estimation is fast and only costs 8ms for one image. Therefore, SPSVO can operate at 14 fps; this speed can be further boosted by parallel implementation. It can be observed that the proposed SPSVO method has faster processing speed compared to ORB-SLAM2.

Table IV. Computational cost of ORB-SLAM2 and SPSVO on Case 0.