Problem
Zaragoza is home to one of the most respected SLAM groups in the world: that of Montiel and Tardós, the people behind ORB-SLAM. For some time now, their challenge hasn’t been getting a robot to find its way down a corridor. It’s something much harder: finding the way inside the human body.
The idea is a powerful one. If the endoscope builds a 3D map of the colon as it moves forward, the doctor can see which areas are still left to check, or measure a polyp on the spot.
The catch is that the colon is close to the worst possible setting for SLAM. There’s little light, and what there is bounces around. The walls are smooth and featureless, so there are few points to latch on to. Everything deforms, both during the procedure and from one colonoscopy to the next. And to top it all, water, blur and instruments keep getting in front of the camera.
That’s why the group built CudaSIFT-SLAM, the first GPU-accelerated SLAM able to get through a full colonoscopy in real time. Even so, getting lost in there is inevitable. And every time it gets lost, it starts a new map from scratch, so the same colon ends up split into several disconnected maps.
To stitch them back together, and to correct the drift that builds up when it passes through the same place again (loop closure), the system has to answer one question over and over: have I been here before? That’s place recognition. And in the colon, it’s precisely the piece that struggles most.
Constraints
- Don’t break what works. CudaSIFT-SLAM was already a serious system. The new piece had to slot into place without rebuilding anything around it.
- Real time, no excuses. Every new keyframe triggers a query. If the query is slow, the whole SLAM falls behind.
- From Python to C++. The network was trained in PyTorch and the SLAM is written in C++. It had to move from one world to the other without its answers changing.
Approach
When a new keyframe comes in, the SLAM compares it with every keyframe it has stored, across all its maps, and keeps the most similar ones as candidates. CudaSIFT-SLAM did this with Bag of Words, which describes each image by counting the “visual words” it contains (patterns of keypoints), a bit like summing up a book by the words it uses most. It works very well on a street or in an office. In the colon, with so few points and such unreliable ones, it falls short.
On their own, candidates don’t decide anything. The final say lies with a geometric check, which matches points between the new keyframe, the candidate and its two neighbours, and verifies in 3D that everything fits. If it passes, the maps are merged.
My job was to replace the piece that asks the question. The same group had developed ColonMapper (Morlana et al.), a network trained on real colonoscopies to recognise places in the colon even when the scene changes a lot, for example between two colonoscopies of the same patient. At its core is a global descriptor: the network boils each image down to a single vector, and similar places give similar vectors.
The idea was to take that descriptor out of the network and put it where Bag of Words used to be, inside CudaSIFT-SLAM. Place recognition learned with deep learning, tailored to this problem.
In practice, it came down to three steps:
- Bring the network to C++. I exported it from PyTorch to TorchScript and wrote a library with LibTorch and OpenCV. It takes an image, runs it through the network on the GPU and returns its vector.
- Make it live alongside the SLAM. This was the hard part. CudaSIFT-SLAM already uses CUDA to extract and match its SIFT points, and the network brings its own CUDA work, so the two had to coexist without getting in each other’s way. On top of that, the SLAM splits its work across three threads running in parallel (tracking, mapping and map merging), and the network had to fit into that arrangement without slowing any of them down.
- Change who proposes the candidates. Each keyframe computes its descriptor when it’s created and stores it. When the SLAM runs a query, it compares the new keyframe with all the stored ones and keeps the closest matches. Those candidates then go through the same geometric check as before, both for merging maps and for closing loops.
Result
ColonMapper ended up integrated and running inside the SLAM, in both map merging and loop closure. On the global metrics it performs on par with Bag of Words. Where it differs is in the candidates: ColonMapper’s are more consistent, while Bag of Words jumps from one part of the colon to another.
Here’s a real example from the evaluation: the same query and the top five candidates from each method.
With better candidates, the bottleneck moved. The weak link was no longer the piece that recognises a place it has seen before, but the one that confirms it. The geometric check needs 3D points in common with the candidate, and in the colon the point cloud is so sparse that it rejected many correct ones. The problem shifted from recognition to geometry.
Stack and deliverables
- C++ library Runs ColonMapper on the GPU and returns the descriptor for each image.
- SLAM integration Inside CudaSIFT-SLAM, in map merging and in loop closure.
- Evaluation ColonMapper against Bag of Words, query by query.
C++ · LibTorch (TorchScript) · OpenCV · CUDA · CMake
Images: C3VD (Bobrow et al., Johns Hopkins), CC BY-NC-SA 4.0.
Where this fits
It’s a situation that comes up in any vision system that already works. There’s a new model, trained in Python, that promises to do better, but the system is in C++, runs in real time and can’t be taken offline to be rebuilt.
Bridging that gap is what I do: bringing the model into the system without breaking what already works, measuring it against what was there before and showing with data where the next limit lies. And choosing the right tool for each job: LibTorch when getting the model into C++ is enough, TensorRT when every millisecond of GPU time counts.