Method and Apparatus

Abstract

A computer-implemented method of localizing a radar sensor, the method comprising: obtaining a first radar scan of a first environment of the radar sensor, wherein the first radar scan comprises a set of power-range spectra, including a first power-range spectrum; extracting a first set of landmarks, including a first landmark, from the first radar scan, wherein the first landmark is defined by a range and an azimuth; computing a respective first set of descriptors, including a first descriptor, of the first set of landmarks, wherein the first descriptor defines the first landmark by respective relative ranges and azimuths in relation to one or more landmarks included in the first set of landmarks; accessing one or more reference sets of landmarks of respective environments and computing respective reference sets of descriptors of the reference sets of landmarks; matching the first set of descriptors to a corresponding first reference set of descriptors; and localizing a first location of the radar sensor using a first result of the matching.

Claims

1. A computer-implemented method of localizing a radar sensor, the method comprising: obtaining a first radar scan of a first environment of the radar sensor, wherein the first radar scan comprises a set of power-range spectra, including a first power-range spectrum; extracting a first set of landmarks, including a first landmark, from the first radar scan, wherein the first landmark is defined by a range and an azimuth; computing a respective first set of descriptors, including a first descriptor, of the first set of landmarks, wherein the first descriptor defines the first landmark by respective relative ranges and azimuths in relation to at least one landmark included in the first set of landmarks; accessing at least one reference set of landmarks of respective environments and computing at least one respective reference set of descriptors, where each of the at least one reference set of descriptors corresponds to one of the at least one reference set of landmarks; matching the first set of descriptors to a corresponding first reference set of descriptors from the at least one respective reference set of descriptors; and localizing a first location of the radar sensor using a first result of the matching; wherein the method further comprises: projecting the first descriptor to a first projected descriptor, wherein the first descriptor has a dimensionality N and the first projected descriptor has a dimensionality M, wherein MN; and wherein matching the first set of descriptors to the corresponding first reference set of descriptors comprises matching a first set of projected descriptors, including the first projected descriptor, to the corresponding first reference set of descriptors of the plurality of sets of descriptors.

2. The method of claim 1, wherein the dimensionality N is a matrix dimension having a number of rows corresponding to a number of points in a scan and a number of columns corresponding to a size of a point descriptor.

3. The method of claim 2, wherein the projecting the first descriptor to a first projected descriptor comprises: obtaining a vector of a mean of each element of the first descriptor; obtaining a shifted first descriptor by subtracting the mean from each element in the first descriptor; calculating a covariance matrix of the shifted first descriptor; generating an eigenvector of the covariance matrix; ordering eigenvalues from the eigenvector in descending order; and selecting a subset of largest eigenvalues as the first projected descriptor.

4. The method of claim 3, wherein the subset includes M eigenvalues.

5. The method of claim 4, wherein M is 1, and the selected eigenvalue is the largest eigenvalue.

6. The method according to claim 1, comprising: obtaining a second radar scan of the first environment of the radar sensor; extracting a second set of landmarks, including a second landmark, from the second radar scan; computing a respective second set of descriptors, including a second descriptor, of the second set of landmarks; matching the second set of descriptors to a corresponding second reference set of descriptors; localizing a second location of the radar sensor using a second result, where the second result is from the matching of the second set of descriptors to the corresponding second reference set of descriptors; and calculating a motion of the radar sensor using the second location and the first location.

7. The method according to claim 1, wherein matching the first set of descriptors to the corresponding first reference set of descriptors comprises projecting the first descriptor to a first projected descriptor.

8. The method according to claim 1, wherein matching the first set of descriptors to the corresponding first reference set of descriptors comprises projecting the first descriptor to a first Eigen projected descriptor, wherein the first descriptor has a dimensionality n.sub.p>1 and the first projected Eigen descriptor has a dimensionality n.sub.p=1.

9. The method according to claim 8, wherein matching the first set of descriptors to the corresponding first reference set of descriptors comprises comparing a first set of Eigen projected descriptors, including the first Eigen projected descriptor, with a corresponding first reference set of Eigen projected descriptors.

10. The method according to claim 9, wherein comparing the first set of Eigen projected descriptors, including the first Eigen projected descriptor, with the corresponding first reference set of Eigen projected descriptors comprises identifying M closest Eigen projected descriptors of the corresponding first reference set of Eigen projected descriptors.

