Matlab code for mean shift algorithm