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
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
|
import pybullet as p
import math
import numpy as np
import pybullet_data
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
plane = p.loadURDF("plane100.urdf")
cube = p.loadURDF("cube.urdf", [0, 0, 1])
def getRayFromTo(mouseX, mouseY):
width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
)
camPos = [
camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
camTarget[2] - dist * camForward[2]
]
farPlane = 10000
rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
lenFwd = math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] * rayForward[1] +
rayForward[2] * rayForward[2])
invLen = farPlane * 1. / lenFwd
rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
rayFrom = camPos
oneOverWidth = float(1) / float(width)
oneOverHeight = float(1) / float(height)
dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
rayToCenter = [
rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[2]
]
ortho = [
-0.5 * horizon[0] + 0.5 * vertical[0] + float(mouseX) * dHor[0] - float(mouseY) * dVer[0],
-0.5 * horizon[1] + 0.5 * vertical[1] + float(mouseX) * dHor[1] - float(mouseY) * dVer[1],
-0.5 * horizon[2] + 0.5 * vertical[2] + float(mouseX) * dHor[2] - float(mouseY) * dVer[2]
]
rayTo = [
rayFrom[0] + rayForward[0] + ortho[0], rayFrom[1] + rayForward[1] + ortho[1],
rayFrom[2] + rayForward[2] + ortho[2]
]
lenOrtho = math.sqrt(ortho[0] * ortho[0] + ortho[1] * ortho[1] + ortho[2] * ortho[2])
alpha = math.atan(lenOrtho / farPlane)
return rayFrom, rayTo, alpha
width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
)
camPos = [
camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
camTarget[2] - dist * camForward[2]
]
farPlane = 10000
rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
lenFwd = math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] * rayForward[1] +
rayForward[2] * rayForward[2])
oneOverWidth = float(1) / float(width)
oneOverHeight = float(1) / float(height)
dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
lendHor = math.sqrt(dHor[0] * dHor[0] + dHor[1] * dHor[1] + dHor[2] * dHor[2])
lendVer = math.sqrt(dVer[0] * dVer[0] + dVer[1] * dVer[1] + dVer[2] * dVer[2])
cornersX = [0, width, width, 0]
cornersY = [0, 0, height, height]
corners3D = []
imgW = int(width / 10)
imgH = int(height / 10)
img = p.getCameraImage(imgW, imgH, renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgbBuffer = np.reshape(img[2], (imgH, imgW, 4))
# NOTE: this depth buffer's reshaping does not match the [w, h] convention for
# OpenGL depth buffers. See getCameraImageTest.py for an OpenGL depth buffer
depthBuffer = np.reshape(img[3], [imgH, imgW])
print("rgbBuffer.shape=", rgbBuffer.shape)
print("depthBuffer.shape=", depthBuffer.shape)
#disable rendering temporary makes adding objects faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_SPHERE, rgbaColor=[1, 1, 1, 1], radius=0.03)
collisionShapeId = -1 #p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale)
for i in range(4):
w = cornersX[i]
h = cornersY[i]
rayFrom, rayTo, _ = getRayFromTo(w, h)
rf = np.array(rayFrom)
rt = np.array(rayTo)
vec = rt - rf
l = np.sqrt(np.dot(vec, vec))
newTo = (0.01 / l) * vec + rf
#print("len vec=",np.sqrt(np.dot(vec,vec)))
p.addUserDebugLine(rayFrom, newTo, [1, 0, 0])
corners3D.append(newTo)
count = 0
stepX = 5
stepY = 5
for w in range(0, imgW, stepX):
for h in range(0, imgH, stepY):
count += 1
if ((count % 100) == 0):
print(count, "out of ", imgW * imgH / (stepX * stepY))
rayFrom, rayTo, alpha = getRayFromTo(w * (width / imgW), h * (height / imgH))
rf = np.array(rayFrom)
rt = np.array(rayTo)
vec = rt - rf
l = np.sqrt(np.dot(vec, vec))
depthImg = float(depthBuffer[h, w])
far = 1000.
near = 0.01
depth = far * near / (far - (far - near) * depthImg)
depth /= math.cos(alpha)
newTo = (depth / l) * vec + rf
p.addUserDebugLine(rayFrom, newTo, [1, 0, 0])
mb = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=newTo,
useMaximalCoordinates=True)
color = rgbBuffer[h, w]
color = [color[0] / 255., color[1] / 255., color[2] / 255., 1]
p.changeVisualShape(mb, -1, rgbaColor=color)
p.addUserDebugLine(corners3D[0], corners3D[1], [1, 0, 0])
p.addUserDebugLine(corners3D[1], corners3D[2], [1, 0, 0])
p.addUserDebugLine(corners3D[2], corners3D[3], [1, 0, 0])
p.addUserDebugLine(corners3D[3], corners3D[0], [1, 0, 0])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
print("ready\n")
#p.removeBody(plane)
#p.removeBody(cube)
while (1):
p.setGravity(0, 0, -10)
|
