Transform FittingΒΆ
Demonstrate how to estimate rigid body motion of a point cloud
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 }