Skip to content

Commit

Permalink
add Project and deproject method in camera
Browse files Browse the repository at this point in the history
lukashermann committed Jan 6, 2022
1 parent bd28576 commit 9b43fb9
Showing 3 changed files with 52 additions and 5 deletions.
47 changes: 47 additions & 0 deletions calvin_env/camera/camera.py
Original file line number Diff line number Diff line change
@@ -42,3 +42,50 @@ def process_rgbd(self, obs, nearval, farval):
depth_buffer = np.reshape(depthPixels, [height, width])
depth = self.z_buffer_to_real_distance(z_buffer=depth_buffer, far=farval, near=nearval)
return rgb_img, depth

# Reference: world2pixel
# https://github.com/bulletphysics/bullet3/issues/1952
def project(self, point):
'''
Projects a world point in homogeneous coordinates to pixel coordinates
Args
point: np.array of len 4; indicates the desired point to project
Output
(x, y): tuple (u, v); pixel coordinates of the projected point
'''

# reshape to get homogeneus transform
persp_m = np.array(self.projectionMatrix).reshape((4, 4)).T
view_m = np.array(self.viewMatrix).reshape((4, 4)).T

# Perspective proj matrix
world_pix_tran = persp_m @ view_m @ point
world_pix_tran = world_pix_tran / world_pix_tran[-1] # divide by w
world_pix_tran[:3] = (world_pix_tran[:3] + 1) / 2
x, y = world_pix_tran[0] * self.width, (1 - world_pix_tran[1]) * self.height
x, y = np.floor(x).astype(int), np.floor(y).astype(int)
return (x, y)

def deproject(self, point, depth_img, homogeneous=False):
'''
Deprojects a pixel point to 3D coordinates
Args
point: tuple (u, v); pixel coordinates of point to deproject
depth_img: np.array; depth image used as reference to generate 3D coordinates
homogeneous: bool; if true it returns the 3D point in homogeneous coordinates,
else returns the world coordinates (x, y, z) position
Output
(x, y): np.array; world coordinates of the deprojected point
'''
T_world_cam = np.linalg.inv(np.array(self.viewMatrix).reshape((4, 4)).T)

u, v = point
z = depth_img[v, u]
foc = self.height / (2 * np.tan(np.deg2rad(self.fov) / 2))
x = (u - self.width // 2) * z / foc
y = -(v - self.height // 2) * z / foc
z = -z
world_pos = (T_world_cam @ np.array([x, y, z, 1]))
if(not homogeneous):
world_pos = world_pos[:3]
return world_pos
8 changes: 4 additions & 4 deletions calvin_env/camera/gripper_camera.py
Original file line number Diff line number Diff line change
@@ -31,15 +31,15 @@ def render(self):
cam_rot = np.array(cam_rot).reshape(3, 3)
cam_rot_y, cam_rot_z = cam_rot[:, 1], cam_rot[:, 2]
# camera: eye position, target position, up vector
view_matrix = p.computeViewMatrix(camera_pos, camera_pos + cam_rot_y, -cam_rot_z)
projection_matrix = p.computeProjectionMatrixFOV(
self.view_matrix = p.computeViewMatrix(camera_pos, camera_pos + cam_rot_y, -cam_rot_z)
self.projection_matrix = p.computeProjectionMatrixFOV(
fov=self.fov, aspect=self.aspect, nearVal=self.nearval, farVal=self.farval
)
image = p.getCameraImage(
width=self.width,
height=self.height,
viewMatrix=view_matrix,
projectionMatrix=projection_matrix,
viewMatrix=self.view_matrix,
projectionMatrix=self.projection_matrix,
physicsClientId=self.cid,
)
rgb_img, depth_img = self.process_rgbd(image, self.nearval, self.farval)
2 changes: 1 addition & 1 deletion egl_check/EGL_options.cpp
Original file line number Diff line number Diff line change
@@ -277,7 +277,7 @@ void EGLOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)

eglQueryDeviceAttribEXT(egl_devices[m_data->m_renderDevice], EGL_CUDA_DEVICE_NV, &cudaIndex);
if (eglGetError() == EGL_SUCCESS) {
printf("CUDA_DEVICE=%d\n" ,cudaIndex);
printf("CUDA_DEVICE=%d\n" , (int) cudaIndex);
}
const GLubyte* ren = glGetString(GL_RENDERER);
printf("GL_RENDERER=%s\n", ren);

0 comments on commit 9b43fb9

Please sign in to comment.