Transform FittingΒΆ

Demonstrate how to estimate rigid body motion of a point cloud

ExampleTransformFitting.java

 1    public static void main(String[] args) {
 2        Random rand = new Random(234);
 3 
 4        // Create a transform which will be applied to the point cloud
 5        Se3_F64 actual = new Se3_F64();
 6        ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ,0.5,-2,0.15,actual.R);
 7        actual.T.setTo(2,3,-2);
 8 
 9        // Create a random point cloud and transform it
10        List<Point3D_F64> fromPts = new ArrayList<Point3D_F64>();
11        List<Point3D_F64> toPts = new ArrayList<Point3D_F64>();
12 
13        for( int i = 0; i < 50; i++ ) {
14            double x = rand.nextGaussian();
15            double y = rand.nextGaussian();
16            double z = rand.nextGaussian();
17 
18            Point3D_F64 p = new Point3D_F64(x,y,z);
19            Point3D_F64 tranP = new Point3D_F64();
20 
21            SePointOps_F64.transform(actual,p,tranP);
22 
23            fromPts.add(p);
24            toPts.add(tranP);
25        }
26 
27        // Estimate the transform from the point pairs
28        MotionTransformPoint<Se3_F64, Point3D_F64> estimator = new MotionSe3PointSVD_F64();
29 
30        if(!estimator.process(fromPts,toPts))
31            throw new RuntimeException("Estimation of Se3 failed");
32 
33        Se3_F64 found = estimator.getTransformSrcToDst();
34 
35        // Print out the results and see how it compares to ground truth
36        actual.print();
37        System.out.println();
38        found.print();
39    }