Как сравнить однородные матрицы преобразования 4x4: матрица GT (с точкой поворота) и матрица ICP (с центроидом) ⇐ Python

[Anonymous]

Я хочу вычислить ошибку вращения/перевода между двумя матрицами преобразования 4x4 gt_mat и est_mat, используя приведенный ниже код Python:

Код: Выделить всё

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

Моя gt-матрица gt_mat связана с точкой поворота np.asarray([x,y,z]), которая используется для вращения и перемещения облако точек, а est_mat вычисляется реализацией ICP в open3D.
Мой вопрос:
Могу ли я напрямую сравнить оба матрицы преобразования с использованием Compute_transformation_diff для вычисления ошибок? Или мне нужно учитывать различные точки поворота? Я подозреваю, что ICP-преобразование open3D происходит относительно центроида облака точек, а это означает, что я получаю другой перевод в est_mat, чем в gt_mat.
Я думаю, что матрицы вращения инвариантны к переводу, то есть я могу напрямую сравнивать матрицы вращения, используя мою функцию get_angular_error.
Что касается ошибки перевода, учитывая различные точки поворота, я бы подойдите к этому так:

Код: Выделить всё

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)

Однако я не совсем уверен, верен ли мой мыслительный процесс и является ли это допустимым способом расчета ошибки вращения/переноса. Есть ли способ/код для проверки этого подхода или какие-либо ссылки/документы, связанные с этой проблемой? Буду очень признателен за любые советы, так как я не очень знаком с этой темой. И наконец: является ли точка поворота icp центром тяжести облака точек?

Подробнее здесь: https://stackoverflow.com/questions/789 ... h-pivot-po