Method and system for tracking a state of a camera

1 . Method for determining a state xx of a camera at a time t k , the state x k being a representation of a state random variable X k , wherein the state is related to a state-space model of a movement of the camera, the method comprising:

a) receiving an image of a scene of interest in an indoor environment captured by the camera at the time t k , wherein the indoor environment comprises N landmarks having known positions in a world coordinate system, N being a natural number;

b) receiving a state estimate of the camera at the time t k , wherein the state estimate comprises an estimate of the pose of the camera;

c) determining positions of M features in the image, M being a natural number; and

d) determining the state x k of the camera at the time t k based on (i) observation Z k at the time t k , the observation z k being a representation of a joint observation random variable Z k , the observation z k comprising the positions of the M features and data indicative of distance between each of the M features and its corresponding object point in the scene of interest, respectively, and (ii) the state estimate , wherein the determining of the state x k comprises determining an injective mapping estimate from at least a subset of the M features into the set of the N landmarks, and wherein the determining of the state x k is based on an observation model set up based on the determined injective mapping estimate.

2 . Method according to claim 1 , wherein the joint observation random variable Z k comprises M observation random variables Z k,i , i=1, . . . , M, which M observation random variables are ordered in an observation random variable sequence, wherein a representation of the observation random variable sequence provides an observation sequence z k,i , i=1, . . . , M.

3 . Method according to claim 2 , wherein the determining of the injective mapping estimate, as part of the determining of the state x k , from at least the subset of the M features into the set of N landmarks, the subset comprising L features, is based on a tree data structure having leaf nodes and inner nodes, and having a start node and at most a tree depth of L, wherein the tree data structure is constructed during the determining of the state x k and the determining of the injective mapping estimate as part of the determining of the state x k , and wherein each layer of the L layers of the tree data structure is associated with a sequence element of a subsequence of the observation sequence, wherein said subsequence comprises those sequence elements from the observation sequence corresponding to the L features, and wherein each node in the tree data structure except the start node is associated with a 5-tuple comprising (a) a layer number i related to the layer of the tree data structure in which the respective node is arranged, (b) an observation z k,i , (c) a landmark assigned to the observation z k,i , wherein the assigned landmark and the layer number i provide a feature-to-landmark assignment associated to the respective node, (d) a node-specific intermediate state of the camera, wherein said node-specific intermediate state of the camera is determined based on feature-to-landmark assignments corresponding to nodes on a path from the start node to the respective node in layer number i, and (e) a cost related to (i) the observation Z k,i , (ii) the assigned landmark, and (iii) the node-specific intermediate state of the camera, wherein the start node is associated to a cost of zero and has layer number 0, and wherein each node in the tree data structure comprises a unique path to the start node, and wherein the determining of the injective mapping estimate is based on

(i) a cost determination function which is configured to determine (a) for each leaf node in a currently constructed tree data structure, a cost, which cost is a sum of all costs associated to the nodes on the path between the start node and the respective leaf node and including the respective leaf node, and (b) a leaf node with the lowest cost, and

(ii) a tree construction function which receives as input a leaf node in a currently constructed tree data structure having layer number i, and constructs N-i new leaf nodes in layer i+1 corresponding to observation Z k,i+1 , wherein said N-i new leaf nodes are connected to the received leaf node, and which N-i new leaf nodes are related to the N-i landmarks which have not been previously considered on the path between the start node and the received leaf node including the received leaf node, and wherein the cost of each of the N-i new leaf nodes is based on the conditional probability density of observing z k,i+1 under the assumption that z k,i+1 is induced by one of the N-i landmarks using a corresponding observation model, respectively, which corresponding observation model is set up under the assumption that the camera is in a node-specific intermediate state determined based on the feature-to-landmark assignments on the path from the start node to the received leaf node, or under the assumption that the camera is in the state according to the state estimate in case the tree construction function receives the start node as input, wherein the tree construction function outputs a tree data structure with at least i+1 constructed layers,

and wherein the determining of the injective mapping estimate as part of the determining of the state x k proceeds by iteratively invoking the function pair (i) the tree construction function, in a first iteration having the start node as input, and (ii) the cost determination function evaluated on the output of the tree construction function, wherein the leaf node with the lowest cost provided by the cost determination function evaluated on a currently constructed tree data structure is used as input to the tree construction function in a next iteration, and wherein the iteration finishes once all L layers of the tree data structure are constructed and a finish leaf node in the L-th layer has, upon evaluation by the cost determination function, the lowest cost of all leaf nodes in the tree data structure, and wherein upon finishing, the injective mapping estimate is encoded in the path between the start node and the finish leaf node in the L-th layer, and wherein upon finishing, the state x k of the camera is determined based on the feature-to-landmark assignments on the path from the start node to the finish leaf node.

