Python example: mrpt_tfest_example.py
Robust SE(3) transformation estimation from point correspondences with mrpt.tfest.
Modules: mrpt.math, mrpt.poses, mrpt.tfest
1#!/usr/bin/env python3
2"""
3Robust SE(3) transformation estimation from point correspondences with mrpt.tfest.
4
5Demonstrates robust SE(3) alignment between two sets of point correspondences.
6"""
7
8import numpy as np
9import mrpt.math
10import mrpt.poses
11import mrpt.tfest
12
13
14def create_correspondences(local_pts, global_pts):
15 """
16 Convenience function to create a TMatchingPairList from two Nx3 numpy arrays.
17 """
18 import numpy as np
19 pair_list = mrpt.tfest.TMatchingPairList()
20 for i in range(len(local_pts)):
21 p = mrpt.tfest.TMatchingPair()
22 p.global_pt = mrpt.math.TPoint3Df(global_pts[i])
23 p.local_pt = mrpt.math.TPoint3Df(local_pts[i])
24 p.globalIdx = i
25 p.localIdx = i
26 pair_list.push_back(p)
27 return pair_list
28
29
30def main():
31 # 1. Generate Synthetic Data
32 # ---------------------------------------------------------
33 print("Generating synthetic point clouds...")
34 num_points = 50
35 # Create random points in 'this' frame
36 pts_this = np.random.uniform(-5, 5, (num_points, 3))
37
38 # Define a ground truth transformation (Translation + Yaw rotation)
39 gt_pose = mrpt.poses.CPose3D(1.5, -0.8, 0.2, np.radians(30), 0, 0)
40
41 # Transform points to 'other' frame: p_this = pose (+) p_other
42 # Since we want p_other, we apply the inverse transformation
43 inv_gt = gt_pose.getInverseHomogeneousMatrix() # returns 4x4 numpy-compatible
44
45 # Manual transformation for the example:
46 pts_other = []
47 for p in pts_this:
48 # p_other = gt_pose.inverseComposePoint(p)
49 p_other = gt_pose.inverseComposePoint(p[0], p[1], p[2])
50 pts_other.append([p_other.x, p_other.y, p_other.z])
51 pts_other = np.array(pts_other)
52
53 # Add some outliers to test RANSAC robustness
54 pts_other[0] += [10.0, 10.0, 10.0]
55 pts_other[1] += [-5.0, 20.0, 0.0]
56
57 # 2. Create Correspondence List
58 # ---------------------------------------------------------
59 # We use the convenience helper from our tfest/__init__.py
60 print(f"Creating {num_points} correspondences...")
61 corr_list = create_correspondences(pts_this, pts_other)
62
63 # 3. Configure and Run Robust SE(3) Estimation
64 # ---------------------------------------------------------
65 print("Running Robust SE(3) L2 (RANSAC)...")
66
67 params = mrpt.tfest.TSE3RobustParams()
68 params.ransac_minSetSize = 5
69 params.ransac_nmaxSimulations = 200
70 # params.ransac_mahalanobisDistanceThreshold = 0.05
71
72 # Run estimator: returns (success, results_struct)
73 ok, results = mrpt.tfest.se3_l2_robust(corr_list, params)
74
75 if ok:
76 print("\nEstimation Successful!")
77 print("-" * 30)
78 # The transformation is a CPose3DQuat object
79 print(f"Estimated Pose: {results.transformation}")
80 print(f"Ground Truth: {gt_pose}")
81
82 # Check inliers
83 inliers = results.inliers_idx
84 print(f"Inliers found: {len(inliers)} / {num_points}")
85
86 # 4. Use the result with NumPy
87 # ---------------------------------------------------------
88 # results.transformation is already a CPose3D (converted in the binding)
89 final_pose = results.transformation
90 rot_mat = np.array(final_pose.getRotationMatrix().as_numpy())
91
92 print("\nRotation Matrix (NumPy):")
93 print(rot_mat)
94 else:
95 print("Estimation failed!")
96
97
98if __name__ == "__main__":
99 main()