72 lines
2.1 KiB
Python
72 lines
2.1 KiB
Python
|
'''
|
|||
|
|
@ Date: 2021-01-25 21:27:56
|
|||
|
|
@ Author: Qing Shuai
|
|||
|
|
@ LastEditors: Qing Shuai
|
|||
|
|
@ LastEditTime: 2021-06-25 15:50:40
|
|||
|
|
@ FilePath: /EasyMocapRelease/easymocap/affinity/plucker.py
|
|||
|
|
'''
|
|||
|
|
import numpy as np
|
|||
|
|
|
|||
|
|
def plucker_from_pl(point, line):
|
|||
|
|
""" construct plucker line from a point and directions
|
|||
|
|
|
|||
|
|
Arguments:
|
|||
|
|
point {tensor} -- N, 3
|
|||
|
|
line {tensor} -- N, 3
|
|||
|
|
"""
|
|||
|
|
norm = np.linalg.norm(line, axis=1, keepdims=True)
|
|||
|
|
lunit = line/norm
|
|||
|
|
moment = np.cross(point, lunit, axis=1)
|
|||
|
|
return lunit, moment
|
|||
|
|
|
|||
|
|
def plucker_from_pp(point1, point2):
|
|||
|
|
line = point2 - point1
|
|||
|
|
return plucker_from_pl(point1, line)
|
|||
|
|
|
|||
|
|
def dist_pl(query_points, line, moment):
|
|||
|
|
moment_q = moment - np.cross(query_points, line)
|
|||
|
|
dist = np.linalg.norm(moment_q, axis=1)
|
|||
|
|
return dist
|
|||
|
|
|
|||
|
|
def reciprocal_product(l1, m1, l2, m2):
|
|||
|
|
l1 = l1[:, None]
|
|||
|
|
m1 = m1[:, None]
|
|||
|
|
l2 = l2[None, :]
|
|||
|
|
m2 = m2[None, :]
|
|||
|
|
product = np.sum(l1*m2, axis=2) + np.sum(l2*m1, axis=2)
|
|||
|
|
return np.abs(product)
|
|||
|
|
|
|||
|
|
def dist_pl_pointwise(p0, p1):
|
|||
|
|
moment_q = p1[..., 3:6] - np.cross(p0[..., :3], p1[..., :3])
|
|||
|
|
dist = np.linalg.norm(moment_q, axis=-1)
|
|||
|
|
return dist
|
|||
|
|
|
|||
|
|
def dist_ll_pointwise(p0, p1):
|
|||
|
|
product = np.sum(p0[..., :3] * p1[..., 3:6], axis=-1) + np.sum(p1[..., :3] * p0[..., 3:6], axis=-1)
|
|||
|
|
return np.abs(product)
|
|||
|
|
|
|||
|
|
def dist_ll_pointwise_conf(p0, p1):
|
|||
|
|
dist = dist_ll_pointwise(p0, p1)
|
|||
|
|
conf = np.sqrt(p0[..., -1] * p1[..., -1])
|
|||
|
|
dist = np.sum(dist*conf, axis=-1)/(1e-5 + conf.sum(axis=-1))
|
|||
|
|
dist[conf.sum(axis=-1)<0.1] = 1e5
|
|||
|
|
return dist
|
|||
|
|
|
|||
|
|
def computeRay(keypoints2d, invK, R, T):
|
|||
|
|
# 将点转为世界坐标系下plucker坐标
|
|||
|
|
# points: (nJoints, 3)
|
|||
|
|
# invK: (3, 3)
|
|||
|
|
# R: (3, 3)
|
|||
|
|
# T: (3, 1)
|
|||
|
|
# cam_center: (3, 1)
|
|||
|
|
if len(keypoints2d.shape) == 3:
|
|||
|
|
keypoints2d = keypoints2d[0]
|
|||
|
|
conf = keypoints2d[..., -1:]
|
|||
|
|
cam_center = - R.T @ T
|
|||
|
|
N = keypoints2d.shape[0]
|
|||
|
|
kp_pixel = np.hstack([keypoints2d[..., :2], np.ones_like(conf)])
|
|||
|
|
kp_all_3d = (kp_pixel @ invK.T - T.T) @ R
|
|||
|
|
l, m = plucker_from_pp(cam_center.T, kp_all_3d)
|
|||
|
|
res = np.hstack((l, m, conf))
|
|||
|
|
# 兼容cpp版本，所以补一个维度
|
|||
|
|
return res[None, :, :]
|