Add test for guess matrix in GICP
transform the target pointcloud and adjust the expected FitnessScore to reveal the error
Merge remote-tracking branch 'gicp_test/gicp_guess'
further fix gicp tests and gicp final_transformation computation noted by @taketwo