METHODS FOR SIMULTANEOUS LOCALIZATION AND MAPPING (SLAM) AND RELATED APPARATUS AND SYSTEMS

• US 20190178654A1
• Filed: 02/04/2019
• Published: 06/13/2019
• Est. Priority Date: 08/04/2016
• Status: Active Grant

• Current Assignee: Reification Inc.
• Original Assignee: Reification Inc.
• Inventors: Hare, Gabriel Archacki
• Granted Patent: US 10,444,021 B2
• Time in Patent Office: Days
• CPC Class Codes: G01C 21/3841 Data obtained from two or more sources, e.g. probe vehicles
• CPC Class Codes: G01C 21/3848 Data obtained from both position sensors and additional sensors
• CPC Class Codes: G06F 16/29 Geographical information databases
• CPC Class Codes: G06T 2207/30244 Camera pose
• CPC Class Codes: G06T 7/70 Determining position or orientation of objects or cameras camera calibration G06T7/80

Abstract

Some embodiments of location estimation methods may (1) facilitate the task of efficiently finding the location of a mobile platform in scenarios in which the uncertainties associated with the coordinates of the map features are anisotropic and/or non-proportional, and/or (2) facilitate decoupling of location estimation from feature estimation. Some embodiments of feature estimation methods may (1) facilitate the combining of environmental descriptions provided by two or more mobile platforms, and/or (2) facilitate decoupling of a data aggregation from feature re-estimation.

Claims

1. A method of estimating a location of a mobile device in a two-dimensional (2D) or three-dimensional (3D) space, the method comprising:

obtaining a first map comprising coordinates of a plurality of first features within a first coordinate space and respective first regions of uncertainty of the coordinates of each of the first features, wherein the first regions of uncertainty include at least two regions with non-proportional dimensions;

obtaining a second map comprising coordinates of a plurality of second features within a second coordinate space and respective second regions of uncertainty for the coordinates of each of the second features;

determining a plurality of feature pairs, wherein each feature pair includes a first feature of the first map and a second feature of the second map;

performing one or more iterations of an iterative process, including;

(a) determining third regions of uncertainty of the coordinates of the respective first features,

(b) determining a potential transformation between the first coordinate space and the second coordinate space,

(c) determining probabilities of the feature pairs based, at least in part, on the third regions of uncertainty, wherein the probability of each feature pair is a probability that the coordinates of the first feature of the feature pair represent a measurement of the second feature of the feature pair obtained from a potential location of the mobile device corresponding to the potential transformation,

(d) determining a value representative of a statistical optimality of the potential transformation by evaluating an objective function, wherein the objective function aggregates the probabilities of the feature pairs,

(e) determining whether the value of the objective function is approaching a local extreme value of the objective function, and

(f) terminating the iterative process if the value of the objective function has reached the local extreme value, otherwise performing another iteration of the iterative process,

wherein for at least one of the iterations, the third regions of uncertainty are determined based, at least in part, on the first regions of uncertainty, and the probabilities of the feature pairs are determined based, at least in part, on the first regions of uncertainty with the non-proportional dimensions; and

estimating the location of the mobile device based on the potential transformation from a final iteration of the iterative process.

2. The method of claim 1, wherein at least two of the first regions of uncertainty have anisotropic dimensions along at least two orthogonal axes of the first coordinate space.

3. The method of claim 2, wherein:

the second regions of uncertainty include at least two regions with non-proportional dimensions,

the probabilities of the feature pairs are further determined based, at least in part, on the second regions of uncertainty, and

for the at least one iteration, the probabilities of the feature pairs are determined based, at least in part, on second regions of uncertainty with the non-proportional dimensions.

5. The method of claim 3, wherein:

at least two of the second regions of uncertainty have anisotropic dimensions along at least two orthogonal axes of the second coordinate space, and

the first and second regions of uncertainty are three-dimensional.

9. The method of claim 1, wherein for a first iteration of the iterative process:

the potential transformation is determined based, at least in part, on the third regions of uncertainty of the coordinates of the first features,

the third regions of uncertainty are mutually proportional, and

dimensions of each of the proportional regions of uncertainty are isotropic.

13. The method of claim 1, wherein:

for a first iteration of the iterative process, the third regions of uncertainty are mutually proportional,.

for a second iteration of the iterative process logically subsequent to the first iteration, for each of the first features, at least one dimension of the corresponding third region of uncertainty is determined based, at least in part, on a dimension of the corresponding first region of uncertainty and a dimension of the corresponding proportional region of uncertainty, and

