127 lines
4.4 KiB
Python
127 lines
4.4 KiB
Python
import matplotlib.pyplot as plt
|
|
import numpy as np
|
|
import pybullet as p
|
|
import time
|
|
|
|
direct = p.connect(p.GUI) #, options="--window_backend=2 --render_device=0")
|
|
#egl = p.loadPlugin("eglRendererPlugin")
|
|
|
|
p.loadURDF('plane.urdf')
|
|
p.loadURDF("r2d2.urdf", [0, 0, 1])
|
|
p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.0, 0.025])
|
|
cube_trans = p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.1, 0.025])
|
|
p.changeVisualShape(cube_trans, -1, rgbaColor=[1, 1, 1, 0.1])
|
|
width = 128
|
|
height = 128
|
|
|
|
fov = 60
|
|
aspect = width / height
|
|
near = 0.02
|
|
far = 1
|
|
|
|
view_matrix = p.computeViewMatrix([0, 0, 0.5], [0, 0, 0], [1, 0, 0])
|
|
projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far)
|
|
|
|
# Get depth values using the OpenGL renderer
|
|
images = p.getCameraImage(width,
|
|
height,
|
|
view_matrix,
|
|
projection_matrix,
|
|
shadow=True,
|
|
renderer=p.ER_BULLET_HARDWARE_OPENGL)
|
|
rgb_opengl = np.reshape(images[2], (height, width, 4)) * 1. / 255.
|
|
depth_buffer_opengl = np.reshape(images[3], [width, height])
|
|
depth_opengl = far * near / (far - (far - near) * depth_buffer_opengl)
|
|
seg_opengl = np.reshape(images[4], [width, height]) * 1. / 255.
|
|
time.sleep(1)
|
|
|
|
# Get depth values using Tiny renderer
|
|
images = p.getCameraImage(width,
|
|
height,
|
|
view_matrix,
|
|
projection_matrix,
|
|
shadow=True,
|
|
renderer=p.ER_TINY_RENDERER)
|
|
depth_buffer_tiny = np.reshape(images[3], [width, height])
|
|
depth_tiny = far * near / (far - (far - near) * depth_buffer_tiny)
|
|
rgb_tiny = np.reshape(images[2], (height, width, 4)) * 1. / 255.
|
|
seg_tiny = np.reshape(images[4], [width, height]) * 1. / 255.
|
|
|
|
bearStartPos1 = [-3.3, 0, 0]
|
|
bearStartOrientation1 = p.getQuaternionFromEuler([0, 0, 0])
|
|
bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
|
|
bearStartPos2 = [0, 0, 0]
|
|
bearStartOrientation2 = p.getQuaternionFromEuler([0, 0, 0])
|
|
bearId2 = p.loadURDF("teddy_large.urdf", bearStartPos2, bearStartOrientation2)
|
|
textureId = p.loadTexture("checker_grid.jpg")
|
|
for b in range(p.getNumBodies()):
|
|
p.changeVisualShape(b, linkIndex=-1, textureUniqueId=textureId)
|
|
for j in range(p.getNumJoints(b)):
|
|
p.changeVisualShape(b, linkIndex=j, textureUniqueId=textureId)
|
|
|
|
viewMat = [
|
|
0.642787516117096, -0.4393851161003113, 0.6275069713592529, 0.0, 0.766044557094574,
|
|
0.36868777871131897, -0.5265407562255859, 0.0, -0.0, 0.8191521167755127, 0.5735764503479004,
|
|
0.0, 2.384185791015625e-07, 2.384185791015625e-07, -5.000000476837158, 1.0
|
|
]
|
|
projMat = [
|
|
0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0,
|
|
0.0, 0.0, -0.02000020071864128, 0.0
|
|
]
|
|
images = p.getCameraImage(width,
|
|
height,
|
|
viewMatrix=viewMat,
|
|
projectionMatrix=projMat,
|
|
renderer=p.ER_BULLET_HARDWARE_OPENGL,
|
|
flags=p.ER_USE_PROJECTIVE_TEXTURE,
|
|
projectiveTextureView=viewMat,
|
|
projectiveTextureProj=projMat)
|
|
proj_opengl = np.reshape(images[2], (height, width, 4)) * 1. / 255.
|
|
time.sleep(1)
|
|
|
|
images = p.getCameraImage(width,
|
|
height,
|
|
viewMatrix=viewMat,
|
|
projectionMatrix=projMat,
|
|
renderer=p.ER_TINY_RENDERER,
|
|
flags=p.ER_USE_PROJECTIVE_TEXTURE,
|
|
projectiveTextureView=viewMat,
|
|
projectiveTextureProj=projMat)
|
|
proj_tiny = np.reshape(images[2], (height, width, 4)) * 1. / 255.
|
|
|
|
# Plot both images - should show depth values of 0.45 over the cube and 0.5 over the plane
|
|
plt.subplot(4, 2, 1)
|
|
plt.imshow(depth_opengl, cmap='gray', vmin=0, vmax=1)
|
|
plt.title('Depth OpenGL3')
|
|
|
|
plt.subplot(4, 2, 2)
|
|
plt.imshow(depth_tiny, cmap='gray', vmin=0, vmax=1)
|
|
plt.title('Depth TinyRenderer')
|
|
|
|
plt.subplot(4, 2, 3)
|
|
plt.imshow(rgb_opengl)
|
|
plt.title('RGB OpenGL3')
|
|
|
|
plt.subplot(4, 2, 4)
|
|
plt.imshow(rgb_tiny)
|
|
plt.title('RGB Tiny')
|
|
|
|
plt.subplot(4, 2, 5)
|
|
plt.imshow(seg_opengl)
|
|
plt.title('Seg OpenGL3')
|
|
|
|
plt.subplot(4, 2, 6)
|
|
plt.imshow(seg_tiny)
|
|
plt.title('Seg Tiny')
|
|
|
|
plt.subplot(4, 2, 7)
|
|
plt.imshow(proj_opengl)
|
|
plt.title('Proj OpenGL')
|
|
|
|
plt.subplot(4, 2, 8)
|
|
plt.imshow(proj_tiny)
|
|
plt.title('Proj Tiny')
|
|
plt.subplots_adjust(hspace=0.7)
|
|
|
|
plt.show()
|