MAPPING AND TRACKING SYSTEM

• US 20150304634A1
• Filed: 08/03/2012
• Published: 10/22/2015
• Est. Priority Date: 08/04/2011
• Status: Active Grant

• Current Assignee: TRX Systems, Inc.
• Original Assignee: TRX Systems, Inc.
• Inventors: Karvounis, John George
• Granted Patent: US 10,027,952 B2
• Time in Patent Office: Days
• US Class Current: 1/1
• CPC Class Codes: G06F 18/21355 nonlinear criteria, e.g. embedding a manifold in a Euclidean space
• CPC Class Codes: G06T 2200/28 involving image processing hardware
• CPC Class Codes: G06T 2207/10021 Stereoscopic video; Stereoscopic image sequence
• CPC Class Codes: G06T 2207/20021 Dividing image into blocks, subimages or windows
• CPC Class Codes: G06T 2207/20088 Trinocular vision calculations; trifocal tensor
• CPC Class Codes: G06T 7/246 using feature-based methods, e.g. the tracking of corners or segments
• CPC Class Codes: G06T 7/277 involving stochastic approaches, e.g. using Kalman filters
• CPC Class Codes: G06T 7/85 Stereo camera calibration
• CPC Class Codes: G06V 10/462 Salient features, e.g. scale invariant feature transforms [SIFT]
• CPC Class Codes: G06V 20/10 Terrestrial scenes scenes under surveillance with static cameras G06V20/52; scenes perceived from the exterior of a vehicle G06V20/56; scenes perceived from the interior of a vehicle G06V20/59
• CPC Class Codes: G06V 20/64 Three-dimensional objects
• CPC Class Codes: G06V 2201/11 Technique with transformation invariance effect
• CPC Class Codes: H04N 13/122 Improving the 3D impression of stereoscopic images by modifying image signal contents, e.g. by filtering or adding monoscopic depth cues H04N13/128 takes precedence
• CPC Class Codes: H04N 13/239 using two 2D image sensors having a relative position equal to or related to the interocular distance H04N13/243 takes precedence
• CPC Class Codes: H04N 13/246 Calibration of cameras
• CPC Class Codes: H04N 13/282 for generating image signals corresponding to three or more geometrical viewpoints, e.g. multi-view systems
• CPC Class Codes: H04N 2013/0092 Image segmentation from stereoscopic image signals
• CPC Class Codes: H04N 5/2224 related to virtual studio applications

Abstract

LK-SURF, Robust Kalman Filter, HAR-SLAM, and Landmark Promotion SLAM methods are disclosed. LK-SURF is an image processing technique that combines Lucas-Kanade feature tracking with Speeded-Up Robust Features to perform spatial and temporal tracking using stereo images to produce 3D features can be tracked and identified. The Robust Kalman Filter is an extension of the Kalman Filter algorithm that improves the ability to remove erroneous observations using Principal Component Analysis and the X84 outlier rejection rule. Hierarchical Active Ripple SLAM is a new SLAM architecture that breaks the traditional state space of SLAM into a chain of smaller state spaces, allowing multiple tracked objects, multiple sensors, and multiple updates to occur in linear time with linear storage with respect to the number of tracked objects, landmarks, and estimated object locations. In Landmark Promotion SLAM, only reliable mapped landmarks are promoted through various layers of SLAM to generate larger maps.

Claims

1. A method for real-time tracking of features, the method comprising:

acquiring a collection of images and one or more subsequent collections of images to create a three-dimensional space, wherein the collection of images and the one or more subsequent collections of images are substantially simultaneously captured from multiple sources;

identifying a candidate feature in the three-dimensional space for tracking; and

tracking the candidate feature in the three-dimensional space over time.

2. The method of claim 1, wherein the multiple sources comprise left and right cameras.

3. The method of claim 2, wherein the collection of images and the one or more subsequent collections of images are captured utilizing stereo imaging by the left and right cameras.

4. The method of claim 1, wherein identifying the candidate feature comprises selecting features from each image in the collection of images and correlating the features using feature descriptors.

5. The method of claim 1, wherein identifying the candidate feature comprises processing the collection of images through a speeded-up robust features algorithm.