for a third iteration of the iterative process logically subsequent to the first and second iterations, the third regions of uncertainty are the first regions of uncertainty.

18. The method of claim 1, wherein:

determining the probabilities of the feature pairs comprises determining respective weights of the feature pairs, and

for each feature pair, the weight of the feature pair represents a confidence that the features of the feature pair are correctly paired and stationary.

20. The method of claim 18, wherein:

determining the weights of the feature pairs comprises determining a respective pull of each feature pair, and

for each feature pair, the pull is determined based on a residual of the feature pair and an uncertainty covariance of (1) the third region of uncertainty of the first feature of the feature pair and (2) the second region of uncertainty of the second feature of the feature pair.

27. The method of claim 20, wherein:

determining the weights of the feature pairs comprises determining a mean and a covariance of a distribution of the pulls of the feature pairs,

the covariance of the distribution of pulls comprises a stabilized covariance of the distribution of pulls, and

the respective weight of each feature pair is determined based on a probability of the pull of the feature pair according to the distribution of the pulls of the feature pairs.

30. The method of claim 18, wherein:

determining the probabilities of the feature pairs further comprises determining squared sigma distances of the respective feature pairs, and

for each feature pair, the squared sigma distance (SSD) is a distance in a common coordinate space between (1) the third region of uncertainty associated with the coordinates first feature of the feature pair, and (2) the second region of uncertainty associated with the coordinates of the second feature of the feature pair.

32. The method of claim 30, wherein for each feature pair, determining the SSD comprises:

transforming the coordinates of the first feature in the first coordinate space to coordinates of the first feature in the second coordinate space based on the potential transformation, wherein the common coordinate space is the second coordinate space;

determining a residual of the feature pair;

determining a sum of an inverse of an uncertainty covariance of the first feature and an inverse of an uncertainty covariance of the second feature; and

determining a product of the residual, the sum, and the residual,

wherein the SSD comprises the product.

33. The method of claim 30, wherein the first and second coordinate spaces are 3D spaces, and wherein for each feature pair, determining the SSD comprises:

mapping the third region of uncertainty of the coordinates of the first feature in the first 3D coordinate space and the second region of uncertainty of the coordinates of the second feature in the second 3D coordinate space to a region in a 6D coordinate space;

mapping the potential transformation to a 3D subspace in the 6D coordinate space; and

determining a sigma distance between the region in the 6D coordinate space representing the feature pair and a point on the 3D subspace along a vector orthogonal to the 3D subspace,

wherein the SSD comprises a square of the sigma distance in the 6D space.

36. The method of claim 18, further comprising:

after terminating the iterative process, determining whether a first feature pair included in the plurality of feature pairs is outlying or inlying.

37. The method of claim 36, wherein determining whether the first feature pair is outlying or inlying comprises:

comparing the weight of the first feature pair to a threshold weight;

if the weight of the first feature pair is less than the threshold weight, classifying the first feature pair as outlying; and

otherwise, classifying the first feature pair as inlying.

38. The method of claim 37, wherein the first feature pair is outlying, and wherein the method further comprises determining whether the outlying first feature pair represents an incorrect pairing of features or a non-stationary feature.

39. The method of claim 38, wherein the threshold weight is a first threshold weight, and wherein determining whether the outlying first feature pair represents an incorrect pairing of features or a non-stationary feature comprises:

comparing the weight of the outlying first feature pair to a second threshold weight less than the first threshold weight;

if the weight of the outlying first feature pair is less than the second threshold weight, determining that the outlying first feature pair represents an incorrect pairing of features; and

otherwise, determining that the outlying first feature pair represents a non-stationary feature.

40. The method of claim 38, wherein determining whether the outlying first feature pair represents an incorrect pairing of features or a non-stationary feature comprises:

transforming the coordinates of the first feature in the first coordinate space to coordinates of the first feature in the second coordinate space based on the potential transformation from the final iteration;

determining a distance between the coordinates of the first feature in the second coordinate space and the coordinates of the second feature in the second coordinate space;

comparing the distance between the coordinates to a threshold distance;

if the distance between the coordinates is less than the threshold distance, determining that the outlying first feature pair represents a non-stationary feature; and

otherwise, determining that the outlying first feature pair represents an incorrect pairing of features.

4. (canceled)

6-8. -8. (canceled)

10-12. -12. (canceled)

