Topology Processing for Waypoint-based Navigation Maps

The operations of a computer-implemented method include obtaining a topological map of an environment including a series of waypoints and a series of edges. Each edge topologically connects a corresponding pair of adjacent waypoints. The edges represent traversable routes for a robot. The operations include determining, using the topological map and sensor data captured by the robot, one or more candidate alternate edges. Each candidate alternate edge potentially connects a corresponding pair of waypoints that are not connected by one of the edges. For each respective candidate alternate edge, the operations include determining, using the sensor data, whether the robot can traverse the respective candidate alternate edge without colliding with an obstacle and, when the robot can traverse the respective candidate alternate edge, confirming the respective candidate alternate edge as a respective alternate edge. The operations include updating, using nonlinear optimization and the confirmed alternate edges, the topological map.