Optimal Topology Recovery Scheme for Multi-robot Formation Control