14-17. -17. (canceled)

19. (canceled)

21-26. -26. (canceled)

28-29. -29. (canceled)

31. (canceled)

34-35. -35. (canceled)

41. A method of estimating a location of a mobile device in a two-dimensional (2D) or three-dimensional (3D) space, the method comprising:

obtaining a first map comprising coordinates of a plurality of first features within a first coordinate space and first data characterizing uncertainties associated with the coordinates of the first features;

obtaining a second map comprising coordinates of a plurality of second features within a second coordinate space and first data characterizing uncertainties associated with the coordinates of the second features;

determining a plurality of feature pairs, wherein each feature pair includes a first feature of the first map and a second feature of the second map;

performing one or more iterations of an iterative process, including;

(a) determining third data characterizing uncertainties associated with the coordinates of the first features,

(b) determining a potential transformation between the first coordinate space and the second coordinate space,

(c) determining probabilities of the feature pairs based, at least in part, on the third regions of uncertainty, wherein

the probability of each feature pair is a probability that the coordinates of the first feature of the feature pair represent a measurement of the second feature of the feature pair obtained from a potential location of the mobile device corresponding to the potential transformation,

the probability of each feature pair is determined based, at least in part, on a weight associated with the feature pair,

the weight associated with each feature pair is determined based, at least in part, on relationship between a pull of the feature pair and a distribution of pulls of the plurality of feature pairs, and

the pull of each feature pair comprises a product of a residual of the feature pair and an inverse square root of an uncertainty covariance of the feature pair,

(d) determining a value representative of a statistical optimality of the potential transformation by evaluating an objective function, wherein the objective function aggregates the probabilities of the feature pairs,

(e) determining whether the value of the objective function is approaching a local extreme value of the objective function, and

(f) terminating the iterative process if a value of a stabilized covariance of the pull distribution is less than a threshold value on each axis of the pull distribution, otherwise performing another iteration of the iterative process; and

estimating the location of the mobile device based on the potential transformation from a final iteration of the iterative process.

42. A mapping method comprising:

obtaining first feature data comprising first estimated coordinates of a feature in a coordinate space of a map of an environment;

obtaining virtual object data indicating (1) an anchor relationship between a virtual object and the feature, and (2) a displacement of the virtual object relative to the feature;

determining first coordinates of the virtual object based on the first estimated coordinates of the feature and the displacement of the virtual object relative to the feature;

after re-estimation of the coordinates of the feature, obtaining second feature data comprising second estimated coordinates of the feature, wherein there is a displacement between the first and second estimated coordinates of the feature; and

determining second coordinates of the virtual object based on the second estimated coordinates of the feature and the displacement of the virtual object relative to the feature.

43-47. -47. (canceled)

48. A mapping method comprising:

obtaining first feature data comprising first estimated coordinates of a plurality of features in a coordinate space of a first map of an environment;

obtaining first uncertainty data representing (1) for each of the features, a first distribution of individual uncertainty of the first estimated coordinates of the feature, and (2) for each pair of the features, a first distribution of correlated uncertainty of the first estimated coordinates of the pair of features;

obtaining first lowered position data representing a product of the first uncertainty data and the first feature data;

performing an aggregation step, including;

obtaining second uncertainty data representing (1) for each of the features, a second distribution of individual uncertainty of second estimated coordinates of the feature, and (2) for each pair of the features, a second distribution of correlated uncertainty of the second estimated coordinates of the pair of features,

aggregating the first uncertainty data and the second uncertainty data to generate third uncertainty data representing (1) for each of the features, a third distribution of individual uncertainty of third estimated coordinates of the feature, and (2) for each pair of the features, a third distribution of correlated uncertainty of the third estimated coordinates of the pair of features,

obtaining second lowered position data representing a product of the second uncertainty data and second feature data comprising the second estimated coordinates of the features, and

aggregating the first lowered position data and the second lowered position data to generate third lowered position data representing a product of the third uncertainty data and third feature data comprising the third estimated coordinates of the features; and

performing a feature estimation step, comprising;

for each of the features, determining a mean of the third distribution of individual uncertainty of the respective feature based on (1) the third distribution of individual uncertainty of the respective feature, (2) the third distributions of correlated uncertainty of each pair of features that includes the respective feature, and (3) the third lowered position data,

wherein for each of the features, the third estimated coordinates of the feature comprise the mean of the third distribution of individual uncertainty of the feature.

49-55. -55. (canceled)