Using sensor-based observations of agents in an environment to estimate the pose of an object in the environment and to estimate an uncertainty measure for the pose

1. A computer-implemented method, comprising:

receiving a plurality of observations from a plurality of agents in an environment, each of the observations generated based on sensor data from one or more sensors of a corresponding agent of the agents and including:

an observation time, a source identifier of the corresponding agent, an observed object identifier of a corresponding observed object of multiple objects of the environment, a measured object pose for the corresponding observed object, and an observation uncertainty measure for the measured object pose;

identifying a group of the observations for a target time based on the observation times of the observations of the group satisfying a threshold relative to the target time;

generating, based on the group of observations, a multigraph having a plurality of nodes and edges connecting the nodes, the generating comprising defining a reference frame node of the nodes, defining object nodes of the nodes that are based on the observed object identifiers of the group, and generating the edges connecting the nodes based on the observed object identifiers, the source identifiers, and the measured object poses and observation uncertainty measures of the group,

wherein generating each of the edges comprises:

connecting the edge with a respective pair of nodes,

determining an edge measured object pose based on at least one of one or more of the measured object poses corresponding to the respective pair of nodes, and

determining an edge object uncertainty measure based on at least one of one or more of the object uncertainty measures corresponding to the respective pair of the nodes, and

wherein generating the multigraph comprises:

generating a first set of multiple of the edges, wherein the first set of the edges define a first simple path that connects the reference frame node and a given object node of the object nodes, the first set of the edges comprising a first set first edge that connects the given object node to a first object node, and a first set second edge that connects the first object node to a second object node or to the reference frame node, and

generating a second set of multiple of the edges that define a second simple path that connects the reference frame node and the given object node, the second set of edges comprising a second set first edge that is in addition to the edges of the first set of the edges that define the first simple path;

generating a composite pose and composite uncertainty measure for each of a plurality of simple paths along the edges that connect the reference frame node and the given object node of the object nodes, including the first simple path and the second simple path, wherein the composite pose and the composite uncertainty measure for each of the simple paths is based on the edge measured object pose and the edge observation uncertainty measure for each of the edges of the simple path;

generating, based on the composite poses and the composite uncertainty measures, a pose and an uncertainty measure for the object identifier on which the given object node is based;

assigning the pose and the uncertainty measure to the object identifier on which the given object node is based; and

providing the object identifier, the pose, and the uncertainty measure to at least one robotic agent, of the agents in the environment, to influence control commands generated by the robotic agent.

2. The computer-implemented method of claim 1 , wherein at least one of the observed object identifiers of the observations identifies one of the agents of the environment.

3. The computer-implemented method of claim 1 , wherein the object identifier on which the given object node is based identifies a robotic agent of the agents of the environment.

4. The computer-implemented method of claim 1 , wherein:

the observed object identifier of a first observation identifies a mobile robot agent of the agents and the source identifier of the first observation identifies the mobile robot agent; and

the observed object identifier of a second observation identifies a non-agent object of the agents and the source identifier of the second observation identifies the mobile robot agent.

5. The computer-implemented method of claim 4 , wherein:

the observed object identifier of a third observation identifies the non-agent object of the agents and the source identifier of the third observation identifies a fixed sensor agent of the agents.

6. The computer-implemented method of claim 1 , wherein generating the pose and the uncertainty measure based on the composite pose and the composite uncertainty measure comprises:

merging the composite poses and the composite uncertainty measures for the plurality of simple paths.

7. The computer-implemented method of claim 6 , wherein merging the composite poses and the composite uncertainty measures for the plurality of simple paths comprises weighting, in the merging, the composite poses and the composite uncertainty measures based on the composite uncertainty measures.

8. The computer-implemented method of claim 1 , wherein generating an edge of the edges along a simple path of the simple paths comprises:

