Update kernel based mean shift original code.cpp