public static void main(String[] args) throws IOException
{
List<Point3D_F64> cloudA = read("/home/pja/Downloads/scan.dat");
List<Point3D_F64> cloudB = read("/home/pja/Downloads/scan2.dat");
Se3_F64 refToCurrent = new Se3_F64();
refToCurrent.getT().set(0.3,0.05,0);
RotationMatrixGenerator.eulerXYZ(0,0, UtilAngle.degreeToRadian(20),refToCurrent.getR());
for( Point3D_F64 p : cloudB )
SePointOps_F64.transform(refToCurrent,p,p);
IcpCloud3D icp = new IcpCloud3D(0.2,30,1e-8);
icp.setReference(cloudA);
if( !icp.setCurrent(cloudB) )
throw new RuntimeException("ICP failed");
Se3_F64 found = icp.getReferenceToCurrent();
System.out.println("Found");
found.print();
System.out.println("actual");
refToCurrent.print();
System.out.println("Fit Fraction: "+icp.getFitFraction());
}