-
Notifications
You must be signed in to change notification settings - Fork 0
/
camera.py
58 lines (50 loc) · 2.17 KB
/
camera.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
import pybullet as p
import numpy as np
class Camera:
def __init__(self, width=640, height=480, fov=60, near_val=0.01, far_val=10):
self.pose = np.eye(4)
self.focus_distance = 1
self.set_pose(np.eye(4))
self.width = width
self.height = height
self.near_val = near_val
self.far_val = far_val
self.projection_matrix = p.computeProjectionMatrixFOV(fov=fov, # width angle
aspect=self.width / self.height,
nearVal=near_val,
farVal=far_val)
f = height / 2 / np.tan(fov / 2 / 180 * np.pi)
self.intrinsic = np.array([
[f, 0, width / 2],
[0, f, height / 2],
[0, 0, 1]
])
def set_pose(self, pose):
self.pose = pose
self.eye_position = pose[:3, 3]
self.up_vector = -pose[:3, 1]
self.target_position = self.eye_position + pose[:3, 2] * self.focus_distance
p.stepSimulation()
self.view_matrix = p.computeViewMatrix(cameraEyePosition=self.eye_position,
cameraTargetPosition=self.target_position,
cameraUpVector=self.up_vector)
def get_image(self):
p.stepSimulation()
_, _, color, depth, seg = p.getCameraImage(
width=self.width, height=self.height,
viewMatrix=self.view_matrix,
projectionMatrix=self.projection_matrix,
# shadow=False,
# lightDirection=[1,1,1],
# lightDistance=-1,
# lightAmbientCoeff = -1,
# lightSpecularCoeff = -1,
# lightDiffuseCoeff = -1,
renderer=p.ER_BULLET_HARDWARE_OPENGL
# renderer=p.ER_TINY_RENDERER
)
return color, self.far_val * self.near_val / (self.far_val - (self.far_val - self.near_val) * depth), seg
if __name__ == '__main__':
p.connect(p.GUI)
cam = Camera()
print(np.array([np.tan(30/180*np.pi), np.tan(30/180*np.pi)/640*480, 1]) @ cam.intrinsic.T)