Outlier removal and discontinuity preserving smoothing of range data