xyz2uvd.py

#!/usr/bin/env python
# -*- coding:UTF-8 -*-
# File Name : xyz2uvd.py
# Creation Date : 26-07-2018
# Created By : Jeasine Ma [jeasinema[at]gmail[dot]com]

from pyquaternion import Quaternion
import numpy as np 


def xyz2uvd(quat, bias, intrinsic, p_xyz):
    """
    quat: (4) x, y, z, w
    bias: (3) x, y, z
    intrinsic: (3*3)
    p_xyz: (3)
    """
    x, y, z, w = quat 
    quat = Quaternion(w, x, y, z)
    x, y, z = bias 
    px, py, pz = p_xyz 
    # quat.rotate just rotates, but returns result in the same coordiante
    cam_p = quat.rotation_matrix.T @ (np.array([px, py, pz]) - np.array([x, y, z]))
    print(cam_p)
    cam_p = np.array([-cam_p[1], -cam_p[2], cam_p[0]])
    tmp = intrinsic @ cam_p
    uvd = tmp/tmp[2]
    uvd[2] = tmp[2]
    return uvd
    

def main():
    # orientation: x,y,z,w in /gazebo/model_state
    quat = np.array([0.088, 0.088, -0.701, 0.702])
    # position: x,y,z in /gazebo/model_state
    bias = np.array([0, 1, 1.6])
    # get from /camera/depth/camera_info
    intrinsic = np.array([[554.255, 0, 320.5],[0, 554.25, 240.5],[0,0,1]])
    uvd = xyz2uvd(quat, bias, intrinsic, np.array([0,-0.7, 0.964]))
    print(uvd)

if __name__ == '__main__':
    main()