Minimally invasive surgery has the potential to reduce pain, costs and hospital stays. We are developing robots and software to enter through a small incision or a natural orifice to perform surgical diagnostics or delivery therapies. Currently, we are developing three-dimensional visualization algorithms to restore a surgeon's view of the operating site when the procedure became minimally invasive. We are developing these algorithms for a surgical snake robot and the Intuitive Surgical robot.