(one_pose, first_pose)
return Tmat, pose
return the pose mat in kitti ground-truth and the pose needed for next segment
return the pose mat in kitti ground-truth and the pose needed for next segment
return the pose mat in kitti ground - truth and the pose needed for next segment
def decode_pose(one_pose, first_pose):
    """return the pose mat in kitti ground-truth and the pose needed for next segment"""
    _, tx, ty, tz, qx, qy, qz, qw = one_pose.strip().split()
    rot = quat2mat([float(qw), float(qx), float(qy), float(qz)])
    rz, ry, rx = mat2euler(rot)
    pose_inv = pose_vec2mat([tx, ty, tz, rx, ry, rz], True).astype(np.float32)
    pose = np.dot(np.linalg.inv(pose_inv), first_pose)

    rot = pose[:3,:3].transpose().astype(float)
    trans = -np.dot(rot, pose[:3,3].transpose())
    Tmat = np.concatenate((rot, trans.reshape(3,1)), axis=1)
    return Tmat, pose
def decode_pose ( one_pose , first_pose ) : _ , tx , ty , tz , qx , qy , qz , qw = one_pose . strip ( ) . split ( ) rot = quat2mat ( [ float ( qw ) , float ( qx ) , float ( qy ) , float ( qz ) ] ) rz , ry , rx = mat2euler ( rot ) pose_inv = pose_vec2mat ( [ tx , ty , tz , rx , ry , rz ] , True ) . astype ( np . float32 ) pose = np . dot ( np . linalg . inv ( pose_inv ) , first_pose ) rot = pose [ : 3 , : 3 ] . transpose ( ) . astype ( float ) trans = - np . dot ( rot , pose [ : 3 , 3 ] . transpose ( ) ) Tmat = np . concatenate ( ( rot , trans . reshape ( 3 , 1 ) ) , axis = 1 ) return Tmat , pose