From bc6002923dd7e97430e33ec6d188abdb4d7387ea Mon Sep 17 00:00:00 2001 From: Joachim Gehrung Date: Mon, 12 Aug 2019 17:08:47 +0200 Subject: [PATCH 1/2] Added a viewpoint to each point in the point cloud. In order to generate simulated point clouds for raycasting algorithms, the viewpoint of each point is required. Added additional fields to the buffer array (temporarily storing the point cloud. Each simulated sensor (except the kinect) fills this fields with the ray's point of origin. Be aware that for the sake of simplicity the point of origin corresponds to the sensor position. This is only an approximation, but good enough for most applications. --- release/scripts/addons/blensor/blendodyne.py | 24 +++++---- release/scripts/addons/blensor/evd.py | 53 ++++++++++--------- .../scripts/addons/blensor/generic_lidar.py | 8 ++- release/scripts/addons/blensor/ibeo.py | 9 +++- release/scripts/addons/blensor/kinect.py | 18 +++---- release/scripts/addons/blensor/tof.py | 9 ++-- 6 files changed, 67 insertions(+), 54 deletions(-) diff --git a/release/scripts/addons/blensor/blendodyne.py b/release/scripts/addons/blensor/blendodyne.py index 9db77a659c..2f0029a21b 100644 --- a/release/scripts/addons/blensor/blendodyne.py +++ b/release/scripts/addons/blensor/blendodyne.py @@ -190,26 +190,28 @@ 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 + print(world_transformation) + 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) diff --git a/release/scripts/addons/blensor/evd.py b/release/scripts/addons/blensor/evd.py index 68bd2908f7..1db9abfe4f 100644 --- a/release/scripts/addons/blensor/evd.py +++ b/release/scripts/addons/blensor/evd.py @@ -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 @@ -18,10 +19,10 @@ 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 @@ -29,8 +30,6 @@ DATA ascii """ - - PGM_VALUE_RANGE = 65535 PGM_HEADER ="""P2 @@ -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)): @@ -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), @@ -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: @@ -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): @@ -179,7 +178,7 @@ 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() @@ -187,16 +186,18 @@ def appendEvdFile(self): 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 @@ -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 @@ -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() diff --git a/release/scripts/addons/blensor/generic_lidar.py b/release/scripts/addons/blensor/generic_lidar.py index fe4a62e0b1..159f538a55 100644 --- a/release/scripts/addons/blensor/generic_lidar.py +++ b/release/scripts/addons/blensor/generic_lidar.py @@ -159,16 +159,20 @@ 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] @@ -176,7 +180,7 @@ def scan_advanced(scanner_object, simulation_fps=24, evd_file=None,noise_mu=0.0, 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) diff --git a/release/scripts/addons/blensor/ibeo.py b/release/scripts/addons/blensor/ibeo.py index bf4e03a76f..469aac0dab 100644 --- a/release/scripts/addons/blensor/ibeo.py +++ b/release/scripts/addons/blensor/ibeo.py @@ -159,13 +159,18 @@ 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] @@ -173,7 +178,7 @@ def scan_advanced(scanner_object, rotation_speed = 25.0, simulation_fps=24, angl 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) diff --git a/release/scripts/addons/blensor/kinect.py b/release/scripts/addons/blensor/kinect.py index 8a08e7d658..37e86c1633 100644 --- a/release/scripts/addons/blensor/kinect.py +++ b/release/scripts/addons/blensor/kinect.py @@ -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 @@ -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] @@ -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] @@ -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() diff --git a/release/scripts/addons/blensor/tof.py b/release/scripts/addons/blensor/tof.py index 5a8169df89..ff644bd085 100644 --- a/release/scripts/addons/blensor/tof.py +++ b/release/scripts/addons/blensor/tof.py @@ -136,20 +136,23 @@ def scan_advanced(scanner_object, max_distance = 10.0, evd_file=None, add_blende evd_storage = evd.evd_file(evd_file, tof_res_x, tof_res_y, max_distance) - 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] distance_noise = random.gauss(noise_mu, noise_sigma) #If everything works substitute the previous line with this #distance_noise = pixel_noise[returns[idx][-1]] + random.gauss(noise_mu, noise_sigma) + # 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]] 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] - + # Calculate noisy point. vector_length_noise = vector_length+distance_noise if backfolding: #Distances > max_distance/2..max_distance are mapped to 0..max_distance/2 @@ -159,7 +162,7 @@ def scan_advanced(scanner_object, max_distance = 10.0, evd_file=None, add_blende 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], idx=returns[i][-1]) + 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], idx=returns[i][-1]) if evd_file: evd_storage.appendEvdFile() From d9ce4955da379c3b21dc5ef7dd6c0d9d8edbaed4 Mon Sep 17 00:00:00 2001 From: Joachim Gehrung Date: Mon, 12 Aug 2019 18:18:34 +0200 Subject: [PATCH 2/2] Fixed point cloud visualization. Point cloud visualization broke due to changes in the buffer. Changed the buffer indices to fix this. --- release/scripts/addons/blensor/blendodyne.py | 5 ++--- release/scripts/addons/blensor/generic_lidar.py | 4 ++-- release/scripts/addons/blensor/ibeo.py | 4 ++-- release/scripts/addons/blensor/kinect.py | 4 ++-- release/scripts/addons/blensor/tof.py | 4 ++-- 5 files changed, 10 insertions(+), 11 deletions(-) diff --git a/release/scripts/addons/blensor/blendodyne.py b/release/scripts/addons/blensor/blendodyne.py index 2f0029a21b..71d0ef50d8 100644 --- a/release/scripts/addons/blensor/blendodyne.py +++ b/release/scripts/addons/blensor/blendodyne.py @@ -193,7 +193,6 @@ def scan_advanced(scanner_object, rotation_speed = 10.0, simulation_fps=24, angl reusable_vector = Vector([0.0,0.0,0.0,1.0]) vp = (world_transformation * reusable_vector).xyz - print(world_transformation) for i in range(len(returns)): idx = returns[i][-1] @@ -227,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() diff --git a/release/scripts/addons/blensor/generic_lidar.py b/release/scripts/addons/blensor/generic_lidar.py index 159f538a55..276eb260a3 100644 --- a/release/scripts/addons/blensor/generic_lidar.py +++ b/release/scripts/addons/blensor/generic_lidar.py @@ -195,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() diff --git a/release/scripts/addons/blensor/ibeo.py b/release/scripts/addons/blensor/ibeo.py index 469aac0dab..cf32b41446 100644 --- a/release/scripts/addons/blensor/ibeo.py +++ b/release/scripts/addons/blensor/ibeo.py @@ -192,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() diff --git a/release/scripts/addons/blensor/kinect.py b/release/scripts/addons/blensor/kinect.py index 37e86c1633..0ea163b009 100644 --- a/release/scripts/addons/blensor/kinect.py +++ b/release/scripts/addons/blensor/kinect.py @@ -383,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() diff --git a/release/scripts/addons/blensor/tof.py b/release/scripts/addons/blensor/tof.py index ff644bd085..49a8663953 100644 --- a/release/scripts/addons/blensor/tof.py +++ b/release/scripts/addons/blensor/tof.py @@ -174,10 +174,10 @@ def scan_advanced(scanner_object, max_distance = 10.0, evd_file=None, add_blende 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()