6. The method of claim 5 further comprising validating the candidate feature using epipolar geometry.

7. The method of claim 3, wherein identifying the candidate feature comprises:

extracting a left feature from an acquired left image captured by the left camera;

extracting a right feature from an acquired right image captured by the right camera, the right image being captured at substantially the same time as the left image;

refining a location of the left feature to a sub-pixel accuracy;

refining a location of the right feature to a sub-pixel accuracy;

matching the left and right features to create a stereo feature;

filtering the stereo feature;

validating the stereo feature; and

returning the validated stereo feature as the candidate feature.

8. The method of claim 7, wherein the extracting of the left and right features utilizes a speeded-up robust features algorithm, and wherein the extracting of the left and right features utilizes a speeded-up robust features algorithm.

9. The method of claim 7, wherein filtering the stereo feature comprises utilizing a matching strength criteria.

10. The method of claim 7, wherein validating the stereo feature comprises utilizing epipolar geometry.

11. The method of claim 1, wherein tracking the candidate feature comprises processing the candidate feature through a Lucas-Kanade feature tracker.

12. The method of claim 3, wherein tracking the candidate feature comprises:

extracting a left feature from an acquired left image captured by the left camera;

extracting a right feature from an acquired right image captured by the right camera, the right image being captured at substantially the same time as the left image;

initializing a left Lucas-Kanade tracker with the left feature;

initializing a right Lucas-Kanade tracker with the right feature;

tracking the left feature on a subsequently acquired left image, the subsequently acquired left image being an image subsequently captured by the left camera;

tracking the right feature on a subsequently acquired right image, the subsequently acquired right image being an image subsequently captured by the right camera;

matching the tracked left and right features to create a tracked stereo feature;

validating the tracked stereo feature; and

returning the validated stereo feature as the tracked candidate feature.

13. The method of claim 12, wherein matching the tracked left and right features comprises utilizing a speeded-up robust features algorithm.

14. The method of claim 12, wherein validating the tracked stereo feature comprises utilizing epipolar geometry.

15. The method of claim 7, wherein identifying a candidate feature further comprises creating a list of candidate features by:

extracting a subsequent left feature from a subsequently acquired image by the left camera;

extracting a subsequent right feature from a subsequently acquired right image captured by the right camera;

refining a location of the subsequent left feature to a sub-pixel accuracy;

refining a location of the subsequent right feature to a sub-pixel accuracy;

matching the subsequent left and right features to create a subsequent stereo feature;

filtering the subsequent stereo feature;

validating the subsequent stereo feature;

returning the validated subsequent stereo feature as a subsequent candidate feature; and

adding the subsequent candidate feature to the list of candidate features.

16. The method of claim 1, wherein the three dimensional space includes a plurality of corners, further comprising discovering a most prominent corner bounded by the feature by utilizing a sub-pixel corner finding routine.

17. The method of claim 16, wherein the sub-pixel corner finding routine is a Lucas-Kanade optic flow routine.

18. A method for filtering outlier features from captured high-dimensional observations, the method comprising:

mapping a high-dimensional feature into a one-dimensional space; and

applying an outlier rejection rule in the one-dimensional space to remove the mapped feature.

19. The method of claim 18, wherein mapping the high-dimensional feature into a one-dimensional space comprises applying a weighting metric to the high-dimensional observation, the weighting metric being derived from an observation covariance and an expectation covariance.

20. The method of claim 18, wherein the outlier rule comprises using a median of a mapped feature and a median absolute deviation of the mapped feature to determine whether the mapped feature disagrees with a general affine transformation of the observation.

21. The method of claim 19 further comprising:

determining an observation covariance associated with the high-dimensional observation;

predicting an observation and covariance, the predicted observation being related to the high-dimensional observation, the predicted covariance being associated with the predicted observation;

extracting the high-dimensional feature from the high-dimensional observation;

determining a first covariance for the high-dimensional feature, the determination being based on the observation covariance;

determining a second covariance for the high-dimensional feature, the determination being based on the predicted covariance; and

utilizing a weighting metric in the outlier rejection rule, the weighting metric being derived from the first covariance and the second covariance.

