Pose graph downsampling

1 . A system comprising:

one or more processors; and

one or more non-transitory computer-readable media storing computer-executable instructions which, when executed by the one or more processors, cause the one or more processors to perform operations comprising:

determining a first joint probability distribution over states of a first vehicle, the first joint probability distribution associated with a first sequence of time steps;

receiving lidar measurements associated with a second sequence of time steps;

integrating out variables from the first joint probability distribution to determine a second joint probability distribution over poses of the first vehicle, the second joint probability distribution associated with the second sequence of time steps;

determining, using the second joint probability distribution, parameters of a third joint probability distribution over the poses of the first vehicle, the third joint probability distribution being an approximation of the second joint probability distribution and characterized by unary factors for time steps of the second sequence of time steps and binary factors for pairs of neighboring time steps of the second sequence of time steps, determining the parameters of the third joint probability distribution comprising, recursively in a temporal order of the second sequence of time steps or a reverse temporal order of the second sequence of time steps:

determining intermediate covariance matrices associated with the unary factors and binary factor for two subsequent poses of the first vehicle to equate marginal covariances of the two subsequent poses under the third joint probability distribution with marginal covariances of the two subsequent poses under the second joint probability distribution, and to equate a marginal covariance of an incremental chance between the two subsequent poses under the third joint probability distribution with a marginal covariance of an incremental change between the two subsequent poses under the second joint probability distribution;

setting a covariance matrix associated with the binary factor for the two subsequent poses to the intermediate covariance matrix associated with the binary factor for the two subsequent poses;

incrementing an inverse covariance matrix associated with the unary factor for a first pose of the two subsequent poses by an inverse of the intermediate covariance matrix associated with the unary factor for the first pose of the two subsequent poses;

determining a unary covariance matrix by marginalizing the binary factor for the two subsequent poses and the unary factor for a second pose of the two subsequent poses; and

decrementing an inverse covariance matrix associated with the unary factor for the first pose of the two subsequent poses by an inverse of the unary covariance matrix;

based at least in part on the lidar measurements and the third joint probability distribution:

estimating poses of the first vehicle associated with the second sequence of time steps; and

generating a map of an environment; and

transmitting the generated map to a second vehicle, wherein the second vehicle is an autonomous vehicle configured to be controlled to navigate through the environment based at least in part on the generated map.

2 . The system of claim 1 , wherein determining the parameters of the third joint probability distribution is performed recursively in a temporal order of the second sequence of time steps or a reverse temporal order of the second sequence of time steps.

3 . A method comprising:

determining a first joint probability distribution over states of a sensor platform comprising a first vehicle, the first joint probability distribution associated with a first sequence of time steps;

receiving sensor data indicating measurements, by a sensor system onboard the sensor platform, at time steps of a second sequence of time steps;

integrating out variables from the first joint probability distribution to determine a second joint probability distribution over poses of the sensor platform, the second joint probability distribution associated with the second sequence of time steps;

determining, using the second joint probability distribution, parameters of a third joint probability distribution over poses of the sensor platform, the third joint probability distribution being an approximation of the second joint probability distribution and characterized by unary factors for time steps of the second sequence of time steps and binary factors for pairs of neighboring time steps of the second sequence of time steps, determining the parameters of the third joint probability distribution comprising, recursively in a temporal order of the second sequence of time steps or a reverse temporal order of the second sequence of time steps:

determining intermediate covariance matrices associated with the unary factors and binary factor for two subsequent poses of the sensor platform to equate marginal covariances of the two subsequent poses under the third joint probability distribution with marginal covariances of the two subsequent poses under the second joint probability distribution, and to equate a marginal covariance of an incremental change between the two subsequent poses under the third joint probability distribution with a marginal covariance of an incremental change between the two subsequent poses under the second joint probability distribution;

setting a covariance matrix associated with the binary factor for the two subsequent poses to the intermediate covariance matrix associated with the binary factor for the two subsequent poses;

incrementing an inverse covariance matrix associated with the unary factor for a first pose of the two subsequent poses by an inverse of the intermediate covariance matrix associated with the unary factor for the first pose of the two subsequent poses;