11. The method according to claim 1, wherein matching the first set of descriptors to the corresponding first reference set of descriptors comprises identifying M closest descriptors of the corresponding first reference set of descriptors and finding a single closest descriptor from amongst the M closest descriptors.

12. The method according to claim 11, wherein the method comprises: summing a first absolute difference between the first set of descriptors and the first reference set of descriptors and setting a threshold absolute difference as the summed first absolute difference; and summing a second absolute difference between the first set of descriptors and a second reference set of descriptors from the at least one respective reference set of descriptors, while the summing second absolute difference is at most the threshold absolute difference; if the summing second absolute difference exceeds the threshold absolute difference, stop summing the second absolute difference and start summing a third absolute difference between the first set of descriptors and a third reference set of descriptors from the at least one respective reference set of descriptors; else if the summed second absolute difference does not exceed the threshold absolute difference, resetting the threshold absolute difference as the summed second absolute difference.

13. The method according to claim 1, wherein M<N.

14. The method according to claim 1, wherein projecting the first descriptor to the first projected descriptor comprises interpolating elements thereof.

15. The method according to claim 1, wherein projecting the first descriptor to the first projected descriptor comprises averaging or dropping elements thereof.

16. (canceled)

17. The method according to claim 1, comprising storing the at least one respective reference set of descriptors; and wherein matching the first set of descriptors to the corresponding first reference set of descriptors comprises matching the first set of descriptors to the corresponding first reference set of descriptors from the stored at least one respective reference set of descriptors.

18. The method according to claim 1, wherein computing the first set of descriptors uses a set of lookup tables, including a first lookup table.

19. The method according to claim 18, comprising generating the first lookup table at compile time and using the first lookup table at runtime.

20. The method according to claim 18, comprising generating the first lookup table at runtime upon the first calculation of a given value thereof.

21.-23. (canceled)

24. The method according to claim 1, comprising: representing the first set of landmarks as a first signature; representing each of the at least one reference set of landmarks as respective reference signatures; and correlating the first signature and a reference signature, thereby approximating the first location of the radar sensor, and wherein accessing the at least one of the reference set of landmarks comprises selectively accessing the at least one reference set of landmarks based upon their respective reference signatures.

25.-29. (canceled)

30. The method according to claim 1, wherein M>N.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0262] For a better understanding of the invention, and to show how exemplary embodiments of the same may be brought into effect, reference will be made, by way of example only, to the accompanying diagrammatic Figures, in which:

[0263] FIG. 1 schematically depicts a plan view of a radar sensor for an exemplary embodiment;

[0264] FIG. 2 schematically depicts a method according to an exemplary embodiment;

[0265] FIG. 3 schematically depicts the method of FIG. 2, in more detail;

[0266] FIG. 4 is an example of an algorithm of landmark extraction for the method of FIG. 2;

[0267] FIG. 5 schematically depicts the method of FIG. 2, in more detail;

[0268] FIG. 6 schematically depicts the method of FIG. 2, in more detail;

[0269] FIG. 7 schematically depicts the method of FIG. 2, in more detail;

[0270] FIG. 8 is an example of an algorithm of data association for the method of FIG. 2;

[0271] FIG. 9 schematically depicts the method of FIG. 2, in more detail;

[0272] FIGS. 10A to 10C schematically depict a method of constructing signatures from radar scans; and

[0273] FIGS. 11 to 13 show code used in certain embodiments of the present disclosure.

DETAILED DESCRIPTION OF THE DRAWINGS

[0274] FIG. 1 schematically depicts a plan view of a radar sensor, particularly a FMCW scanning radar, for an exemplary embodiment. In this example, the radar sensor (green circle), centered on a vehicle (black box), sequentially gathers power-range spectra (dashed radial green rays) at each azimuth. Variables a and r denote azimuth and range, respectively. A sample signal for a particular azimuth is plotted, as power (dB) as a function of range bin.

[0275] FIG. 2 schematically depicts a method according to an exemplary embodiment. The method is a computer-implemented method of localizing a radar sensor.

[0276] Generally, the method comprises two main steps: landmark extraction and pose estimation.

[0277] The method comprises obtaining a first radar scan of a first environment of the radar sensor, wherein the first radar scan comprises a set of power-range spectra, including a first power-range spectrum (i.e. a 1D signal).

