def convert(self, boneTable):
parentName = self.parent if self.parent else ""
parentIndex = boneTable.get(self.parent, -1)
matflat = [float(x) for x in self.bindmat.split(' ')]
mat = np.array([matflat[0:3], matflat[3:6], matflat[6:9]])
q = quaternions.mat2quat(mat)
(px, py, pz) = [formatValue(v) for v in self.bindpos.split(' ')]
(qw, qx, qy, qz) = [formatValue(v) for v in q]
return f'\t"{self.name}"\t{parentIndex} ( {px} {py} {pz} ) ( {qx} {qy} {qz} )\t\t// {parentName}'
def infTwist(xyz):
pts = xyz.T
ors = normr(pts[1:] - pts[:-1])
start2end_rope = eye(3)
for (or1, or2) in zip(ors[:-1], ors[1:]):
start2end_rope = dot(minRot(or2, or1), start2end_rope)
assert almostEq(dot(start2end_rope, ors[0]), ors[-1])
end2start_min = minRot(ors[0], ors[-1])
twist_mat = dot(end2start_min, start2end_rope)
ang, _ = quat2angle_axis(mat2quat(twist_mat))
return ang
def infTwist(xyz):
pts = xyz.T
ors = normr(pts[1:] - pts[:-1])
start2end_rope = eye(3)
for (or1,or2) in zip(ors[:-1],ors[1:]):
start2end_rope = dot(minRot(or2,or1),start2end_rope)
assert almostEq(dot(start2end_rope,ors[0]),ors[-1])
end2start_min = minRot(ors[0],ors[-1])
twist_mat = dot(end2start_min,start2end_rope)
ang,_ = quat2angle_axis(mat2quat(twist_mat))
return ang
def computeTransformationErr(trans, info):
"""
Computer the transformation error as an approximation of the RMSE of corresponding points.
More informaiton at http://redwood-data.org/indoor/registration.html
Args:
trans (numpy array): transformation matrices [n,4,4]
info (numpy array): covariance matrices of the gt transformation paramaters [n,4,4]
Returns:
p (float): transformation error
"""
t = trans[:3, 3]
r = trans[:3, :3]
q = nq.mat2quat(r)
er = np.concatenate([t, q[1:]], axis=0)
p = er.reshape(1, 6) @ info @ er.reshape(6, 1) / info[0, 0]
return p.item()