Methods and systems for simultaneous localization and calibration

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.

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.