python中两个投影向量之间的角度

Question

我正在使用基于this paper的Python在点云中进行圆柱体拟合。我被困在equation 5了。这是关于计算两个投影向量之间的角度

以下是我的实现，包括一个示例：

import numpy as np

p1 = np.asarray([ -6.08021653,  17.9136792 , -14.31057707])
n1 = np.asarray([ -7.05317241,  18.06131922, -14.13292778])
pStar = np.asarray([  5.38806237,  32.68094222, -28.9836691 ])
a = np.asarray([-0.97062874,  0.22803407,  0.07668319])

def getDeltaNormal(pointnormal, pStar, a):
    pi = pointnormal[0:3] #Split Points
    ni = pointnormal[3:] #Split Normals

    diffPPStar = np.subtract(pi,pStar)
    fraq1 = np.cos(np.dot(np.subtract(diffPPStar, np.dot(np.dot(diffPPStar, a), a)), ni))
    fraq2 = np.linalg.norm(np.subtract(diffPPStar, np.dot(np.dot(diffPPStar, a), a)))
    return fraq1 / fraq2

pn = np.hstack((p1, n1))
getDeltaNormal(pn, pStar, a)

使用示例值得到的结果是-0.042 rad（约）。基于三维重建，我预计约为2.92 rad

知道我的臭虫在哪里吗

Answer 1

我得到大约2.3分

我认为这个等式可能有问题。一般来说

| | a |||b | |。cos（t）=a。b

所以

t=arccos（（a.b）/（a | b |）

我不确定在应用余弦后你能得到一个角度，除非你不应该得到一个角度

p1_m_p = p1 - pStar
pDota = pStar.dot( a )

num = (p1_m_p - pDota*a).dot( n1 )
den = np.linalg.norm(p1_m_p - pDota*a) * np.linalg.norm( n1 )
result = np.arccos( num/den )
result