metrics.py

import numpy as np



def quaternion_from_matrix(matrix, isprecise=False):
    '''Return quaternion from rotation matrix.

    If isprecise is True, the input matrix is assumed to be a precise rotation
    matrix and a faster algorithm is used.

    >>> q = quaternion_from_matrix(numpy.identity(4), True)
    >>> numpy.allclose(q, [1, 0, 0, 0])
    True
    >>> q = quaternion_from_matrix(numpy.diag([1, -1, -1, 1]))
    >>> numpy.allclose(q, [0, 1, 0, 0]) or numpy.allclose(q, [0, -1, 0, 0])
    True
    >>> R = rotation_matrix(0.123, (1, 2, 3))
    >>> q = quaternion_from_matrix(R, True)
    >>> numpy.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786])
    True
    >>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0],
    ...      [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]]
    >>> q = quaternion_from_matrix(R)
    >>> numpy.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611])
    True
    >>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0],
    ...      [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]]
    >>> q = quaternion_from_matrix(R)
    >>> numpy.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603])
    True
    >>> R = random_rotation_matrix()
    >>> q = quaternion_from_matrix(R)
    >>> is_same_transform(R, quaternion_matrix(q))
    True
    >>> R = euler_matrix(0.0, 0.0, numpy.pi/2.0)
    >>> numpy.allclose(quaternion_from_matrix(R, isprecise=False),
    ...                quaternion_from_matrix(R, isprecise=True))
    True

    '''

    M = np.array(matrix, dtype=np.float64, copy=False)[:4, :4]
    if isprecise:
        q = np.empty((4, ))
        t = np.trace(M)
        if t > M[3, 3]:
            q[0] = t
            q[3] = M[1, 0] - M[0, 1]
            q[2] = M[0, 2] - M[2, 0]
            q[1] = M[2, 1] - M[1, 2]
        else:
            i, j, k = 1, 2, 3
            if M[1, 1] > M[0, 0]:
                i, j, k = 2, 3, 1
            if M[2, 2] > M[i, i]:
                i, j, k = 3, 1, 2
            t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3]
            q[i] = t
            q[j] = M[i, j] + M[j, i]
            q[k] = M[k, i] + M[i, k]
            q[3] = M[k, j] - M[j, k]
        q *= 0.5 / math.sqrt(t * M[3, 3])
    else:
        m00 = M[0, 0]
        m01 = M[0, 1]
        m02 = M[0, 2]
        m10 = M[1, 0]
        m11 = M[1, 1]
        m12 = M[1, 2]
        m20 = M[2, 0]
        m21 = M[2, 1]
        m22 = M[2, 2]

        # symmetric matrix K
        K = np.array([[m00 - m11 - m22, 0.0, 0.0, 0.0],
                      [m01 + m10, m11 - m00 - m22, 0.0, 0.0],
                      [m02 + m20, m12 + m21, m22 - m00 - m11, 0.0],
                      [m21 - m12, m02 - m20, m10 - m01, m00 + m11 + m22]])
        K /= 3.0

        # quaternion is eigenvector of K that corresponds to largest eigenvalue
        w, V = np.linalg.eigh(K)
        q = V[[3, 0, 1, 2], np.argmax(w)]

    if q[0] < 0.0:
        np.negative(q, q)

    return q

def angle_error_mat(R1, R2):
    cos = (np.trace(np.dot(R1.T, R2)) - 1) / 2
    cos = np.clip(cos, -1., 1.)  # numerical errors can make it out of bounds
    return np.rad2deg(np.abs(np.arccos(cos)))

def angle_error_vec(v1, v2):
    n = np.linalg.norm(v1) * np.linalg.norm(v2)
    return np.rad2deg(np.arccos(np.clip(np.dot(v1, v2) / n, -1.0, 1.0)))

def evaluate_R_t_v2(R_gt, t_gt, R, t):
    error_t = angle_error_vec(t, t_gt)
    error_t = np.minimum(error_t, 180 - error_t) 
    error_R = angle_error_mat(R, R_gt)
    return error_R, error_t

def evaluate_R_t(R_gt, t_gt, R, t):
    t = t.flatten()
    t_gt = t_gt.flatten()

    eps = 1e-15

    q_gt = quaternion_from_matrix(R_gt)
    q = quaternion_from_matrix(R)
    q = q / (np.linalg.norm(q) + eps)
    q_gt = q_gt / (np.linalg.norm(q_gt) + eps)
    loss_q = np.maximum(eps, (1.0 - np.sum(q * q_gt)**2))
    err_q = np.arccos(1 - 2 * loss_q)

    t = t / (np.linalg.norm(t) + eps)
    t_gt = t_gt / (np.linalg.norm(t_gt) + eps)
    loss_t = np.maximum(eps, (1.0 - np.sum(t * t_gt)**2))
    err_t = np.arccos(np.sqrt(1 - loss_t))

    if np.sum(np.isnan(err_q)) or np.sum(np.isnan(err_t)):
        # This should never happen! Debug here
        import IPython
        IPython.embed()

    return np.rad2deg(np.abs(err_q)), np.rad2deg(np.abs(err_t))

def pose_auc(errors, thresholds):
    sort_idx = np.argsort(errors)
    errors = np.array(errors.copy())[sort_idx]#
    # errors=np.arange(0,0.01,0.0001)
    recall = (np.arange(len(errors)) + 1) / len(errors)
    errors = np.r_[0., errors]
    recall = np.r_[0., recall]
    aucs = []
    for t in thresholds:
        last_index = np.searchsorted(errors, t)
        # last_index=last_index//3
        r = np.r_[recall[:last_index], recall[last_index-1]]#err小于t的比例
        e = np.r_[errors[:last_index], t]#小于err<t的数量的所有err
        aucs.append(np.trapz(r, x=e)/t)
    return aucs

def compute_map(errors, thresh=5, interval=1):
    ints = np.arange(0, thresh + interval, interval)
    hist_acc = np.histogram(errors, ints)[0]
    hist_acc = hist_acc / (errors.shape[0])
    cum_acc = np.cumsum(hist_acc)
    return np.mean(cum_acc)


def pose_metrics(Rt_err,pr_re_f1_list=None):
    Rt_err = np.asarray(Rt_err)
    Rt_err_adj = np.max(Rt_err,1)
    true_auc = pose_auc(Rt_err_adj, [5, 10, 20])
    names = ['auc_5','auc_10','auc_20']
    vals = [true_auc[0], true_auc[1], true_auc[2]]

    Rt_err_adj = Rt_err[:,0]
    true_auc = pose_auc(Rt_err_adj, [5, 10, 20])
    names+= ['auc_5r','auc_10r','auc_20r']
    vals+= [true_auc[0], true_auc[1], true_auc[2]]

    
    Rt_err_adj = Rt_err[:,1]
    true_auc = pose_auc(Rt_err_adj, [5, 10, 20])
    names+= ['auc_5t','auc_10t','auc_20t']
    vals+= [true_auc[0], true_auc[1], true_auc[2]]


    


    if pr_re_f1_list is not None and len(pr_re_f1_list)>0:
        precision = np.mean(pr_re_f1_list[:, 0])
        recall = np.mean(pr_re_f1_list[:, 1])
        f1 = np.mean(pr_re_f1_list[:, 2])
        vals+=[precision, recall, f1]
        names+=['precision', 'recall', 'f1']
    msg = ''
    for n, v in zip(names, vals):
        msg += f'{n} {v:.5f} '
    return msg