I want to calculate the Rotation/translation Error between two 4x4 transformation matrices gt_mat and est_mat using the below python code:
def get_angular_error(R_gt, R_est):
import math
"""
Get angular error
"""
try:
A = (np.trace(np.dot(R_gt.T, R_est))-1) / 2.0
if A < -1:
A = -1
if A > 1:
A = 1
rotError = math.fabs(math.acos(A));
return math.degrees(rotError)
except ValueError:
import pdb; pdb.set_trace()
return 99999
def compute_transformation_diff(est_mat, gt_mat):
"""
Compute difference between two 4-by-4 SE3 transformation matrices
"""
R_gt = gt_mat[:3,:3]
R_est = est_mat[:3,:3]
rot_error = get_angular_error(R_gt, R_est)
t_gt = gt_mat[:,-1]
t_est = est_mat[:,-1]
trans_error = np.linalg.norm(t_gt - t_est)
return rot_error, trans_error
My gt-matrix gt_mat is related to a pivot point np.asarray([x,y,z]), which is used to rotate and translate a point cloud, while est_mat is computed by the ICP-Implementation in open3D.
My Question:
Can i directly compare both transformation matrices using compute_transformation_diff to compute the errors? Or do i need to account for the different pivot points? I suspect that the ICP transformation of open3D is relative to the centroid of the point cloud, meaning i get a different translation in est_mat than in gt_mat.
I think the rotation matrices are invariant to translation, meaning I can directly compare the rotation matrices using my get_angular_error function.
For the translation error, considering the different pivot points, I would approach it like this:
gt_mat = np.random.rand(4, 4) # pseudo gt mat
est_mat = np.random.rand(4, 4) # pseudo est mat
gt_pivot = np.array([x1,y1,z1]) # pivot point of gt mat
est_pivot = np.array([x2,y2,z2]) # pivot point of icp (=centroid of pcd?)
R_gt = gt_mat[:3,:3]
R_est = est_mat[:3,:3]
t_gt = gt_mat[:3, 3]
t_est = est_mat[:3, 3]
t_gt_adjusted = t_gt - gt_pivot + np.dot(R_gt, gt_pivot)
t_est_adjusted = t_est - est_pivot + np.dot(R_est, est_pivot)
trans_error = np.linalg.norm(t_gt_adjusted - t_est_adjusted)
print(trans_error)
However, I'm not entirely sure if my thought process is correct, and whether this is a valid way to calculate the rotation/translation error. Is there a way/code to validate this approach, or any references/papers related to this problem? I would really appreciate any guidance, as I am not very familiar with this subject. And finally: is the pivot point for icp the centroid of the point cloud?
Can i directly compare both transformation matrices using compute_transformation_diff to compute the errors?
Your metric for rotation only takes into account the angle of rotation. The distance between two rotations must also take into account the change of axis. According to [1], a suitable metric for rotations in matrix form is the simple froben distance between the two rotation matrices dist_R = np.linalg.norm(R_a - R_b, 'fro'). This metric returns a value between 0 and 2*(2**0.5). Dividing the result by this will always give you a value between [0,1].
[1] HUYNH, Du Q. Metrics for 3D rotations: Comparison and analysis. Journal of Mathematical Imaging and Vision, v. 35, p. 155-164, 2009
I suspect that the ICP transformation of open3D is relative to the centroid of the point cloud, meaning i get a different translation in est_mat than in gt_mat.
No, ICP in Open3D do not centralize the cloud. I made an animation of the GICP using a voxel multiscale aproach that you can see here: https://www.youtube.com/watch?v=08CoufK_Kqs. In this video one cloud remains static (target) while the other is registered to it (source). The source cloud is densified every 50 iterations.
When ICP calculates a transformation, it applies it to the source cloud in relation to the coordinate system of the target cloud. If the target cloud is centered, then the ICP pivot will be the center of mass of the target cloud. One way to check this is to try to register two clouds that don't overlap. Simply apply a translation to one of the clouds in the pair so that the minimum distance between their points is more than 1m. Then try to register one cloud on the other using a maximum search distance for matches of less than 1 m, say 0.5 meters. ICP will give you an error and say that it hasn't found any matches. If it centered the clouds, this would almost never happen, as they would intersect somewhere. The ICP is implemented like this because it is only for refinement, it normally receives the output of a global registration algorithm that already brings the two clouds closer together so that they “touch” each other