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):
Related
I am working on a project related to charge distribution on the sphere and I decided to simulate the problem using vpython and Coulomb's law. I ran into an issue when I created a sphere because I am trying to evenly place out like 1000 points (charges) on the sphere and I can't seem to succeed, I have tried several ways but can't seem to make the points be on the sphere.
I defined an arbitrary value SOYDNR as a number to divide the diameter of the sphere into smaller segments. This would allow me to create a smaller rings of charges and fill out the surface of the spahre with charges. Then I make a list with 4 values that represent different parts of the radius to create the charge rings on the surface. Then I run into a problem and I am not sure how to deal with it. I have tried calculating the radius at those specific heights but yet I couldn't implement it. This is how it looks visually:![Sphere with charges on the surface].(https://i.stack.imgur.com/3N4x6.png) If anyone has any suggestions, I would be greatful, thanks!
SOYDNR = 10 #can be changed
SOYD = 2*radi/SOYDNR # strips of Y direction; initial height + SOYD until = 2*radi
theta = 0
dtheta1 = 2*pi/NCOS
y_list = np.arange(height - radi + SOYD, height, SOYD).tolist()
print(y_list)
for i in y_list:
while Nr<NCOS and theta<2*pi:
position = radi*vector(cos(theta),i*height/radi,sin(theta))
points_on_sphere = points_on_sphere + [sphere(pos=position, radius=radi/50, color=vector(1, 0, 0))]
Nr = Nr + 1
theta = theta + dtheta1
Nr = 0
theta = 0
I found a great way to do it, it creates a bunch of spheres in the area that is described by an if statement this is the code I am using for my simulation that creates the sphere with points on it.
def SOSE (radi, number_of_charges, height):
Charged_Sphere = sphere(pos=vector(0,height,0), radius=radi, color=vector(3.5, 3.5, 3.5), opacity=(0.2))
points_on_sphere = []
NCOS = number_of_charges
theta = 0
dtheta = 2*pi/NCOS
dr = radi/60
direcVector = vector(0, height, 0)
while theta<2*pi:
posvec1 = radi*vector(1-radi*random(),1-radi*random()/radi,1-radi*random())
posvec2 = radi*vector(1-radi*random(),-1+radi*random()/radi,1-radi*random())
if mag(posvec1)<radi and mag(posvec1)>(radi-dr):
posvec1 = posvec1+direcVector
points_on_sphere=points_on_sphere+[sphere(pos=posvec1,radius=radi/60,color=vector(1, 0, 0))]
theta=theta + dtheta
if mag(posvec2)<radi and mag(posvec2)>(radi-dr):
posvec2 = posvec2+direcVector
points_on_sphere=points_on_sphere+[sphere(pos=posvec2,radius=radi/60,color=vector(1, 0, 0))]
theta=theta + dtheta
This code can be edited to add more points and I have two if statements because I want to change the height at which the sphere is present, and if I have just one statement I only see half of the sphere. :)
I am currently simulation light passing through an optics system with python and Zemax. I have it set up currently where I define the x and y boundaries of the "sensor" to that i can choose the size of the area I want to simulate. I get 1 rectangle.
I'd like to simulate nine rectangles, in a 3x3 Grid. I am unsure which way would be the most elegant... my first Idea was to "hardcode" the different intervals into 9 different scripts and run those through a bash script, but it seemst a bit to "unelegant".
How do I have to define xmax, xmin, ymax, ymin now so that i can run the same simulation and get those nine retangles?
My thought was to maybe create some sort of list where the boundaries are defined, and then perhaps rerun the simulation with a different boundary each time and finally merging the images that appear.
The current code is quite long, but the parameters are all set in a main functions which looks like this:
if __name__ == '__main__':
DirNameZmx = r'C:\Some\Path'
FileNameZmx = r"Optics.zmx"
DirNameResults = r"C:\Some\Other\Path"
FileNameResults = r"Interferogram_Result"
(QueueFieldsOut, QueueToDetector, ProcessRaytracing, ProcessesPsfWorkers, ProcessDetector) = \
InitializeSimulation(DirNameZmx=DirNameZmx, FileNameZmx=FileNameZmx,
DirNameResults=DirNameResults, FileNameResults=FileNameResults,
FieldAngleHxMin=-0.02, FieldAngleHxMax=+0.02, dFieldAngleX=0.001,
FieldAngleHyMin=-0.06, FieldAngleHyMax=+0.06, dFieldAngleY=0.001,
NbrWavelength=1, Configurations=[1, 2], NbrRaysFieldRow=32, RAperture=0.99,
DetectorImageSize=11., DetectorPixelSize=0.011, ZeroPadding=8,
BatchRaysMax=512**2, NbrProcessWorkers=2)
print(ProcessRaytracing.join())
for Process in ProcessesPsfWorkers:
print(Process.join())
print(Process.name, Process.exitcode)
print(ProcessDetector.join())
data = np.load(os.path.join(DirNameResults, FileNameResults+'.npy'))
plt.imshow(data, cmap="coolwarm")
plt.show()
The FieldAngleHxMin/Max and FieldAngleHyMin/Max are the rectangle boundaries. The result looks like this:
Simple iteration will do the work.
Try this:
def nine_squares(FieldAngleHxMin, FieldAngleHxMax, FieldAngleHyMin, FieldAngleHyMax):
xstep = (FieldAngleHxMax - FieldAngleHxMin) / 3
ystep = (FieldAngleHyMax - FieldAngleHyMin) / 3
for i in range(3):
for j in range(3):
xstartpoint = xstep + i * FieldAngleHxMin
xendpoint = xstep + (i + 1) * FieldAngleHxMin
ystartpoint = ystep + i * FieldAngleHyMin
yendpoint = ystep + (i + 1) * FieldAngleHyMin
yield (xstartpoint, xendpoint, ystartpoint, yendpoint)
It will return lists of start and end coordinates every time it is called.
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]
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,:]
I have an image processing problem I'm currently solving in python, using numpy and scipy. Briefly, I have an image that I want to apply many local contractions to. My prototype code is working, and the final images look great. However, processing time has become a serious bottleneck in our application. Can you help me speed up my image processing code?
I've tried to boil down our code to the 'cartoon' version below. Profiling suggests that I'm spending most of my time on interpolation. Are there obvious ways to speed up execution?
import cProfile, pstats
import numpy
from scipy.ndimage import interpolation
def get_centered_subimage(
center_point, window_size, image):
x, y = numpy.round(center_point).astype(int)
xSl = slice(max(x-window_size-1, 0), x+window_size+2)
ySl = slice(max(y-window_size-1, 0), y+window_size+2)
subimage = image[xSl, ySl]
interpolation.shift(
subimage, shift=(x, y)-center_point, output=subimage)
return subimage[1:-1, 1:-1]
"""In real life, this is experimental data"""
im = numpy.zeros((1000, 1000), dtype=float)
"""In real life, this mask is a non-zero pattern"""
window_radius = 10
mask = numpy.zeros((2*window_radius+1, 2*window_radius+1), dtype=float)
"""The x, y coordinates in the output image"""
new_grid_x = numpy.linspace(0, im.shape[0]-1, 2*im.shape[0])
new_grid_y = numpy.linspace(0, im.shape[1]-1, 2*im.shape[1])
"""The grid we'll end up interpolating onto"""
grid_step_x = new_grid_x[1] - new_grid_x[0]
grid_step_y = new_grid_y[1] - new_grid_y[0]
subgrid_radius = numpy.floor(
(-1 + window_radius * 0.5 / grid_step_x,
-1 + window_radius * 0.5 / grid_step_y))
subgrid = (
window_radius + 2 * grid_step_x * numpy.arange(
-subgrid_radius[0], subgrid_radius[0] + 1),
window_radius + 2 * grid_step_y * numpy.arange(
-subgrid_radius[1], subgrid_radius[1] + 1))
subgrid_points = ((2*subgrid_radius[0] + 1) *
(2*subgrid_radius[1] + 1))
"""The coordinates of the set of spots we we want to contract. In real
life, this set is non-random:"""
numpy.random.seed(0)
num_points = 10000
center_points = numpy.random.random(2*num_points).reshape(num_points, 2)
center_points[:, 0] *= im.shape[0]
center_points[:, 1] *= im.shape[1]
"""The output image"""
final_image = numpy.zeros(
(new_grid_x.shape[0], new_grid_y.shape[0]), dtype=numpy.float)
def profile_me():
for m, cp in enumerate(center_points):
"""Take an image centered on each illumination point"""
spot_image = get_centered_subimage(
center_point=cp, window_size=window_radius, image=im)
if spot_image.shape != (2*window_radius+1, 2*window_radius+1):
continue #Skip to the next spot
"""Mask the image"""
masked_image = mask * spot_image
"""Resample the image"""
nearest_grid_index = numpy.round(
(cp - (new_grid_x[0], new_grid_y[0])) /
(grid_step_x, grid_step_y))
nearest_grid_point = (
(new_grid_x[0], new_grid_y[0]) +
(grid_step_x, grid_step_y) * nearest_grid_index)
new_coordinates = numpy.meshgrid(
subgrid[0] + 2 * (nearest_grid_point[0] - cp[0]),
subgrid[1] + 2 * (nearest_grid_point[1] - cp[1]))
resampled_image = interpolation.map_coordinates(
masked_image,
(new_coordinates[0].reshape(subgrid_points),
new_coordinates[1].reshape(subgrid_points))
).reshape(2*subgrid_radius[1]+1,
2*subgrid_radius[0]+1).T
"""Add the recentered image back to the scan grid"""
final_image[
nearest_grid_index[0]-subgrid_radius[0]:
nearest_grid_index[0]+subgrid_radius[0]+1,
nearest_grid_index[1]-subgrid_radius[1]:
nearest_grid_index[1]+subgrid_radius[1]+1,
] += resampled_image
cProfile.run('profile_me()', 'profile_results')
p = pstats.Stats('profile_results')
p.strip_dirs().sort_stats('cumulative').print_stats(10)
Vague explanation of what the code does:
We start with a pixellated 2D image, and a set of arbitrary (x, y) points in our image that don't generally fall on an integer grid. For each (x, y) point, I want to multiply the image by a small mask centered precisely on that point. Next we contract/expand the masked region by a finite amount, before finally adding this processed sub-image to a final image, which may not have the same pixel size as the original image. (Not my finest explanation. Ah well).
I'm pretty sure that, as you said, the bulk of the calculation time happens in interpolate.map_coordinates(…), which gets called once for every iteration on center_points, here 10,000 times. Generally, working with the numpy/scipy stack, you want the repetitive task over a large array to happen in native Numpy/Scipy functions -- i.e. in a C loop over homogeneous data -- as opposed to explicitely in Python.
One strategy that might speed up the interpolation, but that will also increase the amount of memory used, is :
First, fetch all the subimages (here named masked_image) in a 3-dimensional array (window_radius x window_radius x center_points.size)
Make a ufunc (read that, it's useful) that wraps the work that has to be done on each subimage, using numpy.frompyfunc, which should return another 3-dimensional array (subgrid_radius[0] x subgrid_radius[1] x center_points.size). In short, this creates a vectorized version of the python function, that can be broadcast element-wise on an array.
Build the final image by summing over the third dimension.
Hope that gets you closer to your goals!