4 . Method according to claim 3 , wherein the cost of a node in layer number i of the tree data structure is related to a conditional probability density p(Z k,i |X k =“node−specific intermediate state”), wherein the cost is obtained by taking a logarithm of the conditional probability density.

5 . Method according to claim 3 , wherein for each of the L features, the corresponding data indicative of distance is used for determining a candidate subset of the N landmarks to which the respective feature may correspond, and using the candidate subsets for the determining of the injective mapping estimate as part of the determining of the state x k .

6 . Method according to claim 5 , wherein the tree construction function, evaluated on a node in layer i, considers only those landmarks during construction of the nodes in layer i+1 connected to the node in layer i which are in the candidate subset of observation Z k,i+1 and which are not already associated to another node on the path between the start node and the node in layer i.

7 . Method according to claim 1 , wherein the data indicative of distance comprise distance data obtained using a time-of-flight (TOF) camera, wherein the distance data at least relate to distances between the TOF camera and the landmarks corresponding to the determined M features.

8 . Method according to claim 7 , wherein the TOF camera comprises a plurality of phase detectors and a radio frequency (RF) modulated light source having an RF carrier having an RF carrier wavelength, wherein the distance data provided by the TOF camera are provided as distance data modulo the wavelength of the RF carrier.

9 . Method according to claim 8 , wherein the TOF camera comprises a maximum distance which it is capable of measuring and a minimum distance of zero, and wherein the determining of the injective mapping estimate as part of the determining of the state x k is based on those distance data between the minimum distance and the maximum distance which are, modulo the wavelength of the RF carrier, equivalent to the distance data provided by the TOF camera.

10 . Method according to claim 1 , wherein the data indicative of distance comprises intensity information for at least one of the determined M features.

11 . Method according to claim 1 , wherein the image is captured by the camera as a light source is operated to emit light which illuminates the scene of interest.

12 . Method according to claim 11 , wherein the determining the injective mapping estimate as part of the determining of the state xx comprises converting intensity information into distance data using an illumination model which is configured to determine, using (i) intensity information, (ii) a power of light emitted by the light source, (iii) a directivity of light emission by the light source, and (iii) a reflectivity of the landmarks, the distance of the at least one feature, for which intensity information is available, to its corresponding landmark.

13 . Method according to claim 1 , wherein the observation model comprises a camera model of the camera.

14 . Method according to claim 13 , wherein the camera model is embodied as a pinhole camera model.

15 . Computer program product stored on a non-transitory computer-readable medium comprising instructions which when executed by a computer, cause the computer to carry out a method according to claim 1 .

16 . Assembly, comprising (a) a camera, (b) a plurality of landmarks, and (c) a controller, wherein the controller is configured to carry out a method according to claim 1 .

17 . Assembly according to claim 16 , further comprising a light source.

18 . Assembly according to claim 16 , further comprising a time-of-flight (TOF) camera.

19 . Method according to claim 1 wherein determining the injective mapping estimate comprises constructing a tree data structure having L layers corresponding to L features, iteratively expanding the tree by generating child nodes representing candidate feature-to-landmark assignments, and selecting a path through the tree based on a cost function.

20 . Method according to claim 1 wherein determining the injective mapping estimate comprises representing each candidate feature-to-landmark assignment as a node in a search structure, associating each node with a node-specific intermediate state of the camera computed from the feature-to-landmark assignments along a path from a start node to that node, and determining the injective mapping estimate based on the node-specific intermediate states.

21 . Method according to claim 1 wherein determining the injective mapping estimate comprises evaluating, for each candidate feature-to-landmark assignment, a cost based on a conditional probability density of observing the feature given the landmark and a corresponding camera state, and selecting as the injective mapping estimate the assignment path having the lowest accumulated cost.

22 . Method for determining a state x k of a camera at a time t k , the state x k being a representation of a state random variable X k , wherein the state is related to a state-space model of a movement of the camera, the method comprising:

a) receiving an image of a scene of interest in an indoor environment captured by the camera at the time t k , wherein the indoor environment comprises N landmarks having known positions in a world coordinate system, N being a natural number;

