Methods and systems for simultaneous localization and calibration

1. A method comprising:

receiving sensor data indicative of a plurality of markers detected by a sensor on a vehicle located at a plurality of vehicle poses within an environment;

determining a pose graph representing the plurality of vehicle poses and the plurality of markers, wherein the pose graph comprises a plurality of edges associated with a cost function representing a distance measurement between a matching marker detection at different vehicle poses, wherein the distance measurement incorporates the different vehicle poses and a sensor pose on the vehicle;

determining a sensor pose transform representing the sensor pose on the vehicle that optimizes the cost function associated with the plurality of edges in the pose graph;

determining a second pose graph representing a plurality of subsequent vehicle poses and the plurality of markers based on sensor data subsequently received at the plurality of subsequent vehicle poses;

determining a second sensor pose transform based on the second pose graph; and

validating the sensor pose transform based on the second sensor pose transform; and

providing the sensor pose transform representing the sensor pose on the vehicle.

2. The method of claim 1 , further comprising:

determining the plurality of vehicle poses relative to a map of the plurality of markers to optimize the cost function associated with the plurality of edges in the pose graph.

3. The method of claim 2 , wherein the sensor pose transform and the plurality of vehicle poses relative to the map are determined simultaneously to optimize the cost function associated with the plurality of edges in the pose graph.

4. The method of claim 2 , wherein the sensor pose transform is determined subsequent to determining the plurality of vehicle poses, wherein the plurality of vehicle poses are held fixed while determining the sensor pose transform.

5. The method of claim 1 , wherein the pose graph further comprises a plurality of additional edges associated with an additional cost function representing an additional distance measurement between a marker detection at a vehicle pose and a mapped marker position, wherein the distance measurement incorporates the vehicle pose and the sensor pose on the vehicle; and

wherein the sensor pose transform is determined to optimize the additional cost function associated with the plurality of additional edges.

6. The method of claim 5 , further comprising causing the vehicle to make a series of movements based on previously mapped marker positions to determine the sensor pose transform.

7. The method of claim 1 , wherein the sensor pose transform comprises a position and an orientation of the sensor relative to a coordinate frame fixed to the vehicle.

8. The method of claim 1 , further comprising controlling the vehicle to navigate within the environment based on the determined sensor pose transform.

9. The method of claim 1 , further comprising:

identifying a difference between the sensor pose transform and second sensor pose transform; and

providing a calibration error signal indicating the identified difference.

10. The method of claim 1 , further comprising:

receiving second sensor data indicative of the plurality of markers detected by a second sensor on the vehicle at the plurality of vehicle poses within an environment;

determining the pose graph to further include a plurality of additional edges based on the second sensor data and a second sensor pose on the vehicle;

determining a second sensor pose transform representing the second sensor pose on the vehicle based on the pose graph; and

providing the second sensor pose transform.

11. The method of claim 1 , further comprising:

determining the pose graph to further include a plurality of additional edges associated with an additional cost function representing an error in consecutive vehicle poses based on a vehicle motion model;

determining one or more motion model parameters for the vehicle that optimize the additional cost function associated with the plurality of additional edges in the pose graph; and

providing the one or more motion model parameters for the vehicle.

12. The method of claim 11 , wherein the one or more motion model parameters are determined while the plurality of edges in the pose graph and the sensor pose transform are held fixed.

13. The method of claim 12 , further comprising:

updating one or more of the plurality of vehicle poses in the pose graph based on the one or more motion model parameters; and

subsequently adjusting the sensor pose transform.

14. The method of claim 11 , wherein the one or more motion model parameters comprise one or more of a turning delay, a wheel diameter, a turning radius, and a center point of rotation.

15. A system comprising:

a vehicle;

a sensor coupled to the vehicle;

a computing system;

a non-transitory computer readable medium, and program instructions stored on the non-transitory computer readable medium and executable by the computing system to:

receive sensor data indicative of a plurality of markers detected by the sensor on the vehicle located at a plurality of vehicle poses within an environment;

determine a pose graph representing the plurality of vehicle poses and the plurality of markers, wherein the pose graph comprises a plurality of edges associated with a cost function representing a distance measurement between a matching marker detection at different vehicle poses, wherein the distance measurement incorporates the different vehicle poses and a sensor pose on the vehicle, wherein the pose graph further comprises a plurality of additional edges associated with an additional cost function representing an additional distance measurement between a marker detection at a vehicle pose and a mapped marker position, wherein the distance measurement incorporates the vehicle pose and the sensor pose on the vehicle;

determine a sensor pose transform representing the sensor pose on the vehicle that optimizes the cost function associated with the plurality of edges in the pose graph wherein the sensor pose transform is determined to optimize the additional cost function associated with the plurality of additional edges; and

provide the sensor pose transform representing the sensor pose on the vehicle.

16. The system of claim 15 , wherein the computing system is further configurable to determine the plurality of vehicle poses relative to a map of the plurality of markers to optimize the cost function associated with the plurality of edges in the pose graph.

17. The system of claim 16 , wherein the sensor pose transform and the plurality of vehicle poses relative to the map are determined simultaneously to optimize the cost function associated with the plurality of edges in the pose graph.

18. The system of claim 16 , wherein the program instructions are further executable by the computing system to the vehicle to make a series of movements based on previously mapped marker positions to determine the sensor pose transform.

19. A non-transitory computer readable medium having stored therein program instructions executable by a computing system to cause the computing system to perform operations, the operations comprising:

receiving sensor data indicative of a plurality of markers detected by a sensor on a vehicle located at a plurality of vehicle poses within an environment;

determining a pose graph representing the plurality of vehicle poses and the plurality of markers, wherein the pose graph comprises a plurality of edges associated with a cost function representing a distance measurement between a matching marker detection at different vehicle poses, wherein the distance measurement incorporates the different vehicle poses and a sensor pose on the vehicle, and wherein the plurality of vehicle poses are determined relative to a map of the plurality of markers to optimize the cost function associated with the plurality of edges in the pose graph;

determining a sensor pose transform representing the sensor pose on the vehicle that optimizes the cost function associated with the plurality of edges in the pose graph, wherein the sensor pose transform is determined subsequent to determining the plurality of vehicle poses, and wherein the plurality of vehicle poses are held fixed while determining the sensor pose transform; and

providing the sensor pose transform representing the sensor pose on the vehicle.

20. The non-transitory computer readable medium of claim 19 , wherein the sensor pose transform comprises a position and an orientation of the sensor relative to a coordinate frame fixed to the vehicle.