generating the edge measured object pose and the edge observation uncertainty measure for the edge based on spherical linear interpolation of the edge measured object pose and the edge observation uncertainty measure of a first observation of the observations of the group and the edge measured object pose and the edge observation uncertainty measure of a second observation of the observations of the group, wherein the first observation and the second observation both identify the same observed object identifier and the same source identifier.

9. The computer-implemented method of claim 1 , wherein generating the composite pose and the composite uncertainty measure for each of the plurality of simple paths comprises applying an unscented transform to the edge measured object poses and the edge observation uncertainty measures for the edges of the simple path.

10. The computer-implemented method of claim 1 , wherein the first set second edge connects the first object node to the second object node, and wherein the first set of the edges further comprise a first set third edge that connects the second object node to a third object node or to the reference frame node.

11. A system comprising:

a pose and uncertainty system comprising one or more computing systems;

a plurality of agents in an environment providing sensor-based observations to the pose and uncertainty system over a period of time, wherein each of the observations is generated based on sensor data from one or more sensors of a corresponding agent of the agents and includes:

an observation time, a source identifier of the corresponding agent, an observed object identifier of a corresponding observed object of multiple objects of the environment, a measured object pose for the corresponding observed object, and an observation uncertainty measure for the measured object pose;

wherein the pose and uncertainty system comprises instructions that, when executed by the one or more computing systems, cause the computing systems to:

identify a group of the observations for a target time based on the observation times of the observations of the group satisfying a threshold relative to the target time;

generate, based on the group of observations, a multigraph having a plurality of nodes and edges connecting the nodes, the generating comprising defining a reference frame node of the nodes, defining object nodes of the nodes that are based on the observed object identifiers of the group, and generating the edges connecting the nodes based on the observed object identifiers, the source identifiers, and the measured object poses and observation uncertainty measures of the group,

wherein in generating each of the edges the instructions cause one or more of the computing systems to:

connect the edge with a respective pair of nodes,

determine an edge measured object pose based on at least one of one or more of the measured object poses corresponding to the respective pair of nodes, and

determine an edge object uncertainty measure based on at least one of one or more of the object uncertainty measures corresponding to the respective pair of the nodes, and

wherein in generating the multigraph the instructions cause one or more of the computing systems to:

generate a first set of multiple of the edges, wherein the first set of the edges define a first simple path that connects the reference frame node and a given object node of the object nodes, the first set of the edges comprising a first set first edge that connects the given object node to a first object node, and a first set second edge that connects the first object node to a second object node or to the reference frame node, and

generate a second set of multiple of the edges that define a second simple path that connects the reference frame node and the given object node, the second set of edges comprising a second set first edge that is in addition to the edges of the first set of the edges that define the first simple path;

generate a composite pose and composite uncertainty measure for each of a plurality of simple paths along the edges that connect the reference frame node and the given object node of the object nodes, including the first simple path and the second simple path, wherein the composite pose and the composite uncertainty measure for each of the simple paths is based on the edge measured object pose and the edge observation uncertainty measure for each of the edges of the simple path;

generate, based on the composite poses and the composite uncertainty measures, a pose and an uncertainty measure for the object identifier on which the given object node is based;

assign the pose and the uncertainty measure to the object identifier on which the given object node is based; and

provide the object identifier and the pose to at least one robotic agent, of the agents in the environment, to influence control commands generated by the robotic agent.

12. The system of claim 11 , wherein at least one of the observed object identifiers of the observations identifies one of the agents of the environment.

13. The system of claim 11 , wherein the object identifier on which the given object node is based identifies a robotic agent of the agents of the environment.

14. The system of claim 11 , wherein:

the observed object identifier of a first observation identifies a mobile robot agent of the agents and the source identifier of the first observation identifies the mobile robot agent; and

the observed object identifier of a second observation identifies a non-agent object of the agents and the source identifier of the second observation identifies the mobile robot agent.

15. The system of claim 11 , wherein the instructions that, when executed by the one or more computing systems, cause the computing systems to generate the pose and the uncertainty measure based on the composite pose and the composite uncertainty measure comprise instructions that cause the computing systems to:

