From f5d7e5bed3e89fd976be9aa631d53822d0725d9a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 10 Mar 2018 19:49:30 -0800 Subject: [PATCH] example for Issue #1586 --- .../examples/pointCloudFromCameraImage.py | 123 ++++++++++++++++++ 1 file changed, 123 insertions(+) create mode 100644 examples/pybullet/examples/pointCloudFromCameraImage.py diff --git a/examples/pybullet/examples/pointCloudFromCameraImage.py b/examples/pybullet/examples/pointCloudFromCameraImage.py new file mode 100644 index 000000000..fee30d8e4 --- /dev/null +++ b/examples/pybullet/examples/pointCloudFromCameraImage.py @@ -0,0 +1,123 @@ + +import pybullet as p +import math +import numpy as np + + + +p.connect(p.GUI) +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=img[2] +depthBuffer = img[3] +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) + + + +