[0278] The method comprises extracting a first set of landmarks, including a first landmark, from the first radar scan, wherein the first landmark is defined by a range and an azimuth. This step is also known as landmark extraction or feature extraction. CFAR is a common filtering algorithm but is not distinctive enough. In contrast, the method described herein is able to detect more reliable and distinctive landmarks in the radar scene data. For example, a full radar scan is received (i.e. obtained) from the radar sensor and the method performs landmark extraction in order to accurately detect objects in the environment up to the maximum range of the radar sensor, for example as described with reference to FIGS. 3 and 4. The radar scan is a set of power-range spectra (i.e. 1D signals), one for each azimuth, for example as described with respect to FIG. 1. In this example, the power-range spectra are represented as an array of values per azimuth, all the way around the radar sensor. In this example, the output from the landmark extraction is a point cloud, which is a set of points (corresponding to landmarks) each specified by its range and angle from the center line (i.e. azimuth).

[0279] Next, the pose estimation step uses the new landmark point cloud to determine its relative pose to the previous landmark point cloud, and also its position relative to the map database which contains the landmark point clouds captured over the route during the mapping phase.

[0280] The pose estimation step has two main sub-steps, first to compute the scene point descriptors and then to use these and the landmarks point cloud to align the two point clouds and thus estimate the difference in position between the two. The scene point descriptors are each a set of unique descriptors one for each point in the point cloud. Thus, a scene point descriptor is represented as a matrix of values with each column representing the descriptor of each point. A descriptor must be computed and is represented as a vector of values that uniquely describes that point such that it can be identified and matched in other scans. The descriptor specifies the landmark point by the radial statistics of neighbouring points, both in range and angular slices.

[0281] The method comprises computing a respective first set of descriptors, including a first descriptor, of the first set of landmarks, wherein the first descriptor defines the first landmark by respective relative ranges and azimuths in relation to one or more landmarks included in the first set of landmarks, for example as described with reference to FIG. 5.

[0282] The method comprises accessing one or more reference sets of landmarks of respective environments and computing respective reference sets of descriptors of the reference sets of landmarks, for example as described with reference to FIGS. 6 to 8.

[0283] The method comprises matching the first set of descriptors to a corresponding first reference set of descriptors, for example as described with reference to FIGS. 6 to 8. This is known as data association and matches landmarks across radar scenes. Other approaches use feature descriptors popularised for vision systems, but these do not perform as well for radar data. The inventors use a novel feature descriptor that is better suited to radar landmarks and improves data associations between the live scan and other scans seen in the past e.g. the previous scan in case of radar odometry or map scans in the case of localization.

[0284] The method comprises localizing a first location of the radar sensor using a first result of the matching, for example as described with reference to FIGS. 6 to 8. This step is also known as position estimation and determines the spatial distance between two landmark sets.

[0285] FIG. 3 schematically depicts the method of FIG. 2, in more detail. Particularly, FIG. 3 schematically depicts a procedure for landmark extraction from a power-range spectrum. The input (raw signal) is processed from the top-left to produce the output on the bottom-right, in which the landmarks are denoted with red asterisks. Box 6 in this example highlights the ability of our approach to remove detections due to multipath reflections and noise. Boxes 3 and 5 demonstrate the importance of incorporating the high-frequency signals since using the smooth ones in boxes 2 and 4 alone would discard the high range resolution provided by an FMCW radar.

[0286] The first objective is to accurately detect objects in the radar sensor's environment with minimal false positives. Specifically, the method should find all landmarks perceived by the radar sensor while minimizing the number of redundant returns per landmark and avoiding the detection of nonexistent landmarks, such as those due to noise, multipath reflections, harmonics, and sidelobes. The method accepts power-range spectra (i.e. 1D signals), as inputs and returns a set of landmarks, each specified by its range and azimuth. The core idea is to estimate the signal's noise statistics then scale the power value at each range by the probability that it corresponds to a real detection. Continuous peaks in this reshaped signal are treated as objects; per peak, only the range at the centre of the peak is added to the landmark set.

[0287] Let the vector s(t)R.sup.N1 be the power-range spectrum at time t such that the element si is the power return at the i-th range bin, and a(t) is the associated azimuth. Let r(i)=(i0:5) give the range of bin i{1, 2, . . . , N}, where is the range resolution. Suppose that y(t)R.sup.N1 is the ideal signal if the environment was recorded perfectly. Then, s(t)=y(t)+v(y(t)), where v represents unwanted effects, like noise. Therefore, inferring y(t) from s(t) in order to accurately isolate the landmarks requires an approximation of v(y(t)) such that (t)=s(t){circumflex over (v)}(s(t)), =. Removing {circumflex over (v)} from s is the aim of the method. The landmark detections extracted from (t) are stored in the set L(s(t)). The landmark extraction method, as described next, references FIG. 4 and Algorithm 1.