merge the composite poses and the composite uncertainty measures for the plurality of simple paths.

16. The system of claim 15 , wherein the instruction that, when executed by the one or more computing systems, cause the computing systems to merge the composite poses and the composite uncertainty measures for the plurality of simple paths comprises instructions that cause the computing systems to weight, in the merge, the composite poses and the composite uncertainty measures based on the composite uncertainty measures.

17. The system of claim 11 , wherein the instruction that, when executed by the one or more computing systems, cause the computing systems to generate an edge of the edges along a simple path of the simple paths comprise instructions that cause the computing systems to:

generate the edge measured object pose and the edge observation uncertainty measure for the edge based on spherical linear interpolation of the edge measured object pose and the edge observation uncertainty measure of a first observation of the observations of the group and the edge measured object pose and the edge observation uncertainty measure of a second observation of the observations of the group, wherein the first observation and the second observation both identify the same observed object identifier and the same source identifier.

18. The system of claim 11 , wherein the instruction that, when executed by the one or more computing systems, cause the computing systems to generate the composite pose and the composite uncertainty measure for each of the plurality of simple paths comprise instructions that cause the computing systems to apply an unscented transform to the edge measured object poses and the edge observation uncertainty measures for the edges of the simple path.

19. The system of claim 11 , wherein the first set second edge connects the first object node to the second object node, and wherein the first set of the edges further comprise a first set third edge that connects the second object node to a third object node or to the reference frame node.

20. At least one non-transitory computer-readable medium comprising instructions that, in response to execution of the instructions by one or more processors, cause the one or more processors to perform the following operations:

receiving a plurality of observations from a plurality of agents in an environment, each of the observations generated based on sensor data from one or more sensors of a corresponding agent of the agents and including:

an observation time, a source identifier of the corresponding agent, an observed object identifier of a corresponding observed object of multiple objects of the environment, a measured object pose for the corresponding observed object, and an observation uncertainty measure for the measured object pose;

identifying a group of the observations for a target time based on the observation times of the observations of the group satisfying a threshold relative to the target time;

generating, based on the group of observations, a multigraph having a plurality of nodes and edges connecting the nodes, the generating comprising defining a reference frame node of the nodes, defining object nodes of the nodes that are based on the observed object identifiers of the group, and generating the edges connecting the nodes based on the observed object identifiers, the source identifiers, and the measured object poses and observation uncertainty measures of the group,

wherein generating each of the edges comprises:

connecting the edge with a respective pair of nodes,

determining an edge measured object pose based on at least one of one or more of the measured object poses corresponding to the respective pair of nodes, and

determining an edge object uncertainty measure based on at least one of one or more of the object uncertainty measures corresponding to the respective pair of the nodes, and

wherein generating the multigraph comprises:

generating a first set of multiple of the edges, wherein the first set of the edges define a first simple path that connects the reference frame node and a given object node of the object nodes, the first set of the edges comprising a first set first edge that connects the given object node to a first object node, and a first set second edge that connects the first object node to a second object node or to the reference frame node, and

generating a second set of multiple of the edges that define a second simple path that connects the reference frame node and the given object node, the second set of edges comprising a second set first edge that is in addition to the edges of the first set of the edges that define the first simple path;

generating a composite pose and composite uncertainty measure for each of a plurality of simple paths along the edges that connect the reference frame node and the given object node, including the first simple path and the second simple path, wherein the composite pose and the composite uncertainty measure for each of the simple paths is based on the edge measured object pose and the edge observation uncertainty measure for each of the edges of the simple path;

generating, based on the composite poses and the composite uncertainty measures, a pose and an uncertainty measure for the object identifier on which the given object node is based;

assigning the pose and the uncertainty measure to the object identifier on which the given object node is based; and

providing the object identifier and the pose to at least one robotic agent, of the agents in the environment, to influence control commands generated by the robotic agent.