Transform FittingΒΆ

Demonstrate how to estimate rigid body motion of a point cloud

ExampleTransformFitting.java

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
public static void main(String[] args) {
    Random rand = new Random(234);

    // Create a transform which will be applied to the point cloud
    Se3_F64 actual = new Se3_F64();
    ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ,0.5,-2,0.15,actual.R);
    actual.T.set(2,3,-2);

    // Create a random point cloud and transform it
    List<Point3D_F64> fromPts = new ArrayList<Point3D_F64>();
    List<Point3D_F64> toPts = new ArrayList<Point3D_F64>();

    for( int i = 0; i < 50; i++ ) {
        double x = rand.nextGaussian();
        double y = rand.nextGaussian();
        double z = rand.nextGaussian();

        Point3D_F64 p = new Point3D_F64(x,y,z);
        Point3D_F64 tranP = new Point3D_F64();

        SePointOps_F64.transform(actual,p,tranP);

        fromPts.add(p);
        toPts.add(tranP);
    }

    // Estimate the transform from the point pairs
    MotionTransformPoint<Se3_F64, Point3D_F64> estimator = new MotionSe3PointSVD_F64();

    if(!estimator.process(fromPts,toPts))
        throw new RuntimeException("Estimation of Se3 failed");

    Se3_F64 found = estimator.getTransformSrcToDst();

    // Print out the results and see how it compares to ground truth
    actual.print();
    System.out.println();
    found.print();
}