U ɇd @s8ddlZddlZddlmZddZddZddZdS) N)Pathc Cs8td}i}ddddd}t|d}dd d d d d dg}|D]}dt|}i|d|<||D]\}} | d} d} | D]} | tt| } qi} t| d| d<t| d| d<t| d| d<t| d| d<t| d| d<t| d| d<|| | d<| |d|| <qjq@W5QRX|S)Nz cameras.h5Fini)Z54138969Z55011271Z58860488Z60457274zr+ subjectSNameRTcfkpazimuth) rh5pyFilestritemschrintnparray)fileZcam_fileZ cam_paramsrrZsubjectsssub_paramsnamekeyrvalr'7/home/daniela/catkin_ws/src/hpe/scripts/utils/camera.pyload_camera_paramss.   r)cCsJ|jddkst|j}|dj}t|j||}|j|}|S)z Args: pose: numpy array with shape (-1, 3) R: numpy array with shape (3, 3) T: numyp array with shape (3, 1) r*r+)shapeAssertionErrorcopyreshaperrmatmul)poserroriginal_shape pose_worldpose_camr'r'r( world2camera;s  r6cCstt|j|jddkst|j}|dj}t|jt||}t|jt||j|j}||}|S)z Args: pose: numpy array with shape (..., 3) R: numpy array with shape (3, 3) T: numyp array with shape (3, 1) r*r+r,)printr-r.r/r0rrr1)r2rrr3r5Zpose_world_testr4r'r'r( camera2worldJs     r8)rnumpyrpathlibrr)r6r8r'r'r'r(s  6