Optimized trajectory keyframe insertion