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()