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.
Merge pull request #697 from nizar-sallem/fix_gicp

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