remove useless code
rearrange code to have less diffs with the original linemod.cpp