Skip to content

Added a viewpoint to each point in the point cloud. #34

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
jey3dee wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Added a viewpoint to each point in the point cloud. #34

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
bc60029
Added a viewpoint to each point in the point cloud.
Aug 12, 2019
d9ce495
Fixed point cloud visualization.
Aug 12, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 14 additions & 13 deletions release/scripts/addons/blensor/blendodyne.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -190,26 +190,27 @@ def scan_advanced(scanner_object, rotation_speed = 10.0, simulation_fps=24, angl
rays.extend([ray[0],ray[1],ray[2]])

returns = blensor.scan_interface.scan_rays(rays, max_distance, inv_scan_x = inv_scan_x, inv_scan_y = inv_scan_y, inv_scan_z = inv_scan_z)

# for idx in range((len(rays)//3)):

reusable_4dvector = Vector([0.0,0.0,0.0,0.0])

reusable_vector = Vector([0.0,0.0,0.0,1.0])
vp = (world_transformation * reusable_vector).xyz

for i in range(len(returns)):
idx = returns[i][-1]
reusable_4dvector.xyzw = (returns[i][1],returns[i][2],returns[i][3],1.0)
vt = (world_transformation * reusable_4dvector).xyz

# Calculate noise-free point.
reusable_vector.xyzw = (returns[i][1],returns[i][2],returns[i][3],1.0)
vt = (world_transformation * reusable_vector).xyz
v = [returns[i][1],returns[i][2],returns[i][3]]

distance_noise = laser_noise[idx%len(scanner_angles)] + random.gauss(noise_mu, noise_sigma)
# Calculate noisy point.
distance_noise = laser_noise[idx%len(scanner_angles)] + random.gauss(noise_mu, noise_sigma)
vector_length = math.sqrt(v[0]**2+v[1]**2+v[2]**2)
norm_vector = [v[0]/vector_length, v[1]/vector_length, v[2]/vector_length]
vector_length_noise = vector_length+distance_noise
reusable_4dvector.xyzw=[norm_vector[0]*vector_length_noise, norm_vector[1]*vector_length_noise, norm_vector[2]*vector_length_noise,1.0]
v_noise = (world_transformation * reusable_4dvector).xyz

evd_storage.addEntry(timestamp = ray_info[idx][2], yaw =(ray_info[idx][0]+math.pi)%(2*math.pi), pitch=ray_info[idx][1], distance=vector_length, distance_noise=vector_length_noise, x=vt[0], y=vt[1], z=vt[2], x_noise=v_noise[0], y_noise=v_noise[1], z_noise=v_noise[2], object_id=returns[i][4], color=returns[i][5])
reusable_vector.xyzw=[norm_vector[0]*vector_length_noise, norm_vector[1]*vector_length_noise, norm_vector[2]*vector_length_noise,1.0]
v_noise = (world_transformation * reusable_vector).xyz

evd_storage.addEntry(timestamp = ray_info[idx][2], yaw =(ray_info[idx][0]+math.pi)%(2*math.pi), pitch=ray_info[idx][1], distance=vector_length, distance_noise=vector_length_noise, vp_x=vp[0], vp_y=vp[1], vp_z=vp[2], x=vt[0], y=vt[1], z=vt[2], x_noise=v_noise[0], y_noise=v_noise[1], z_noise=v_noise[2], object_id=returns[i][4], color=returns[i][5])

current_angle = start_angle+float(float(int(lines))*angle_resolution)

Expand All @@ -225,10 +226,10 @@ def scan_advanced(scanner_object, rotation_speed = 10.0, simulation_fps=24, angl
additional_data = evd_storage.buffer

if add_blender_mesh:
mesh_utils.add_mesh_from_points_tf(scan_data[:,5:8], "Scan", world_transformation, buffer=additional_data)
mesh_utils.add_mesh_from_points_tf(scan_data[:,8:11], "Scan", world_transformation, buffer=additional_data)

if add_noisy_blender_mesh:
mesh_utils.add_mesh_from_points_tf(scan_data[:,8:11], "NoisyScan", world_transformation, buffer=additional_data)
mesh_utils.add_mesh_from_points_tf(scan_data[:,11:14], "NoisyScan", world_transformation, buffer=additional_data)

bpy.context.scene.update()

Expand Down
53 changes: 27 additions & 26 deletions release/scripts/addons/blensor/evd.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,13 +2,14 @@
import traceback
import struct
import math
import bpy

PCL_HEADER = """# .PCD v.7 - Exported by BlenSor
VERSION .7
FIELDS x y z rgb
SIZE 4 4 4 4
TYPE F F F F
COUNT 1 1 1 1
FIELDS x y z rgb vp_x vp_y vp_z
SIZE 4 4 4 4 4 4 4
TYPE F F F F F F F
COUNT 1 1 1 1 1 1 1
WIDTH %d
HEIGHT %d
VIEWPOINT 0 0 0 1 0 0 0
Expand All @@ -18,19 +19,17 @@

PCL_HEADER_WITH_LABELS = """# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z rgb label
SIZE 4 4 4 4 4
TYPE F F F F U
COUNT 1 1 1 1 1
FIELDS x y z rgb vp_x vp_y vp_z label
SIZE 4 4 4 4 4 4 4 4
TYPE F F F F F F F U
COUNT 1 1 1 1 1 1 1 1
WIDTH %d
HEIGHT %d
VIEWPOINT 0 0 0 1 0 0 0
POINTS %d
DATA ascii
"""



PGM_VALUE_RANGE = 65535

PGM_HEADER ="""P2
Expand Down Expand Up @@ -110,7 +109,6 @@ def fromMesh(self, mesh):
"object_id": None,
"point_index": None}


data = dict.fromkeys(data_layers.keys(), 0.0)

for i in range(len(bm.verts.layers.float)):
Expand All @@ -125,10 +123,12 @@ def fromMesh(self, mesh):
for dk in data_layers.keys():
if data_layers[dk]:
data[dk] = v[data_layers[dk]]

self.addEntry(timestamp=data.get("timestamp",0.0),
yaw=data.get("yaw",0.0),
pitch=data.get("pitch",0.0),
distance=data.get("distance",0.0),
vp_x=float('NaN'), vp_y=float('NaN'), vp_z=float('NaN'),
x=x,y=y,z=z,
object_id=data.get("object_id",0.0),
color=(data.get("color_red",0.0),
Expand All @@ -139,17 +139,16 @@ def fromMesh(self, mesh):
bm.free()

def addEntry(self, timestamp=0.0, yaw=0.0, pitch=0.0, distance=0.0,
distance_noise=0.0, x=0.0, y=0.0, z=0.0,
distance_noise=0.0, vp_x=0.0, vp_y=0.0, vp_z=0.0, x=0.0, y=0.0, z=0.0,
x_noise = 0.0, y_noise = 0.0, z_noise = 0.0, object_id=0, color=(1.0,1.0,1.0), idx=0):
idx = int(idx) #If the index is a numpy.float (from the kinect)
if self.mode == WRITER_MODE_PGM:
if idx >=0 and idx < len(self.image):
self.image[idx]=distance
self.image_noisy[idx]=distance_noise



self.buffer.append([timestamp, yaw, pitch, distance,distance_noise,
x,y,z,x_noise,y_noise,z_noise,object_id,int(255*color[0]),int(255*color[1]),int(255*color[2]),idx])
vp_x,vp_y,vp_z,x,y,z,x_noise,y_noise,z_noise,object_id,int(255*color[0]),int(255*color[1]),int(255*color[2]),idx])

def writeEvdFile(self):
if self.mode == WRITER_MODE_PCL:
Expand All @@ -163,7 +162,7 @@ def writeEvdFile(self):
evd.buffer.write(struct.pack("i", len(self.buffer)))
for e in self.buffer:
#The evd format does not allow negative object ids
evd.buffer.write(struct.pack("14dQ", float(e[0]),float(e[1]),float(e[2]),float(e[3]),float(e[4]),float(e[5]),float(e[6]),float(e[7]),float(e[8]),float(e[9]),float(e[10]), float(e[12]),float(e[13]),float(e[14]),max(0,int(e[11]))))
evd.buffer.write(struct.pack("14dQ", float(e[0]),float(e[1]),float(e[2]),float(e[3]),float(e[4]),float(e[8]),float(e[9]),float(e[10]),float(e[11]),float(e[12]),float(e[13]), float(e[15]),float(e[16]),float(e[17]),max(0,int(e[14]))))
evd.close()

def appendEvdFile(self):
Expand All @@ -179,24 +178,26 @@ def appendEvdFile(self):
idx = 0
for e in self.buffer:
#The evd format does not allow negative object ids
evd.buffer.write(struct.pack("14dQ", float(e[0]),float(e[1]),float(e[2]),float(e[3]),float(e[4]),float(e[5]),float(e[6]),float(e[7]),float(e[8]),float(e[9]),float(e[10]), float(e[12]),float(e[13]),float(e[14]),max(0,int(e[11]))))
evd.buffer.write(struct.pack("14dQ", float(e[0]),float(e[1]),float(e[2]),float(e[3]),float(e[4]),float(e[8]),float(e[9]),float(e[10]),float(e[11]),float(e[12]),float(e[13]), float(e[15]),float(e[16]),float(e[17]),max(0,int(e[14]))))
idx = idx + 1
print ("Written: %d entries"%idx)
evd.close()

def write_point(self, pcl, pcl_noisy, e, output_labels):
#Storing color values packed into a single floating point number???
#That is really required by the pcl library!
color_uint32 = (e[12]<<16) | (e[13]<<8) | (e[14])
color_uint32 = (e[15]<<16) | (e[16]<<8) | (e[17])
values=struct.unpack("f",struct.pack("I",color_uint32))

# Get the camera location.
cam_loc = bpy.context.scene.camera.location

if output_labels:
pcl.write("%f %f %f %.15e %d\n"%(float(e[5]),float(e[6]),float(e[7]), values[0], int(e[11])))
pcl_noisy.write("%f %f %f %.15e %d\n"%(float(e[8]),float(e[9]),float(e[10]), values[0], int(e[11])))
pcl.write("%f %f %f %.15e %f %f %f %d\n"%(float(e[8]),float(e[9]),float(e[10]), values[0], float(e[5]),float(e[6]),float(e[7]), int(e[14])))
pcl_noisy.write("%f %f %f %.15e %f %f %f %d\n"%(float(e[11]),float(e[12]),float(e[13]), values[0], float(e[5]),float(e[6]),float(e[7]), int(e[14])))
else:
pcl.write("%f %f %f %.15e\n"%(float(e[5]),float(e[6]),float(e[7]), values[0]))
pcl_noisy.write("%f %f %f %.15e\n"%(float(e[8]),float(e[9]),float(e[10]), values[0]))

pcl.write("%f %f %f %.15e %f %f %f\n"%(float(e[8]),float(e[9]),float(e[10]), values[0], float(e[5]),float(e[6]),float(e[7])))
pcl_noisy.write("%f %f %f %.15e %f %f %f\n"%(float(e[11]),float(e[12]),float(e[13]), values[0], float(e[5]),float(e[6]),float(e[7])))

def writePCLFile(self):
global frame_counter #Not nice to have it global but it needs to persist
Expand All @@ -220,8 +221,8 @@ def writePCLFile(self):
pcl_noisy.write(PCL_HEADER%(width,height,width*height))
idx = 0
for e in self.buffer:
if e[15] > idx and not sparse_mode: # e[15] is the idx of the point
for i in range(idx, e[15]):
if e[18] > idx and not sparse_mode: # e[18] is the idx of the point
for i in range(idx, e[18]):
self.write_point(pcl, pcl_noisy, INVALID_POINT, self.output_labels)
idx += 1

Expand All @@ -231,7 +232,7 @@ def writePCLFile(self):
if idx < width*height:
for i in range(idx, width*height):
self.write_point(pcl, pcl_noisy, INVALID_POINT, self.output_labels)

# TODO continue here

pcl.close()
pcl_noisy.close()
Expand Down
12 changes: 8 additions & 4 deletions release/scripts/addons/blensor/generic_lidar.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -159,24 +159,28 @@ def scan_advanced(scanner_object, simulation_fps=24, evd_file=None,noise_mu=0.0,

returns = blensor.scan_interface.scan_rays(rays, max_distance, inv_scan_x = inv_scan_x, inv_scan_y = inv_scan_y, inv_scan_z = inv_scan_z)

reusable_vector = Vector([0.0,0.0,0.0,0.0])
reusable_vector = Vector([0.0,0.0,0.0,1.0])
if len(laser_angles) != len(laser_noise):
randomize_distance_bias(len(laser_angles), noise_mu,noise_sigma)
vp = (world_transformation * reusable_vector).xyz

for i in range(len(returns)):
idx = returns[i][-1]

# Calculate noise-free point.
reusable_vector.xyzw = [returns[i][1],returns[i][2],returns[i][3],1.0]
vt = (world_transformation * reusable_vector).xyz
v = [returns[i][1],returns[i][2],returns[i][3]]

# Calculate noisy point.
vector_length = math.sqrt(v[0]**2+v[1]**2+v[2]**2)
distance_noise = laser_noise[idx%len(laser_noise)] + model.drawErrorFromModel(vector_length)
norm_vector = [v[0]/vector_length, v[1]/vector_length, v[2]/vector_length]
vector_length_noise = vector_length+distance_noise
reusable_vector.xyzw = [norm_vector[0]*vector_length_noise, norm_vector[1]*vector_length_noise, norm_vector[2]*vector_length_noise,1.0]
v_noise = (world_transformation * reusable_vector).xyz

evd_storage.addEntry(timestamp = ray_info[idx][2], yaw =(ray_info[idx][0]+math.pi)%(2*math.pi), pitch=ray_info[idx][1], distance=vector_length, distance_noise=vector_length_noise, x=vt[0], y=vt[1], z=vt[2], x_noise=v_noise[0], y_noise=v_noise[1], z_noise=v_noise[2], object_id=returns[i][4], color=returns[i][5])
evd_storage.addEntry(timestamp = ray_info[idx][2], yaw =(ray_info[idx][0]+math.pi)%(2*math.pi), pitch=ray_info[idx][1], distance=vector_length, distance_noise=vector_length_noise, vp_x=vp[0], vp_y=vp[1], vp_z=vp[2], x=vt[0], y=vt[1], z=vt[2], x_noise=v_noise[0], y_noise=v_noise[1], z_noise=v_noise[2], object_id=returns[i][4], color=returns[i][5])


current_angle = start_angle+float(float(int(lines))*angle_resolution)
Expand All @@ -191,10 +195,10 @@ def scan_advanced(scanner_object, simulation_fps=24, evd_file=None,noise_mu=0.0,
additional_data = evd_storage.buffer

if add_blender_mesh:
mesh_utils.add_mesh_from_points_tf(scan_data[:,5:8], "Scan", world_transformation, buffer=additional_data)
mesh_utils.add_mesh_from_points_tf(scan_data[:,8:11], "Scan", world_transformation, buffer=additional_data)

if add_noisy_blender_mesh:
mesh_utils.add_mesh_from_points_tf(scan_data[:,8:11], "NoisyScan", world_transformation, buffer=additional_data)
mesh_utils.add_mesh_from_points_tf(scan_data[:,11:14], "NoisyScan", world_transformation, buffer=additional_data)

bpy.context.scene.update()

Expand Down
13 changes: 9 additions & 4 deletions release/scripts/addons/blensor/ibeo.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -159,21 +159,26 @@ def scan_advanced(scanner_object, rotation_speed = 25.0, simulation_fps=24, angl

returns = blensor.scan_interface.scan_rays(rays, max_distance, inv_scan_x = inv_scan_x, inv_scan_y = inv_scan_y, inv_scan_z = inv_scan_z)

reusable_vector = Vector([0.0,0.0,0.0,0.0])
reusable_vector = Vector([0.0,0.0,0.0,1.0])
vp = (world_transformation * reusable_vector).xyz

for i in range(len(returns)):
idx = returns[i][-1]

# Calculate noise-free point.
reusable_vector.xyzw = [returns[i][1],returns[i][2],returns[i][3],1.0]
vt = (world_transformation * reusable_vector).xyz
v = [returns[i][1],returns[i][2],returns[i][3]]

# Calculate noisy point.
distance_noise = laser_noise[idx%len(laser_noise)] + random.gauss(noise_mu, noise_sigma)
vector_length = math.sqrt(v[0]**2+v[1]**2+v[2]**2)
norm_vector = [v[0]/vector_length, v[1]/vector_length, v[2]/vector_length]
vector_length_noise = vector_length+distance_noise
reusable_vector.xyzw = [norm_vector[0]*vector_length_noise, norm_vector[1]*vector_length_noise, norm_vector[2]*vector_length_noise,1.0]
v_noise = (world_transformation * reusable_vector).xyz

evd_storage.addEntry(timestamp = ray_info[idx][2], yaw =(ray_info[idx][0]+math.pi)%(2*math.pi), pitch=ray_info[idx][1], distance=vector_length, distance_noise=vector_length_noise, x=vt[0], y=vt[1], z=vt[2], x_noise=v_noise[0], y_noise=v_noise[1], z_noise=v_noise[2], object_id=returns[i][4], color=returns[i][5])
evd_storage.addEntry(timestamp = ray_info[idx][2], yaw =(ray_info[idx][0]+math.pi)%(2*math.pi), pitch=ray_info[idx][1], distance=vector_length, distance_noise=vector_length_noise, vp_x=vp[0], vp_y=vp[1], vp_z=vp[2], x=vt[0], y=vt[1], z=vt[2], x_noise=v_noise[0], y_noise=v_noise[1], z_noise=v_noise[2], object_id=returns[i][4], color=returns[i][5])

current_angle = start_angle+float(float(int(lines))*angle_resolution)

Expand All @@ -187,10 +192,10 @@ def scan_advanced(scanner_object, rotation_speed = 25.0, simulation_fps=24, angl
additional_data = evd_storage.buffer

if add_blender_mesh:
mesh_utils.add_mesh_from_points_tf(scan_data[:,5:8], "Scan", world_transformation, buffer=additional_data)
mesh_utils.add_mesh_from_points_tf(scan_data[:,8:11], "Scan", world_transformation, buffer=additional_data)

if add_noisy_blender_mesh:
mesh_utils.add_mesh_from_points_tf(scan_data[:,8:11], "NoisyScan", world_transformation, buffer=additional_data)
mesh_utils.add_mesh_from_points_tf(scan_data[:,11:14], "NoisyScan", world_transformation, buffer=additional_data)

bpy.context.scene.update()

Expand Down
22 changes: 10 additions & 12 deletions release/scripts/addons/blensor/kinect.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -216,13 +216,11 @@ def scan_advanced(scanner_object, evd_file=None,
"""Calculate a vector that originates at the principal point
and points to the pixel in the sensor. This vector is then
scaled to the maximum scanning distance
"""

"""
physical_x = float(x-cx) * pixel_width
physical_y = float(y-cy) * pixel_height
physical_z = -float(flength)

#ray = Vector([physical_x, physical_y, physical_z])
ray.xyz=[physical_x, physical_y, physical_z]
ray.normalize()
final_ray = max_distance*ray
Expand Down Expand Up @@ -259,6 +257,7 @@ def scan_advanced(scanner_object, evd_file=None,
kinect_image = numpy.zeros((res_x*res_y,16))
kinect_image[:,3:11] = float('NaN')
kinect_image[:,11] = -1.0

"""Calculate the rays from the camera to the hit points of the projector rays"""
for i in range(len(returns)):
idx = returns[i][-1]
Expand Down Expand Up @@ -295,20 +294,20 @@ def scan_advanced(scanner_object, evd_file=None,
abs(camera_rays[idx*3+2]-camera_returns[i][3]) < thresh and
abs(camera_returns[i][3]) <= max_distance and
abs(camera_returns[i][3]) >= min_distance):

"""The ray hit the projected ray, so this is a valid measurement"""
projector_point = get_uv_from_idx(projector_idx, res_x,res_y)
projector_point = get_uv_from_idx(projector_idx, res_x, res_y)

camera_x = get_pixel_from_world(camera_rays[idx*3],camera_rays[idx*3+2],
camera_x = get_pixel_from_world(camera_rays[idx*3], camera_rays[idx*3+2],
flength/pixel_width) + random.gauss(noise_mu, noise_sigma)

camera_y = get_pixel_from_world(camera_rays[idx*3+1],camera_rays[idx*3+2],
camera_y = get_pixel_from_world(camera_rays[idx*3+1], camera_rays[idx*3+2],
flength/pixel_width)

""" Kinect calculates the disparity with an accuracy of 1/8 pixel"""

camera_x_quantized = round(camera_x*8.0)/8.0

#I don't know if this accurately represents the kinect
# I don't know if this accurately represents the kinect.
camera_y_quantized = round(camera_y*8.0)/8.0

disparity_quantized = camera_x_quantized + projector_point[0]
Expand Down Expand Up @@ -370,10 +369,9 @@ def scan_advanced(scanner_object, evd_file=None,

for e in kinect_image:
evd_storage.addEntry(timestamp = e[0], yaw = e[1], pitch=e[2],
distance=e[3], distance_noise=e[4], x=e[5], y=e[6], z=e[7],
distance=e[3], distance_noise=e[4], vp_x=float('NaN'), vp_y=float('NaN'), vp_z=float('NaN'), x=e[5], y=e[6], z=e[7],
x_noise=e[8], y_noise=e[9], z_noise=e[10], object_id=e[11],
color=[e[12],e[13],e[14]], idx=e[15])


if evd_file:
evd_storage.appendEvdFile()
Expand All @@ -385,10 +383,10 @@ def scan_advanced(scanner_object, evd_file=None,
additional_data = evd_storage.buffer

if add_blender_mesh:
mesh_utils.add_mesh_from_points_tf(scan_data[:,5:8], "Scan", world_transformation, buffer=additional_data)
mesh_utils.add_mesh_from_points_tf(scan_data[:,8:11], "Scan", world_transformation, buffer=additional_data)

if add_noisy_blender_mesh:
mesh_utils.add_mesh_from_points_tf(scan_data[:,8:11], "NoisyScan", world_transformation, buffer=additional_data)
mesh_utils.add_mesh_from_points_tf(scan_data[:,11:14], "NoisyScan", world_transformation, buffer=additional_data)

bpy.context.scene.update()

Expand Down
Loading