[0288] FIG. 4 is an example of an algorithm, Algorithm 1, of landmark extraction for the method of FIG. 2

[0289] To begin, an unbiased signal q that preserves high-frequency information (box 2) is acquired by subtracting the noise floor of v(s) from s (line 1). The result is then smoothed to obtain the underlying low frequency signal p (box 3), which better exposes obvious landmark peaks (line 2). At this point, q is not discarded for two reasons: radar landmarks often manifest as high frequency peaks, so smoothing would dampen their presence; and smoothing muddles the peaks of landmarks that are in close proximity, making it difficult to distinguish between them. Thus, we integrate the information of both q and p. To estimate the noise characteristics, we treat the values of q that fall below zero as Gaussian noise with mean .sub.q=0 and standard deviation .sub.q (line 4). Let f(,.sup.2) be the probability density at x for the normal distribution N(, .sup.2). Then, for every range bin, the power values are scaled by the probability that they do not correspond to noise in two steps. First, each value of the smoothed signal p.sub.i is scaled by f(0,.sub.q.sup.2) (box 4 and line 8). This process is repeated for the high-frequency signal q.sub.i relative to the smoothed signal p.sub.i such that the scaling factor is f(p.sub.i,.sub.q.sup.2) (box 5 and line 9). The sum of both values is stored in .sub.i. These steps integrate high- and low-frequency information to preserve range accuracy while suppressing signal corruptions due to noise. Finally, the .sub.i values that are below the upper z.sub.q-value confidence bound of N(, .sub.q.sup.2) and therefore less likely to represent real landmarks are set to zero (box 6 and line 10).

[0290] The method extracts landmarks from .sub.i (the black signal in box 6) as follows. All values of are now either zero or belong to a peak. For each peak's centre located at range bin i, the tuple (a, r(i)) is added to the landmark set L(s) (line 11). These landmarks are then tested, and those identified as multipath reflections (MR) are removed (box 6 and line 12). Since MRs cause peaks with similar wavelet transform (WT) signatures to appear in the power-range spectrum at different ranges with amplitudes that decrease with distance, this step compares the continuous WTs w.sub.i,w.sub.jR.sup.H1 for each set of peaks P.sub.i and P.sub.j where j>i. If

[00001]$\frac{d_{\mathrm{ij}}}{H}<d_{\mathrm{thresh}}$

and the maximum power of P.sub.i is greater than that for P.sub.j, where

[00002]$d_{\mathrm{ij}}=.Math.\frac{w_i}{\max \left(w_i\right)}-\frac{w_j}{\max \left(w_j\right)}.Math.$

is a measure of dissimilarity, then P.sub.j is considered a MR, and (a,r(j)) is removed from the landmark set L(s). MR removal produces good results but requires significant computation time, making it optional. The method requires three free parameters with an optional fourth. In general, w.sub.median should represent a distance large enough to span multiple landmarks, and W.sub.binom should be around the width of an average peak

[00003]$\left(\frac{w_{\mathrm{median}}}{2}\right).$

A greater z.sub.q value raises the standard for peaks to be chosen as landmarks over noise, and d.sub.thresh is the minimum difference between WTs for detections to be considered independent. For the following analyses, let L=U.sub.t<t,L(s()) be the set of all landmarks in one full scan from time t to t.

[0291] FIG. 5 schematically depicts the method of FIG. 2, in more detail.

[0292] In this example, the pose estimation step uses the output landmark point cloud to determine the position (i.e. the first location) of the radar sensor, relative to a map database which contains reference landmark point clouds (i.e. reference landmarks), for example captured or acquired over a route during a mapping phase of the radar sensor. In addition, in this example, the pose estimation step uses the output landmark point cloud to determine the position relative to previous landmark point cloud (i.e. for odometry).

[0293] The pose estimation step has two main sub-steps: firstly to compute the scene point descriptors and secondly to use the scene point descriptors and the landmarks point cloud to align the two point clouds and thus estimate the difference in position between the two. The scene point descriptors are each a set of unique descriptors, one for each point in the point cloud. Thus, in this example, a scene point descriptor is represented as a matrix of values with each column representing the descriptor of each point. A descriptor is computed and is represented as a vector of values that uniquely describes that point such that the point can be identified and matched in other scans. The descriptor specifies the landmark point by the radial statistics of neighbouring points, both in range and angular slices.

