[Python] How to compare homogeneous 4x4 transformation matrices: GT matrix (with pivot point)...

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?

Continue reading...