align z now never use stationary times after crash

__init__.py

@@ -30,7 +30,7 @@
 bpy.types.Scene.use_gps = BoolProperty(
     name="Use GPS Data",
     description="Use GPS data",
-    default = True)
+    default = False)
 
 bpy.types.Scene.crash = BoolProperty(
     name="Reconstruct crash",
@@ -128,8 +128,6 @@ def execute(self, context):
             # make car object in first scene local so when the object in the second scene will be deleted
             # this object will not be
             obj = scene.objects['Camaro'].make_local()
-            # set the car object as the active object in the first scene
-            scene.objects.active = obj
             # for each material slot in the car object
             for material_slot in obj.material_slots:
                 # set to not use nodes for material, so the car is not grey
@@ -141,6 +139,8 @@ def execute(self, context):
             bpy.data.scenes.remove(scene2, True)
         # get car object from scene
         obj = scene.objects['Camaro']
+        # set the car object as the active object in the first scene
+        scene.objects.active = obj
         # get from checkbox if using gps data to reconstruct
         use_gps = scene.use_gps
         # get from checkbox if using crash mode
@@ -174,7 +174,7 @@ def execute(self, context):
             step = 1
         # number of keyframes that will be inserted
         n_keyframe = math.ceil(positions.shape[1] / step)
-
+        scene.frame_start = 0
         if not crash:
             scene.frame_end = round(times[-1] * fps)
         else:
@@ -198,7 +198,10 @@ def execute(self, context):
             fcurve_kinematic = obj.animation_data.action.fcurves.new(data_path="rigid_body.kinematic")
             fcurve_kinematic.keyframe_points.add(3)
             fcurve_kinematic.keyframe_points[0].co = 0, True
-            obj.location = positions[:, -1]
+            obj.location.x = positions[0,-1]
+            obj.location.y = positions[1,-1]
+            if ignore_z:
+                obj.location.z = 0.55
             # set keyframe at the end using physics engine
             fcurve_kinematic.keyframe_points[1].co = round((times[-1] * fps) - 10), True
             fcurve_kinematic.keyframe_points[2].co = round(times[-1] * fps), False
@@ -237,12 +240,12 @@ def execute(self, context):
             bpy.ops.ptcache.free_bake_all()
             bpy.ops.ptcache.bake_all()
 
-        #create a curve that shows the vehicle path
+        # create a curve that shows the vehicle path
         curveData = bpy.data.curves.new('myCurve', type='CURVE')
         curveData.dimensions = '3D'
         curveData.resolution_u = 2
         polyline = curveData.splines.new('POLY')
-        # -1 for avoinding closing curve
+        # -1 for avoiding closing curve
         polyline.points.add(positions.shape[1]-1)
         for i, location in enumerate(positions.T):
              # set x,y,z,weight

src/__init__.py

@@ -42,8 +42,6 @@ def get_trajectory_from_path(path,use_gps=True,crash=False):
     :return: 3 numpy array: 3xn position, 1xn times, 4xn angular position as quaternions
     """
 
-    window_size = 20
-
     if (use_gps):
         print("using GPS")
         # currently default format is unmodified fullinertial but other formats are / will be supported
@@ -54,12 +52,14 @@ def get_trajectory_from_path(path,use_gps=True,crash=False):
 
         converts_measurement_units(accelerations, angular_velocities, gps_speed, coordinates, heading)
 
-        times, gps_speed, accelerations, angular_velocities, coordinates, heading = \
+        times, gps_speed, accelerations, angular_velocities, coordinates, heading, crash_time = \
             truncate_if_crash(crash, times, gps_speed, accelerations, angular_velocities, coordinates, heading)
 
         # get positions from GNSS data
         gnss_positions, headings_2 = get_positions(coordinates, altitudes)
 
+        window_size = 20
+
         # reduce accelerations disturbance
         times, accelerations = reduce_disturbance(times, accelerations, window_size)
         # reduce angular velocities disturbance
@@ -73,7 +73,7 @@ def get_trajectory_from_path(path,use_gps=True,crash=False):
         real_speeds = np.linalg.norm(real_velocities, axis=0)
 
         # get time windows where vehicle is stationary
-        stationary_times = get_stationary_times(gps_speed,period)
+        stationary_times = get_stationary_times(gps_speed,period,crash_time)
 
         # clear gyroscope drift
         angular_velocities = clear_gyro_drift(angular_velocities, stationary_times)
@@ -119,16 +119,16 @@ def get_trajectory_from_path(path,use_gps=True,crash=False):
 
         converts_measurement_units(accelerations, angular_velocities, gps_speed)
 
-        times, gps_speed, accelerations, angular_velocities, _, _ = \
+        times, gps_speed, accelerations, angular_velocities, _, _ , crash_time = \
             truncate_if_crash(crash, times, gps_speed, accelerations, angular_velocities)
 
         # reduce accelerations disturbance
-        times, accelerations = reduce_disturbance(times, accelerations, window_size)
+        times, accelerations = reduce_disturbance(times, accelerations)
         # reduce angular velocities disturbance
-        _, angular_velocities = reduce_disturbance(times, angular_velocities, window_size)
+        _, angular_velocities = reduce_disturbance(times, angular_velocities)
 
         # get time windows where vehicle is stationary
-        stationary_times = get_stationary_times(gps_speed,period)
+        stationary_times = get_stationary_times(gps_speed, period, crash_time)
 
         # clear gyroscope drift
         angular_velocities = clear_gyro_drift(angular_velocities, stationary_times)

src/clean_data_utils.py

@@ -51,14 +51,18 @@ def parse_input(df):
     gps_speed = df['speed'].values.T
     return times, gps_speed, accelerations, angular_velocities
 
-def get_stationary_times(gps_speed,period):
+def get_stationary_times(gps_speed, period, crash_time=None):
     """ Returns list of index where the gps speed is near zero
 
     :param gps_speed: 1xn numpy array of gps speed in m/s
     :return: list of tuples, each one with start and final timestamp of a stationary time index
     """
 
     speed_threshold = 0.05  # 0.2 m/s
+    if crash_time is not None:
+        # truncate speeds after crash time because acceleremeter data is unreliable
+        # so the routines that use stationary_time don't work on data after crash
+        gps_speed = gps_speed[:crash_time]
     stationary_times = []
     # repeat until at least a stationary time is found
     while (len(stationary_times)==0):
@@ -99,6 +103,7 @@ def truncate_if_crash(crash, times, gps_speed, accelerations, angular_velocities
     Removed unreliable data after a crash
     If crash==True then search for a acceleration over a certain treshold, remove records after that
     Coordinates and heading are optional, for supporting both using gps data and not.
+    Returning also first crash time if present. If not present crash time is null
 
     :param crash: bool
     :param times: 1xn numpy array
@@ -107,12 +112,13 @@ def truncate_if_crash(crash, times, gps_speed, accelerations, angular_velocities
     :param angular_velocities: 3xn numpy array
     :param coordinates: 2xn numpy array
     :param heading: 1xn numpy array
-    :return:times, gps_speed, accelerations, angular_velocities, coordinates, [coordinates, heading]
+    :return:times, gps_speed, accelerations, angular_velocities, coordinates, [coordinates, heading, crash_time]
     """
-    if crash:
-        crashes = np.argwhere(np.linalg.norm(accelerations, axis=0) > crash_acceleration_treshold)
-        if (len(crashes) > 1):
-            crash_1 = crashes[0][0]
+    crashes = np.argwhere(np.linalg.norm(accelerations, axis=0) > crash_acceleration_treshold)
+    crash_1 = None
+    if (len(crashes) > 1):
+        crash_1 = crashes[0][0]
+        if crash:
             accelerations = accelerations[:, :crash_1]
             angular_velocities = angular_velocities[:, :crash_1]
             gps_speed = gps_speed[:crash_1]
@@ -121,7 +127,7 @@ def truncate_if_crash(crash, times, gps_speed, accelerations, angular_velocities
                 coordinates = coordinates[:,:crash_1]
             if heading is not None:
                 heading = heading[:crash_1]
-    return times, gps_speed, accelerations, angular_velocities, coordinates, heading
+    return times, gps_speed, accelerations, angular_velocities, coordinates, heading, crash_1
 
 def converts_measurement_units(accelerations, angular_velocities, gps_speed=None, coordinates=None, heading=None):
     """ Convert physics quantity measurement unit