SwRI developed a factor-graph-based algorithm to simultaneously localize and map the environment for automated vehicles. It intelligently combines sparse image features and landmark data with inertial measurement unit (IMU) data and wheel encoder information to produce highly accurate mapping and localization data, similar to those produced by lidar sensors. The factor graph fused visual odometry – a technique for estimating vehicle motion from a sequence of camera images – with the IMU data to develop key points and pose estimation, which enable automated vehicles to navigate off-road courses.