b) receiving a state estimate of the camera at the time t k , wherein the state estimate comprises an estimate of the pose of the camera;

c) determining positions of M features in the image, M being a natural number; and

d) determining the state xx of the camera at the time t k based on (i) observation Z k at the time t k , the observation z k being a representation of a joint observation random variable Z k , the observation z k comprising the positions of the M features and data indicative of distance between each of the M features and its corresponding object point in the scene of interest, respectively, and (ii) the state estimate , wherein the determining of the state x k comprises determining an injective mapping estimate from at least a subset of the M features into the set of the N landmarks, and wherein the determining of the state x k is based on an observation model set up based on the determined injective mapping estimate;

wherein the joint observation random variable Z k comprises M observation random variables Z k,i , i=1, . . . , M, which M observation random variables are ordered in an observation random variable sequence, wherein a representation of the observation random variable sequence provides an observation sequence z k,i , i=1, . . . , M; and

wherein the determining of the injective mapping estimate, as part of the determining of the state x k , from at least the subset of the M features into the set of N landmarks, the subset comprising L features, is based on a tree data structure having leaf nodes and inner nodes, and having a start node and at most a tree depth of L, wherein the tree data structure is constructed during the determining of the state x k and the determining of the injective mapping estimate as part of the determining of the state x k , and wherein each layer of the L layers of the tree data structure is associated with a sequence element of a subsequence of the observation sequence, wherein said subsequence comprises those sequence elements from the observation sequence corresponding to the L features, and wherein each node in the tree data structure except the start node is associated with a 5-tuple comprising (a) a layer number i related to the layer of the tree data structure in which the respective node is arranged, (b) an observation z k,i , (c) a landmark assigned to the observation z k,i , wherein the assigned landmark and the layer number i provide a feature-to-landmark assignment associated to the respective node, (d) a node-specific intermediate state of the camera, wherein said node-specific intermediate state of the camera is determined based on feature-to-landmark assignments corresponding to nodes on a path from the start node to the respective node in layer number i, and (e) a cost related to (i) the observation z k,i , (ii) the assigned landmark, and (iii) the node-specific intermediate state of the camera, wherein the start node is associated to a cost of zero and has layer number 0, and wherein each node in the tree data structure comprises a unique path to the start node, and wherein the determining of the injective mapping estimate is based on

(i) a cost determination function which is configured to determine (a) for each leaf node in a currently constructed tree data structure, a cost, which cost is a sum of all costs associated to the nodes on the path between the start node and the respective leaf node and including the respective leaf node, and (b) a leaf node with the lowest cost, and

(ii) a tree construction function which receives as input a leaf node in a currently constructed tree data structure having layer number i, and constructs N-i new leaf nodes in layer i+1 corresponding to observation z k,i+1 , wherein said N-i new leaf nodes are connected to the received leaf node, and which N-i new leaf nodes are related to the N-i landmarks which have not been previously considered on the path between the start node and the received leaf node including the received leaf node, and wherein the cost of each of the N-i new leaf nodes is based on the conditional probability density of observing z k,i+1 under the assumption that z k,i+1 is induced by one of the N-i landmarks using a corresponding observation model, respectively, which corresponding observation model is set up under the assumption that the camera is in a node-specific intermediate state determined based on the feature-to-landmark assignments on the path from the start node to the received leaf node, or under the assumption that the camera is in the state according to the state estimate in case the tree construction function receives the start node as input, wherein the tree construction function outputs a tree data structure with at least i+1 constructed layers, and wherein the determining of the injective mapping estimate as part of the determining of the state x k proceeds by iteratively invoking the function pair (i) the tree construction function, in a first iteration having the start node as input, and (ii) the cost determination function evaluated on the output of the tree construction function, wherein the leaf node with the lowest cost provided by the cost determination function evaluated on a currently constructed tree data structure is used as input to the tree construction function in a next iteration, and wherein the iteration finishes once all L layers of the tree data structure are constructed and a finish leaf node in the L-th layer has, upon evaluation by the cost determination function, the lowest cost of all leaf nodes in the tree data structure, and wherein upon finishing, the injective mapping estimate is encoded in the path between the start node and the finish leaf node in the L-th layer, and wherein upon finishing, the state x k of the camera is determined based on the feature-to-landmark assignments on the path from the start node to the finish leaf node.