Sensor fusion, registration and planning methods