22. The method of claim 21 further comprising determining first and second state matrices, the first state matrix being associated with the high-dimensional observation and the second state matrix being associated with the predicted observation, and wherein the first and second matrices are Jacobian matrices.

23. A method for managing objects in a multi-dimensional space when the objects are no longer visible in a collection of images captured from the multi-dimensional space over time, the method comprising:

identifying one or more objects visible in a first image among the collection of images;

adding the identified objects to a first list of tracked identified objects;

tracking the identified objects from the first list of objects in one or more subsequent images among the collection of images, the one or more subsequent images being captured after capturing the first image;

determining whether the tracked identified objects are absent in the one or more subsequent images; and

removing, based on the determination, the tracked identified objects from the first list.

24. The method of claim 23 further comprising:

filtering outlier objects in the first image to create a second list comprising the outlier objects; and

tracking the outlier objects in the second list independently of the identified objects in the first list.

25. The method of claim 23, wherein the first list represents a state space of the multi-dimensional space, the state space comprising object identifiers, a state vector, and a covariance matrix.

26. The method of claim 23 further comprising:

determining whether an object is visible in the one or more subsequent images;

determining whether the object is absent in the first image; and

adding the object to the first list.

27. The method of claim 23 further comprising correcting locations of tracked identified objects.

28. The method of claim 27, wherein correcting locations of tracked identified objects comprise processing the tracked identified objects through an extended Kalman filter for simultaneous localization and mapping.

29. The method of claim 23, wherein tracking the identified objects from the first list comprises determining whether a tracked identified object from the first list is an unreliable object and removing the tracked identified object from the first list based on the determination.

30. The method of claim 23 further comprising saving the removed object in a second list.

31. A method for creating a connected graph, the method comprises:

associating a first pose of a first sensor with landmarks detected in a first image from a multi-dimensional space acquired by the first sensor;

adding the first pose and the landmarks to a state space;

determining that the landmarks are absent from a second image from the multi-dimensional space acquired by the first sensor in a second pose;

removing the landmarks from the state space based on the determination;

correlating the first and second poses; and

correlating the landmarks with the second pose.

32. The method of claim 31, wherein the state space comprises a state vector for each pose, a covariance matrix for each pose, a state vector for each landmark, a covariance matrix for each landmark, and a cross-covariance matrix for each correlation between a landmark and the second pose.

33. The method of claim 31, wherein correlating the first and second poses comprises linking the first pose to the second pose by utilizing a first state associated with the first pose, a first covariance matrix associated with the first pose, a second state associated with the second pose, a second covariance matrix associated with the second pose, and a cross-covariance matrix associated with the first and second poses.

34. The method of claim 31, wherein removing the landmarks comprises maintaining a list that includes identifications of the removed landmarks, each removed landmark mean, each removed landmark covariance, the first pose mean, and the first pose covariance.

35. The method of claim 31 further comprising recovering a removed landmark in the second pose by correlating the removed landmark with the first pose by utilizing a covariance matrix associated with the removed landmark and a covariance matrix associated with the first pose.

36. The method of claim 31 further comprising creating a cross-covariance matrix between the first and second poses by utilizing landmarks visible in the first and the second poses.

37. The method of claim 31, wherein the first pose is associated with a first state and the second pose is associated with a second state, and wherein the second state is expanded to comprise the first state.

38. The method of claim 31 further comprising:

updating the first pose from an update applied to the second pose by propagating the update of the second pose to the first pose; and

propagating the update of the first pose from the first pose to landmarks correlated to the first pose.

39. The method of claim 38, wherein propagating the update to the first pose comprises updating a covariance matrix associated with the first pose and a cross-covariance matrix associated with the first and second poses.

40. The method of claim 38, wherein propagating the update to the correlated landmarks comprises updating, for each correlated landmark, a covariance matrix associated with the correlated landmark and a cross-covariance matrix associated with the first pose and the correlated landmark.

41. The method of claim 31 further comprising:

determining whether a landmark is re-observed;

deleting a link, a state, and covariance matrix associated with a past observation of the landmark based on the determination; and

updating each cross covariance matrix associated with the landmark.

42. The method of claim 31 further comprising:

utilizing a second sensor to acquire additional images from the multi-dimensional space; and

creating a coordinate state to unite coordinates of the first and second sensors.

43. The method of claim 42, wherein the coordinates of the first and second sensors are united by translating and rotating the coordinates of each sensor into the coordinate state.

44. The method of claim 42, wherein the first and second sensors are three-dimensional sensors, and wherein the coordinates of each sensor are represented in the coordinate state as a four-dimensional quaternion comprising a three-dimensional origin and three orthogonal rotations.

45. The method of claim 42 further comprising determining whether the first and second sensors observed a same landmark and updating the coordinate state.

46. The method of claim 42, wherein an update to a landmark observed by a first sensor is propagated to a landmark observed by the second sensor.

47. The method of claim 32 further comprising matching landmarks based on a nearest neighbor ratio.

48. The method of claim 47 wherein matching the landmarks comprises applying homography calculation to two groups of landmarks to determine spatial fitness, wherein each group comprises at least four landmarks.

49. The method of claim 47, wherein matching the landmarks comprises limiting the number of compared landmarks based on covariances of the landmarks.

50. A method for promoting high-dimensional objects through various layers of a simultaneous location and mapping system, the method comprising:

determining at a first level of the system a first set of reliable high-dimensional objects associated with a first mapped region; and

promoting the first set of reliable high-dimensional objects to a second level of the system, wherein the second level combines the first set of reliable high-dimensional objects with a second set of reliable high-dimensional objects associated with a second mapped region to produce a map corresponding to a region larger than each of the first and second mapped regions.

51. A system for creating a map of a high-dimensional area and locating the system on the map, the system comprising:

a processor; and

a memory communicatively coupled to the processor when the system is operational, the memory bearing processor instructions that, when executed on the processor, cause the system to at least;

create a state associated with stereo images of the area to be mapped;

process a first stereo image to identify a visual feature within the area, wherein the first stereo image is associated with a first pose of the system;

process a second stereo image to determine whether the visual feature is absent from the area in the second stereo image, wherein the second stereo image is associated with a second pose of the system;

remove, based on the determination, the first pose and the visual feature from the state and send the first pose and the visual feature to a mapping module configured to maintain a structure that correlates poses and visual features; and

create a map of the area and at least one location of the system on the map.

52. The system of claim 51, wherein processing the first stereo image comprises applying Lucas-Kanade and speeded-up robust features algorithm to the first stereo image.

53. The system of claim 51, wherein the visual feature is associated with the location, a descriptor, a covariance matrix, a cross-covariance matrix, and the first pose.

54. The system of claim 51, wherein the structure further correlates the poses and the visual features to poses and visual features associated with a second system.

55. A computer readable storage medium having stored thereon instructions that, when executed by a processor, cause the processor to:

receive a visual feature, wherein the visual feature is identified in a three-dimensional image and is associated with a pose, wherein the pose is associated with a first device that captured the three-dimensional image;

compare the visual feature to visual features comprised in a database to find a match;

transmit the visual feature and the match to a module configured to maintain a map that correlates poses and visual features;

determine whether the visual feature is over a quality threshold; and

update the visual feature with information received from a second device capturing three-dimensional images.

56. The computer readable storage medium of claim 55, wherein the visual feature is linked between the first and second devices.

57. The computer readable storage medium of claim 56, wherein the visual feature is linked through the map maintained by the module.

58. The computer readable storage medium for claim 55, wherein the instructions to determine whether the visual feature is over a quality threshold comprise instructions to transmit the visual feature to the second device based on the determination.

59. The computer readable storage medium for claim 55, wherein the quality threshold is based on a cross covariance matrix between the visual feature and the pose in combination with the inverse of a covariance matrix of the visual feature.

60. The computer readable storage medium for claim 55 further comprising instructions that, when executed by the processor, cause the processor to match visual features between the first and second devices by comparing a first array of visual features with a second array of visual features, wherein the first array is associated with the first device and the second array is associated with the second device.

61. The computer readable storage medium for claim 55 further comprising instructions that, when executed by the processor, cause the processor to:

transmit visual features associated with the second device to the first device; and

instruct the first device to match visual features associated with the first device with visual features associated with the second device.