[0294] FIG. 6 schematically depicts the method of FIG. 2, in more detail.

[0295] Once the scene point descriptors have been computed, the scene point descriptors can be used to perform the data association step to match each landmark point from one radar scan to a landmark point in the other radar scan. Finding the best matching descriptors from one scan to those in the other can be computationally expensive and the inventors have developed a number of improvements for efficiency, as described herein. Once a set of correspondences from one point cloud to the other point cloud is determined, the aim is to pick the best matches to ensure that the alignment between the point clouds is robust to outliers and false positives. Given good overlap between scans and stable associations, the motion of the sensor that must have occurred from one scan to the other may be computed. In this example, the motion estimate is output by the computer.

[0296] FIG. 7 schematically depicts the method of FIG. 2, in more detail. The core idea behind the data association algorithm that seeks to find similar shapes within the two landmark point clouds (in red) extracted from radar scans. The unary candidate matches (dotted green lines) are generated by comparing the points' angular characteristics. The selected matches (A,A) and (B,B.sub.2) minimize the difference between pairwise distances (|d.sub.ABd.sub.AB2|<|d.sub.ABd.sub.AB1|). In this way, shape matching by sequentially comparing angles and side lengths is approximated.

[0297] In more detail, the scan matching algorithm achieves robust point correspondences using high-level information in the radar scan. Intuitively, it seeks to find the largest subsets of two point clouds that share a similar shape. Unlike ICP, this method functions without a priori knowledge of the scans' orientations or displacements relative to one another. Thus, our algorithm is not constrained to have a good initial estimate of the relative pose and can be compare point clouds captured at arbitrary times without a map. The only requirements are that the areas observed lie in the same plane and contain sufficient overlap. One of the key attributes of our approach is to perform data association using not only individual landmark (i.e. unary) descriptors, but also the relationships between landmarks. For instance, imagine three landmarks that form the vertices of a scalene triangle. Then, the set of distances from each point to its neighbours is unique to that point regardless of the overall point cloud's placement, allowing the landmark to be straightforwardly matched to its counterpart in any other point cloud acquired by applying a rigid body transformation to the original triangle. The greater the number of points, the less likely it is for an individual point to have the same set of pairwise distances to its neighbours as another. Moreover, the exact position and orientation of the point cloud does not influence the pairwise relationships within it, so great disparities between the placements and orientations of the point clouds are inconsequential. We harness these observations to obtain reliable matches for our large landmark sets. With real data, the main challenges are that the landmark locations and detections are noisy, meaning that points do not always survive the rigid body transformation and the locations of those that do are affected by noise. A simple example illustrating the concept behind the data association algorithm is shown in FIG. 7.

