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();
   }