diff --git a/release/scripts/addons/blensor/blendodyne.py b/release/scripts/addons/blensor/blendodyne.py
index 9db77a659c..71d0ef50d8 100644
--- a/release/scripts/addons/blensor/blendodyne.py
+++ b/release/scripts/addons/blensor/blendodyne.py
@@ -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)
 
@@ -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()
 
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..276eb260a3 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)
@@ -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()
 
diff --git a/release/scripts/addons/blensor/ibeo.py b/release/scripts/addons/blensor/ibeo.py
index bf4e03a76f..cf32b41446 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)
             
@@ -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()
 
diff --git a/release/scripts/addons/blensor/kinect.py b/release/scripts/addons/blensor/kinect.py
index 8a08e7d658..0ea163b009 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()
@@ -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()  
         
diff --git a/release/scripts/addons/blensor/tof.py b/release/scripts/addons/blensor/tof.py
index 5a8169df89..49a8663953 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()
@@ -171,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()