ITOMP: Incremental Trajectory Optimization for Real-Time Replanning in Dynamic Environments