Fix: fix silly bug in GICP preventing the inner optimization loop

Should have done this before but just couldn't find time previously.
Now PCL GICP result is exactly same as original GICP code.