Real-time monocular visual odometry
View Patent ↗Systems and methods are disclosed for multithreaded visual odometry by acquired with a single camera on-board a vehicle; using 2D-3D correspondences for continuous pose estimation; and combining the pose estimation with 2D-2D epipolar search to replenish 3D points.
1. A method for multithreaded visual odometry, comprising acquiring images with a single camera on-board a vehicle;
using 2D-3D correspondences for continuous pose estimation with a pose local bundle adjustment (LBA) system including a feature matching engine using three or more parallel CPU threads with pose estimation across all threads, followed by local bundle adjustment in the primary thread;
using a local bundle cache including a frame cache and a match cache, wherein the frame cache stores feature locations, descriptors and camera poses from most recent frames and images, wherein the match cache comprises a list of tables, one element corresponding to each frame with a key into the table being an identity of a 3D point visible in the frame and stored entries are identities of corresponding 2D features in the frames;
combining the pose estimation with 2D-2D epipolar search to replenish 3D points; and
generating structure-from-motion (SFM).
2. The method of claim 1 , comprising using the visual odometry for autonomous driving applications.
3. The method of claim 1 , comprising pose-guided matching to provide fast 3D-2D correspondences.
4. The method of claim 1 , comprising performing epipolar constrained search to produce per-frame 2D-2D correspondences.
5. The method of claim 4 , comprising constructing long tracks, and inserting new 3D points guaranteed to be accurate.
6. The method of claim 1 , comprising validating each 3D point against all frames in real-time.
7. The method of claim 1 , comprising performing local bundle adjustment to refine cameras and 3D points.
8. The method of claim 7 , comprising collecting and refining 3D points from an epipolar thread.
9. The method of claim 1 , comprising inserting new 3D points in a main thread.
10. The method of claim 1 , comprising collecting and refinding mechanism allows bundle adjustment using long tracks.
11. The method of claim 1 , comprising performing real-time global bundle adjustment in a thread-safe architecture with real-time pose estimation.
12. The method of claim 1 , comprising handling failures due to tracking or scale drift.
13. The method of claim 1 , comprising using a 1-point RANSAC between 3D points reconstructed before and after a tracking failure.
14. The method of claim 1 , comprising scale correcting by combining scale estimates from 3D points and planar homography mappings.
15. The method of claim 1 , comprising planar homography between a set of road points in two frames, using a 4-point RANSAC where hypotheses are generated by linear estimation of algebraic error and followed by Levenberg-Marquardt based nonlinear optimization of the optimal symmetric reprojection error over a largest consensus set:
min
H
∑
i
x
i
′
-
Hx
i
2
+
x
i
-
H
-
1
x
i
′
2
,
where H is a planar homography that maps homogeneous inlier point set x in a previous frame to a corresponding set x′ in a current frame, where a form of a homography H is:
H
=
R
+
1
h
2
tn
T
,
where (R, t) is a relative pose, n is a unit normal of a proposed ground plane and h 2 is a distance of a plane from a camera and height h 2 is recovered from H using a singular value decomposition of H T H and a scale factor computed as a ratio
s
2
=
h
0
h
2
.
16. A system for multithreaded visual odometry, comprising:
a single camera on-board a vehicle; and
a multi-threaded processor coupled to the camera, the multi-threaded processor using 2D-3D correspondences for continuous pose estimation with a pose local bundle adjustment (LBA) system including a feature matching engine using three or more parallel CPU threads with pose estimation across all threads, followed by local bundle adjustment in the primary thread;
a local bundle cache including a frame cache and a match cache, wherein the frame cache stores feature locations, descriptors and camera poses from most recent frames and images, wherein the match cache comprises a list of tables, one element corresponding to each frame with a key into the table being an identity of a 3D point visible in the frame and stored entries are identities of corresponding 2D features in the frames;
combining the pose estimation with 2D-2D epipolar search to replenish 3D points.
17. The system of claim 16 , comprising code for autonomous driving applications using the visual odometry.
18. The system of claim 16 , comprising code for pose-guided matching to provide fast 3D-2D correspondences.
19. The system of claim 16 , comprising code to perform epipolar constrained search to produce per-frame 2D-2D correspondences.
20. The system of claim 19 , comprising code for constructing long tracks, and inserting new 3D points guaranteed to be accurate.