zhigangjiang's picture
no message
88b0dcb
raw
history blame
1.51 kB
"""
@Date: 2021/11/06
@description:
"""
import cv2
import numpy as np
def xyz2json(xyz, ratio, camera_height=1.6):
xyz = xyz * camera_height
ceiling_height = camera_height * ratio
layout_height = camera_height + ceiling_height
data = {
'cameraHeight': camera_height,
'layoutHeight': layout_height,
'cameraCeilingHeight': ceiling_height,
'layoutObj2ds': {
'num': 0,
'obj2ds': []
},
'layoutPoints': {
'num': xyz.shape[0],
'points': []
},
'layoutWalls': {
'num': xyz.shape[0],
'walls': []
}
}
xyz = np.concatenate([xyz, xyz[0:1, :]], axis=0)
R_180 = cv2.Rodrigues(np.array([0, -1 * np.pi, 0], np.float32))[0]
for i in range(xyz.shape[0] - 1):
a = np.dot(R_180, xyz[i, :])
a[0] *= -1
b = np.dot(R_180, xyz[i + 1, :])
b[0] *= -1
c = a.copy()
c[1] = 0
normal = np.cross(a - b, a - c)
normal /= np.linalg.norm(normal)
d = -np.sum(normal * a)
plane = np.asarray([normal[0], normal[1], normal[2], d])
data['layoutPoints']['points'].append({'xyz': a.tolist(), 'id': i})
next_i = 0 if i + 1 >= (xyz.shape[0] - 1) else i + 1
tmp = {
'normal': normal.tolist(),
'planeEquation': plane.tolist(),
'pointsIdx': [i, next_i]
}
data['layoutWalls']['walls'].append(tmp)
return data