i am using pyBullet, which is python wrapper to bullet3 physics engine and i need to create point cloud from virtual camera.
This engine uses basic OpenGL renderer and i am able to get values from OpenGL depth buffer
img = p.getCameraImage(imgW, imgH, renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgbBuffer = img[2]
depthBuffer = img[3]
Now i have width*height array with depth values. How can i get world coordinates from this? i tried to save .ply point cloud with points (width, height, depthBuffer(width, height)) but this doesn't create point cloud that looks like objects on the scene.
I also tried to correct depth with near far plane:
depthImg = float(depthBuffer[h, w])
far = 1000.
near = 0.01
depth = far * near / (far - (far - near) * depthImg)
but result with this was also some weird point cloud. How can i create realistic point cloud from data from depth buffer? is it even possible?
i did something similar in c++, but there i used glm::unproject
for (size_t i = 0; i < height; i = i = i+density) {
for (size_t j = 0; j < width; j = j = j+density) {
glm::vec3 win(i, j, depth);
glm::vec4 position(glm::unProject(win, identity, projection, viewport), 0.0);
EDIT:
based on Rabbid76 answer i used PyGLM which worked, i am now able to obtain XYZ world coordinates to create point cloud, but depth values in point cloud look distorted, am i getting depth from depth buffer correctly?
for h in range(0, imgH, stepX):
for w in range(0, imgW, stepY):
depthImg = float(np.array(depthBuffer)[h, w])
far = 1000.
near = 0.01
depth = far * near / (far - (far - near) * depthImg)
win = glm.vec3(h, w, depthBuffer[h][w])
position = glm.unProject(win, model, projGLM, viewport)
f.write(str(position[0]) + " " + str(position[1]) + " " + str(depth) + "\n")
Here is my solution. We just need to know how the view Matrix and the projection matrix work. There are computeProjectionMatrixFOV and computeViewMatrix funtions in pybullet.
http://www.songho.ca/opengl/gl_projectionmatrix.html and http://ksimek.github.io/2012/08/22/extrinsic/
In a word, point_in_world = inv(projection_matrix * viewMatrix) * NDC_pos
glm.unProject is an another solution
stepX = 10
stepY = 10
pointCloud = np.empty([np.int(img_height/stepY), np.int(img_width/stepX), 4])
projectionMatrix = np.asarray(projection_matrix).reshape([4,4],order='F')
viewMatrix = np.asarray(view_matrix).reshape([4,4],order='F')
tran_pix_world = np.linalg.inv(np.matmul(projectionMatrix, viewMatrix))
for h in range(0, img_height, stepY):
for w in range(0, img_width, stepX):
x = (2*w - img_width)/img_width
y = -(2*h - img_height)/img_height # be careful！ deepth and its corresponding position
z = 2*depth_np_arr[h,w] - 1
pixPos = np.asarray([x, y, z, 1])
position = np.matmul(tran_pix_world, pixPos)
pointCloud[np.int(h/stepY),np.int(w/stepX),:] = position / position[3]
Related
What i want to do is visualize the data streamed from my xbox360 kinect in real time.
Essentially I am creating a point cloud from the data and then visualizing it. The code I have is working but it is really slow.
Basically this code adds a point cloud once it is received from the kinect. Whenever another point cloud is recieved, the previous one is removed and a new one is added.
Is there a better way to do this? Something that is much more responsive with higher frame rates
mat = rendering.Material()
mat.base_color = [
0,
0,
0, 1.0
]
mat.shader = "defaultLit"
pcl = o3d.geometry.PointCloud()
# This line recieves the data from the kinect in the format [x,y,z,r,g,b]
pcl.points = o3d.utility.Vector3dVector(kinect.streamCloud())
self.scene.scene.remove_geometry("kinect")
self.scene.scene.add_geometry("kinect", pcl, mat)
This is the code for streaming data from the kinect
def streamCloud():
depth = freenect.sync_get_depth()
pcl = np.zeros(shape=(307200,3))
c = 0
for i in range(480):
for j in range(640):
z = depth[0][i,j]
#z = 1.0 / (d[i,j] * -0.0030711016 + 3.3309495161)
#z = depth[0][i,j].astype(np.uint8)
#x = (i - cx) * z / fx
x = j
y = i
#y = (j - cy) * z / fy
pcl[c] = [x,y,z]
c = c+1
return pcl
Kinect generates about 300.000 points in every frame, too much data to draw. At 30 FPS this is 9.000.000 points in one second. The first thing you can do is downsample the cloud, you can use cloud.uniform_down_sample(every_k_points) to take points at every k points of the cloud. Or you can modify your read function, just change the loop of i and j to take pixels at every 10:
for i in range(0,480,10):
for j in range(0,640,10):
I have a grid containing some data in polar coordinates, simulating data obtained from a LIDAR for the SLAM problem. Each row in the grid represents the angle, and each column represents a distance. The values contained in the grid store a weighted probability of the occupancy map for a Cartesian world.
After converting to Cartesian Coordinates, I obtain something like this:
This mapping is intended to work in a FastSLAM application, with at least 10 particles. The performance I am obtaining isn't good enough for a reliable application.
I have tried with nested loops, using the scipy.ndimage.geometric_transform library and accessing directly the grid with pre-computed coordinates.
In those examples, I am working with a 800x800 grid.
Nested loops: aprox 300ms
i = 0
for scan in scans:
hit = scan < laser.range_max
if hit:
d = np.linspace(scan + wall_size, 0, num=int((scan+ wall_size)/cell_size))
else:
d = np.linspace(scan, 0, num=int(scan/cell_size))
for distance in distances:
x = int(pos[0] + d * math.cos(angle[i]+pos[2]))
y = int(pos[1] + d * math.sin(angle[i]+pos[2]))
if distance > scan:
grid_cart[y][x] = grid_cart[y][x] + hit_weight
else:
grid_cart[y][x] = grid_cart[y][x] + miss_weight
i = i + 1
Scipy library (Described here): aprox 2500ms (Gives a smoother result since it interpolates the empty cells)
grid_cart = S.ndimage.geometric_transform(weight_mat, polar2cartesian,
order=0,
output_shape = (weight_mat.shape[0] * 2, weight_mat.shape[0] * 2),
extra_keywords = {'inputshape':weight_mat.shape,
'origin':(weight_mat.shape[0], weight_mat.shape[0])})
def polar2cartesian(outcoords, inputshape, origin):
"""Coordinate transform for converting a polar array to Cartesian coordinates.
inputshape is a tuple containing the shape of the polar array. origin is a
tuple containing the x and y indices of where the origin should be in the
output array."""
xindex, yindex = outcoords
x0, y0 = origin
x = xindex - x0
y = yindex - y0
r = np.sqrt(x**2 + y**2)
theta = np.arctan2(y, x)
theta_index = np.round((theta + np.pi) * inputshape[1] / (2 * np.pi))
return (r,theta_index)
Pre-computed indexes: 80ms
for i in range(0, 144000):
gird_cart[ys[i]][xs[i]] = grid_polar_1d[i]
I am not very used to python and Numpy, and I feel I am skipping an easy and fast way to solve this problem. Are there any other alternatives to solve that?
Many thanks to you all!
I came across a piece of code that seems to behave x10 times faster (8ms):
angle_resolution = 1
range_max = 400
a, r = np.mgrid[0:int(360/angle_resolution),0:range_max]
x = (range_max + r * np.cos(a*(2*math.pi)/360.0)).astype(int)
y = (range_max + r * np.sin(a*(2*math.pi)/360.0)).astype(int)
for i in range(0, int(360/angle_resolution)):
cart_grid[y[i,:],x[i,:]] = polar_grid[i,:]
In short, I want to make a radar of sorts, using specifically the graphics.py library, that detects if pre-drawn Shape exists at a Point.
Below I've drafted up a code that will show me all the points within a radar (and return it as an array), but I want to be able to take a point/coordinate and check if it is inside a Circle or Rectangle etc without knowing anything else (i.e only x and y parameters, no knowledge of other existing Shapes). Something like:
if Point(x,y).getObject() is None:
return False
or even
if Point(x,y).getObject().config["fill"] == "#f00":
return True
Here's what I've done so far:
from graphics import *
def getAllPointsInRadius(circle, win):
cx = int(circle.getCenter().getX())
cy = int(circle.getCenter().getY())
r = circle.getRadius()# + 2
points = []
print cx, cy
for i in range(cx-r, cx+r):
for j in range(cy-r, cy+r):
x = i-circle.getRadius()
y = j - circle.getRadius()
if ((i - cx) * (i - cx) + (j - cy) * (j - cy) <= r * r):
points.append(Point(i,j)) #if within, append as Point
p = Point(i, j)
p.setFill('#551A8B')
p.draw(win) #illustrate all points inside radar
else:
p = Point(i, j)
p.setFill('#0000ff')
p.draw(win) #points outside
return points
win = GraphWin('Radar', width = 500, height = 500)
win.setCoords(0, 0, 30, 30)
win.setBackground('black')
#object to be detected
objectCircle = Circle(Point(11, 12), 2)
objectCircle.setFill('#f00')
objectCircle.draw(win)
#radius within objects may be detected
radiusCircle = Circle(Point(10, 10), 3)
radiusCircle.setOutline('#fff')
radiusCircle.draw(win)
print getAllPointsInRadius(radiusCircle, win)
#print Point(11, 12).config["outline"]
win.getMouse()
Addendum: I think maybe by making Point(x,y) I'm actually creating a point and not returning the coordinates. I'm not sure how to do it otherwise, though.
Python PIL library allows me to map any quadrilateral in an image to rectangle using
im.transform(size, QUAD, data)
What I need is a function that does the opposite, i.e. map a rectangular image to specified quadrilateral.
I figured this might be achieved with the above mentioned function like this:
I.e. I would find such quad (the red one in the image) that would, using the function im.transform(size, QUAD, data) transform the image to quad I want. The problem is I don't know how to find the red quad.
I would appreciate any idea on how to find the red quad or any other way to map a rect image to quad, only with PIL if possible.
So I solved the issue with a simple forward mapping, rather than inverse mapping, which is usually better, but in my application I only ever map the rectangle to a quad that is smaller than the rectangle, so there are usually no holes in the transformed image. The code is as follows:
def reverse_quad_transform(image, quad_to_map_to, alpha):
# forward mapping, for simplicity
result = Image.new("RGBA",image.size)
result_pixels = result.load()
width, height = result.size
for y in range(height):
for x in range(width):
result_pixels[x,y] = (0,0,0,0)
p1 = (quad_to_map_to[0],quad_to_map_to[1])
p2 = (quad_to_map_to[2],quad_to_map_to[3])
p3 = (quad_to_map_to[4],quad_to_map_to[5])
p4 = (quad_to_map_to[6],quad_to_map_to[7])
p1_p2_vec = (p2[0] - p1[0],p2[1] - p1[1])
p4_p3_vec = (p3[0] - p4[0],p3[1] - p4[1])
for y in range(height):
for x in range(width):
pixel = image.getpixel((x,y))
y_percentage = y / float(height)
x_percentage = x / float(width)
# interpolate vertically
pa = (p1[0] + p1_p2_vec[0] * y_percentage, p1[1] + p1_p2_vec[1] * y_percentage)
pb = (p4[0] + p4_p3_vec[0] * y_percentage, p4[1] + p4_p3_vec[1] * y_percentage)
pa_to_pb_vec = (pb[0] - pa[0],pb[1] - pa[1])
# interpolate horizontally
p = (pa[0] + pa_to_pb_vec[0] * x_percentage, pa[1] + pa_to_pb_vec[1] * x_percentage)
try:
result_pixels[p[0],p[1]] = (pixel[0],pixel[1],pixel[2],min(int(alpha * 255),pixel[3]))
except Exception:
pass
return result
I am trying to implement a really easy astrometry code. I have manually found the coordinates of a couple of stars in my picture (RA/DEC and x/y in pixel).
Everything seems straight forward but I still get weird results, that are off by a couple of degrees.
I am trying to solve for the plate constants of a CCD image I took, where I found the stars coordinates and position in the picture by hand and now I want to try and find the (0,0) point's real worlds coordinates.
I hope someone can help me with my code or can tell me how to do it properly.
Thanks a lot in advance!
Here is my code:
import numpy as np
import os
def astrometry(star_pos, xpix, ypix, focallength, target_RA, target_DEC):
pi = np.pi
DegToRad = pi / 180
RadToDeg = 180 / pi
n = len(star_pos)
(target_RA, target_DEC) = (target_RA, target_DEC)
print(target_RA, target_DEC)
# 1) Obtain star coordinates in pixel and RA/DEC
x_pix = [row[0] for row in star_pos]
y_pix = [row[1] for row in star_pos]
ra_star = [row[2] for row in star_pos]
dec_star = [row[3] for row in star_pos]
# 2) Calculate the standard coordinates of the stars
X_star = np.zeros(n)
Y_star = np.zeros(n)
for i in range(n):
X_star[i] = -(np.cos(DegToRad*dec_star[i])*np.sin(DegToRad*(ra_star[i] - target_RA)))/(np.cos(DegToRad*target_DEC)*np.cos(DegToRad*dec_star[i])*np.cos(DegToRad*(ra_star[i]-target_RA)) + np.sin(DegToRad*target_DEC)*np.sin(DegToRad*dec_star[i]))
Y_star[i] = -(np.sin(DegToRad*target_DEC)*np.cos(DegToRad*dec_star[i])*np.cos(DegToRad*(ra_star[i]-target_RA)) - np.cos(DegToRad*target_DEC)*np.sin(DegToRad*dec_star[i]))/(np.cos(DegToRad*target_DEC)*np.cos(DegToRad*dec_star[i])*np.cos(DegToRad*(ra_star[i]-target_RA)) + np.sin(DegToRad*target_DEC)*np.sin(DegToRad*dec_star[i]))
# 3) Calculate the plate constants (Check my notes)
def calc_plate_const(k,x,y,X):
c_down = ((x[k+1]-x[k])*(y[k]*x[k+2]-y[k+2]*x[k])-(x[k+2]-x[k])*(y[k]*x[k+1]-y[k+1]*x[k]))
c_up = (X[k]*x[k+1]*(y[k]*x[k+2]-y[k+2]*x[k])-X[k+1]*x[k]*(y[k]*x[k+2]-y[k+2]*x[k])-X[k]*x[k+2]*(y[k]*x[k+1]-y[k+1]*x[k])-X[k+2]*x[k]*(y[k]*x[k+1]-y[k+1]*x[k]))
c = c_up/c_down
print('c',c)
b = ((X[k]*x[k+1]-X[k+1]*x[k])-(x[k+1]-x[k])*c)/(y[k]*x[k+1]-y[k+1]*x[k])
print('b',b)
a = (X[k]-b*y[k]-c)/x[k]
print('a', a)
return(a,b,c)
(a,b,c) = calc_plate_const(0,x_pix,y_pix,X_star)
(d,e,f) = calc_plate_const(0,x_pix,y_pix,Y_star)
print(target_RA,target_DEC)
# 4) Calculate the standard coordinates for the object
# HIER object at (0,0)
(x_ob, y_ob) = (0,0)
X_ob = a*x_ob + b*y_ob + c
Y_ob = d*x_ob + e*y_ob + f
print('x', x_pix, x_ob, 'y', y_pix, y_ob)
print('X', X_star, X_ob, 'Y', Y_star, Y_ob)
print('RA', ra_star, 'DEC', dec_star)
# 5) Calculate the RA/DEC of the objects standard coordinates
a = target_RA + np.arctan(DegToRad*((-X_ob)/(np.cos(DegToRad*target_DEC)- Y_ob*np.sin(DegToRad*target_DEC))))
d = target_DEC - np.arcsin(DegToRad*((np.sin(DegToRad*target_DEC) + Y_ob*np.cos(DegToRad*target_DEC))/(np.sqrt(1 + X_ob**2 + Y_ob**2))))
print('RA in rad', a, 'DEC in rad', d)
print('RA',a,target_RA, 'DEC',d, target_DEC)
return(a,d)
The Input is for example an array with the stars position in pixel of the image and degree of the real world
star pos = [[1948.2, 1205.8, 132.34058333333334, -3.4429722222222225], [153.90000000000001, 1892.5, 131.08620833333333, -5.0947499999999994]
# star_pos [x_pos in pix, y_pos in pix, RA, DEC]
(x_pix, y_pix) = (0.0135, 0.0135)
# pixel size
focallength = 0.7168
(target_RA, target_DEC) = (131.683014444 -3.91890194444)
# But I am not sure how accurate that is, it is more of an assumption. I would say if I look at the star map manually, it looks quite a bit off...
I would expect to see for the (0,0) point RA to be around 133° and DEC -5.75°