determining a unary covariance matrix by marginalizing the binary factor for the two subsequent poses and the unary factor for a second pose of the two subsequent poses; and

decrementing an inverse covariance matrix associated with the unary factor for the first pose of the two subsequent poses by an inverse of the unary covariance matrix;

estimating, based at least in part on the sensor data and the third joint probability distribution, poses of the sensor platform associated with the second sequence of time steps;

generating, based at least in part on the sensor data and the third joint probability distribution, a map of an environment; and

transmitting the map of the environment to a second vehicle, wherein the second vehicle is configured to be controlled to navigate through the environment based at least in part on the map.

4 . The method of claim 3 , wherein determining the parameters of the third joint probability distribution is performed recursively in a temporal order of the second sequence of time steps or a reverse temporal order of the second sequence of time steps.

5 . The method of claim 3 , wherein determining the intermediate covariance matrices comprises performing a coordinate descent.

6 . The method of claim 3 , further comprising:

identifying a determined covariance matrix as having a negative eigenvalue; and

replacing the identified covariance matrix with a matrix having only positive eigenvalues.

7 . The method of claim 3 , wherein the sensor system comprises a lidar sensor and the obtained sensor data indicates measurements obtained using the lidar sensor.

8 . The method of claim 3 , wherein estimating the poses of the sensor platform associated with the second sequence of time steps comprises:

identifying, using the sensor data, a loop closure associated with two time steps of the second sequence of time steps; and

updating the third joint probability distribution based on the identified loop closure.

9 . The method of claim 3 , wherein the second sequence of time steps has a lower frequency than the first sequence of time steps.

10 . The method of claim 3 , wherein the determining the first joint probability distribution comprises processing measurements obtained using the sensor system using one or more of a filter or a smoother.

11 . One or more non-transitory computer-readable media storing instructions executable by one or more processors, wherein the instructions, when executed, cause the one or more processors to perform operations comprising:

determining a first joint probability distribution over states of a sensor platform, the first joint probability distribution associated with a first sequence of time steps;

receiving sensor data indicating measurements, by a sensor system onboard the sensor platform, at time steps of a second sequence of time steps;

integrating out variables from the first joint probability distribution to determine a second joint probability distribution over poses of the sensor platform, the second joint probability distribution associated with the second sequence of time steps;

determining, using the second joint probability distribution, parameters of a third joint probability distribution over poses of the sensor platform, the third joint probability distribution being an approximation of the second joint probability distribution and characterized by unary factors for time steps of the second sequence of time steps and binary factors for pairs of neighboring time steps of the second sequence of time steps, determining the parameters of the third joint probability distribution comprising, recursively in a temporal order of the second sequence of time steps or a reverse temporal order of the second sequence of time steps:

determining intermediate covariance matrices associated with the unary factors and binary factor for two subsequent poses of the sensor platform to equate marginal covariances of the two subsequent poses under the third joint probability distribution with marginal covariances of the two subsequent poses under the second joint probability distribution, and to equate a marginal covariance of an incremental change between the two subsequent poses under the third joint probability distribution with a marginal covariance of an incremental change between the two subsequent poses under the second joint probability distribution;

setting a covariance matrix associated with the binary factor for the two subsequent poses to the intermediate covariance matrix associated with the binary factor for the two subsequent poses;

incrementing an inverse covariance matrix associated with the unary factor for a first pose of the two subsequent poses by an inverse of the intermediate covariance matrix associated with the unary factor for the first pose of the two subsequent poses;

determining a unary covariance matrix by marginalizing the binary factor for the two subsequent poses and the unary factor for a second pose of the two subsequent poses; and

decrementing an inverse covariance matrix associated with the unary factor for the first pose of the two subsequent poses by an inverse of the unary covariance matrix;

estimating, based at least in part on the sensor data and the third joint probability distribution, poses of the sensor platform associated with the second sequence of time steps;

generating, based at least in part on the sensor data and the third joint probability distribution, a map of an environment; and

transmitting the map of the environment to a vehicle, wherein the vehicle is configured to be controlled to navigate through the environment based at least in part on the map.