[0298] FIG. 8 is an example of an algorithm, Algorithm 2, of data association for the method of FIG. 2. As inputs, it accepts two point clouds L.sup.o and L.sup.I for each of the two radar scans. The first point cloud L.sup.o is the original set of landmarks in Cartesian coordinates. Because landmarks are detected in polar space, the resulting point cloud will be dense at low ranges and sparse at high ones. The second point cloud L.sup.I compensates for this by generating a binary Cartesian grid of resolution that is interpolated from the binary polar grid of landmarks. The latter point cloud is less exact and only used to sidestep the range-density bias when processing the layout of the environment while data association is performed on the former (i.e. the algorithm returns a set of matches M that contains tuples (i,j) such that the landmark L.sub.1.sup.o{i} corresponds to L.sub.2.sup.o{j}. This distinction is a key insight. It preserves accuracy by operating on the landmarks detected in polar space while correcting for a main difficulty of scanning radars by interpreting the environment in Cartesian space. The data association is then performed in four steps. First, for every point in L.sub.1, the unaryMatches function suggests a potential point match in L.sub.2.sup.o based on some unary comparison method (line 1). Next, the non-negative compatibility score for each pair of proposed matches g=(i,i) and h=(j,j) is computed and assigned to the elements (g, h) and (h, g) of the WW matrix C such that it is symmetric and diagonally dominant (line 2). If the landmark matches g and h are correct, then the relationship between i and j in the first radar scan is similar to that between i and j in the second; the compatibility score reflects this pairwise similarity. In our method, the value is computed from the distances between corresponding pairs of points in the two scans. It reflects the understanding that real, correctly identified landmarks are the same distance apart in any two radar scans. The optimal set of matches M maximizes the overall compatibility, or reward. Suppose that m E {0,1}.sup.w such that (1) m.sub.i=1 if the unary match B{i} is deemed plausible and m.sub.i=0 otherwise; and (2) the selected matches do not conflict (i.e. a point in one point cloud cannot correspond to two in the other). Then, the optimal solution m* satisfies:

m*=m Cm

Due to the discretization of m, this maximization is computationally difficult, so we relax the aforementioned constraint to seek the continuously-valued u* such that:

[00004]$u^{*}=u\mathrm{Cu}$

Under these conditions, u* is the normalized eigenvector of the maximum eigenvalue of the positive semi-definite matrix C. The optimal solution m* is then be approximated from u* using the greedy approach shown in lines 3-11. In short, the greedy method iteratively adds satisfactory matches to the set M. On each iteration, the remaining valid matches are evaluated (line 7), that which returns the maximum reward is accepted (line 9), and those that conflict with it are removed from further consideration (lines 10 and 11). When the most recently selected match yields a reward less than the that if all matches were valued equally (i.e. is a weak match) or more than percent of the landmarks in either set are matched, the algorithm terminates (lines 6 and 8). Note that is the only free parameter in this method, and no outlier removal is required.

[0299] FIG. 9 schematically depicts the method of FIG. 2, in more detail.

[0300] In this example, computing the first set of descriptors, including the first descriptor, of the first set of landmarks comprises triangulating the first landmark with respect to a respective node and a landmark of the first set of landmarks. In this example, triangulating the first landmark with respect to the respective node and the landmark of the first set of landmarks comprises using the cosine rule.

[0301] The root (also known as reference) landmark or point is fixed for landmark or point i. Angles and distances in respect of all landmarks or points may be thus computed efficiently, for each root landmark.

[0302] In summary, the parallel computation using SIMD and the cosine rule are computed simultaneously. As an example, point cloud data in the form of descriptors is retrieved. The point cloud data is divided into a plurality of chunks. The number of points in a chunk is determined based on processor capacity. A single instruction may be executed to apply a single instruction to each point in the group simultaneously. The single instruction may be to compute distances between the points within the chunk. Once the distances are known, the cosine rule is used to compute the angles of the points within the chunk, again using a single instruction such as SIMD.

[0303] Once a first chunk has been processed in this way, a second chunk is selected, and so forth. In this way, the parallel processing of each chunk of the plurality of chunks is performed. Processing of the plurality of chunks occurs in series. In this way, all chunks may processed. The order in which the chunks are selected for processing may be selected at random.

Signatures

[0304] In this example, the radar signature header defines three datatypes: Signature, a two dimensional vector of doubles representing the radar signature itself. ExperienceSignatures, which is a std::map of Signatures indexed by node_id, and MapSignatures, which is a std::map of ExperienceSignatures indexed by experience_name.

[0305] A radon::loopclosure::RadarSignatureBuilder class is defined which has a three argument constructor corresponding to the signature range, azimuth bins and range bins parameters discussed above. The RadarSignatureBuilder will generate signatures which correspond to these parameters. A RadarSignatureBuilder object has a ComputeSignature method which will generate a Signature given a point cloud of radar landmarks extracted from the raw radar scan.

[0306] There is also a ComputeSignaturesForMap method which, given a map_client and a string representing the attribute name used to store radar data, will return a MapSignatures datatype for the entire map.

[0307] A function is provided, CompareSignatures, which will compute the similarity score for any given pair of Signatures. Two further functions make use of this comparison function: [0308] FindCandidateLoopClosureNodes, takes a Signature, ExperienceSignatures and a threshold, and returns a std::vector of all node ids in the experience with similarities below the threshold. This function is intended for using the signatures during map building. [0309] For localization the function, FindBestCandidateMapNode, is provided. Given a Signature, MapSignatures and a threshold, this returns the single best matching node_id in the map, provided it is below the similarity threshold.

[0310] In other words, with reference to FIGS. 10A to 10C, when an autonomous vehicle traverses a route of a map, it captures radar scans. Each scan may be referred to as a node 100 having a location on the map where the scan was captured. In this way, a node may refer to a point at which a radar scan has been captured.

[0311] The scan at a node 100, captures features along a plurality of azimuths as shown in FIG. 1. A plurality of range bins are provided for each azimuth. This can be visualised as a plurality of concentric rings, each having the same number of segments 102 separated according to the distribution of azimuths 104 (only 4 azimuths shown in Figure to avoid obscuring the drawing). Each segment 102 is assigned a number corresponding to a number of features detected in that segment by the radar scan.

[0312] A vector may be generated having a plurality of values. The number of elements in the vector corresponds to the number of segments. The value of each element equals the number from the corresponding segment. This vector is a signature 106. More specifically, this vector is described as a node signature.

[0313] The autonomous vehicle may capture a plurality of scans are different nodes along a route 108. As a result, a node signature for each node is generated, and they combine to form a route signature.

[0314] More than one route signature may be created if multiple routes have been traversed by the same or a plurality of autonomous vehicles. A route signature may be generated which includes the plurality of corresponding route signatures.

[0315] These previously generated signatures correspond to reference signatures.

[0316] In operation, a signature is generated for a current position. The signature for the current position may be called a first signature. The first signature is compared to the reference signatures to determine a closest match. The closest matched reference signature is correlated to the first signature. By correlating we mean that the first signature is equated to the closest matched reference signature. In this way, a position and pose of the first signature can be approximated using the closest matched reference signature.

[0317] Afterwards, position and pose can be determined more precisely using descriptor matching as described above. It is computationally much more efficient to obtain an approximation of position and pose of the radar sensor prior to determine a more precise position and pose as the estimation can indicate which points of the radar point cloud are likely starting points for the calculations.

REFERENCES

[0318] S. H. Cen and P. Newman, Precise Ego-Motion Estimation with Millimeter-Wave Radar Under Diverse and Challenging Conditions, 2018 IEEE International Conference on Robotics and Automation (ICRA), 2018, pp. 6045-6052, doi: 10.1109/ICRA.2018.8460687.

[0319] The subject matter of the references is incorporated herein in entirety by reference.

Notes

[0320] Although a preferred embodiment has been shown and described, it will be appreciated by those skilled in the art that various changes and modifications might be made without departing from the scope of the invention, as defined in the appended claims and as described above.

[0321] At least some of the example embodiments described herein may be constructed, partially or wholly, using dedicated special-purpose hardware. Terms such as component, module or unit used herein may include, but are not limited to, a hardware device, such as circuitry in the form of discrete or integrated components, a Field Programmable Gate Array (FPGA) or Application Specific Integrated Circuit (ASIC), which performs certain tasks or provides the associated functionality. In some embodiments, the described elements may be configured to reside on a tangible, persistent, addressable storage medium and may be configured to execute on one or more processors. These functional elements may in some embodiments include, by way of example, components, such as software components, object-oriented software components, class components and task components, processes, functions, attributes, procedures, subroutines, segments of program code, drivers, firmware, microcode, circuitry, data, databases, data structures, tables, arrays, and variables. Although the example embodiments have been described with reference to the components, modules and units discussed herein, such functional elements may be combined into fewer elements or separated into additional elements. Various combinations of optional features have been described herein, and it will be appreciated that described features may be combined in any suitable combination. In particular, the features of any one example embodiment may be combined with features of any other embodiment, as appropriate, except where such combinations are mutually exclusive. Throughout this specification, the term comprising or comprises means including the component(s) specified but not to the exclusion of the presence of others.

[0322] Attention is directed to all papers and documents which are filed concurrently with or previous to this specification in connection with this application and which are open to public inspection with this specification, and the contents of all such papers and documents are incorporated herein by reference.

[0323] All of the features disclosed in this specification (including any accompanying claims, abstract and drawings), and/or all of the steps of any method or process so disclosed, may be combined in any combination, except combinations where at least some of such features and/or steps are mutually exclusive.

[0324] Each feature disclosed in this specification (including any accompanying claims, abstract and drawings) may be replaced by alternative features serving the same, equivalent or similar purpose, unless expressly stated otherwise. Thus, unless expressly stated otherwise, each feature disclosed is one example only of a generic series of equivalent or similar features.

[0325] The invention is not restricted to the details of the foregoing embodiment(s). The invention extends to any novel one, or any novel combination, of the features disclosed in this specification (including any accompanying claims, abstract and drawings), or to any novel one, or any novel combination, of the steps of any method or process so disclosed.