System and method for estimating the pose of a localizing apparatus using reflective landmarks and other features

Abstract

The invention relates to a method for determining a state x k of a localizing apparatus at a time t. the state x k being a realization of a state random variable x k . The position and orientation of a camera, e.g., mounted on a drone as an example of a localizing apparatus, may be used for determining a state of the localizing apparatus, and the state may be tracked over time. Existing methods for determining the state of a localizing apparatus do not provide an adequate level of accuracy, however, thus making them insufficient for use in many applications. It is an object of the present invention to mitigate at least some of the disadvantages associated with the methods for determining a state x k of a localizing apparatus known from the state of the art.

The invention claimed is: 
     
         1 . Method for determining a state x k  of a localizing apparatus at a time t k , the state x k  being a realization of a state random variable x k , the method comprising:
 a) receiving a first image of a scene of interest in an indoor environment, wherein the indoor environment comprises N pre-arranged landmarks having known positions in a world coordinate system, N being a natural number;   b) receiving a second image of the scene of interest in the indoor environment;   c) receiving a state estimate   of the localizing apparatus at the time t k ;   d) receiving positions of n currently mapped simultaneous-localization-and-mapping (SLAM) landmarks in the scene of interest, wherein a map state s k  comprises at least (i) the state x k  of a localizing apparatus, (ii) the positions of the n currently mapped SLAM landmarks, and (iii) the positions of the N pre-arranged landmarks;   e) determining positions of M features in the first image, M being a natural number smaller than or equal to N, and determining an injective mapping estimate from the M features into the set of N pre-arranged landmarks;   f) determining positions of L SLAM features in the second image, and determining m SLAM features in the L SLAM features, wherein said m SLAM features are related to the n currently mapped SLAM landmarks, and determining a SLAM injective mapping estimate from the m SLAM features into the set of the n currently mapped SLAM landmarks;   g) using the determined injective mapping estimate and the determined SLAM injective mapping estimate to set up a joint observation model as part of a state-space model, wherein the joint observation model is configured to map a map state random variable s k  of which the map state s k  is a realization onto a joint observation random variable z k , wherein at the time t k , an observation z k  is a realization of the joint observation random variable z k , and wherein the observation comprises the position of at least one of the M features in the first image and the position of at least one of the m SLAM features in the second image; and   h) using (i) the state estimate  , (ii) the joint observation model, and (iii) the observation z k , to determine the state x k  of the localizing apparatus at the time t k  and to update the positions of the n currently mapped SLAM landmarks.   
     
     
         2 . Method according to  claim 1 , wherein the first image and the second image are captured at the time t k . 
     
     
         3 . Method according to  claim 1 , wherein the joint observation model comprises a first observation model describing the mapping of the at least one of the N pre-arranged landmarks, corresponding to the at least one of the M features via the injective mapping estimate, onto the position of the at least one of the M features, and wherein the joint observation model comprises a SLAM observation model describing the mapping of the at least one of the n currently mapped SLAM landmarks, corresponding to the at least one of the m SLAM features via the SLAM injective mapping estimate, onto the position of the at least one of the m SLAM features. 
     
     
         4 . Method according to  claim 1 , wherein the injective mapping estimate, subsequently termed IME, links the M features with the M pre-assigned landmarks, and wherein the SLAM injective mapping estimate, subsequently termed SLAM-IME, links the m SLAM features with the m SLAM landmarks, wherein, for a feature i of the M features, the corresponding landmark is landmark IME(i), and wherein for a feature j of the m SLAM features, the corresponding SLAM landmark is SLAM landmark SLAM-IME(j), and wherein, for a feature-landmark pair (i,IME(i)) and for a SLAM feature/SLAM landmark pair (j,SLAM-IME(j)), the joint observation model links an observation random variable z k,i  resp. z k,j , wherein the joint observation random variable z k  comprises the observation random variable z k,i  (resp. z k,j , to the state random variable x k  and the position of landmark POS (IME(i)) resp. SLAM landmark POS (SLAM-IME(j),k):
 z k,i =h 1 (x k , POS(IME(i))), i=1, . . . , M, and   z k,j =h SLAM (x k , POS(SLAM-IME(j), k)), j=1, . . . , m, wherein h 1 (·) is the first observation model and h SLAM (·) is the SLAM observation model.   
     
     
         5 . Method according to  claim 1 , wherein the M features related to the N pre-arranged landmarks are substantially indistinguishable from one another, and wherein the indoor environment comprises K additional landmarks with a priori unknown positions in the world coordinate system, wherein H≤K additional landmarks of the K additional landmarks are in the scene of interest and wherein currently estimated positions of P of the K additional landmarks are part of the map state s k , and wherein the H additional landmarks are captured in the first image as additional features which are substantially indistinguishable from the M features, wherein the method additionally comprises the following: 1) determining, jointly with the determining of the positions of the M features, positions of the H additional features, wherein the determining thereby provides M+H positions of the features and of the additional features in the first image; 2) separating the positions of the M+H features and additional features into a first set comprising M+R positions, with R≤P, and into a second set comprising Q positions, with H=R+Q; 3) determining the injective mapping estimate from the first set into an indistinguishable landmark set comprising the positions of the N pre-arranged landmarks and the P currently estimated positions; and 4) updating the map state s; based on the second set. 
     
     
         6 . Method according to  claim 5 , wherein the first image is captured by a first camera with a first camera center and a first image sensor, wherein a first camera center position in the world coordinate system is determined based on the state estimate  , and wherein the separating proceeds by carrying out the following steps for each element in the set of M+H positions, the M+H positions being 2D positions measured in a coordinate system of the first image sensor: (i) determining a ray having as initial point the first camera center and as further point a 3D position corresponding to the 2D position of the respective element, wherein the 3D position is determined based on the state estimate   and a known spatial relationship between the localizing apparatus and the first image sensor, and (ii) assigning the respective element to the first set if any landmark in the indistinguishable landmark set, measured by way of orthogonal projection onto the respective ray, is closer to the respective ray than a pre-set distance threshold, and otherwise assigning the respective element to the second set. 
     
     
         7 . Method according to  claim 5 , wherein the observation additionally comprises the position of at least one of the R additional features in the first image, and wherein together with the updating of the positions of the n currently mapped SLAM landmarks the P currently estimated positions of additional landmarks are updated. 
     
     
         8 . Method according to  claim 7 , wherein the first observation model additionally describes the mapping of the at least one of the R additional landmarks, corresponding to the at least one of the R additional features via the injective mapping estimate, onto the position of the at least one of the R additional features. 
     
     
         9 . Method according to  claim 1 , wherein the determining of the state x k  using (i) the state estimate  , (ii) the joint observation model, and (iii) the observation z k , is done by using update equations provided by applying an extended Kalman filter to the state-space model, wherein the update equations comprise the Jacobian matrix of the joint observation model, wherein the Jacobian matrix of the first observation model is evaluated at the state estimate  , and wherein the Jacobian matrix of the SLAM observation model is evaluated at at least the state estimate   and at the positions of the n currently mapped SLAM landmarks. 
     
     
         10 . Method according to  claim 9 , wherein the update equations corresponding to all M features and all m SLAM features are consecutively and independently invoked. 
     
     
         11 . Method according to  claim 1 , wherein the first image is captured by the first camera as a light source is operated to emit light which illuminates the scene of interest. 
     
     
         12 . A non-transitory computer program product comprising instructions which when executed by a computer, cause the computer to carry out a method according to  claim 1 . 
     
     
         13 . Assembly, comprising (a) a localizing apparatus, (b) a first camera arranged on the localizing apparatus, (c) a plurality of pre-arranged landmarks in an indoor environment, and (d) a controller, wherein the controller is configured to carry out a method according to  claim 1 . 
     
     
         14 . Assembly according to  claim 13 , further comprising a second camera arranged on the localizing apparatus, wherein the first camera is arranged on the localizing apparatus in such a way that a ceiling of the indoor environment is in the field of view of the first camera, wherein a camera axis of the first camera is in the first camera's field of view, and the second camera is arranged on the localizing apparatus in such a way that the second camera's camera axis is substantially orthogonal to the camera axis of the first camera. 
     
     
         15 . Assembly according to  claim 13 , further comprising a light source, wherein the light source is arranged on the localizing apparatus, and/or at least one additional landmark.