Experiment with prediction

README.md

@@ -16,8 +16,9 @@ These are roughly the steps to go from datagathering to training
 2. Follow the steps in the auxilary  [traptools](https://git.rubenvandeven.com/security_vision/traptools) repository to obtain (1) camera matrix, lens distortion, image dimensions, and (2+3) homography
 3. Run the tracker, e.g. `poetry run tracker --detector ultralytics  --homography ../DATASETS/NAME/homography.json   --video-src ../DATASETS/NAME/*.mp4 --calibration ../DATASETS/NAME/calibration.json  --save-for-training EXPERIMENTS/raw/NAME/`
     * Note: You can run this right of the camera stream: `poetry run tracker --eval_device cuda:0 --detector ultralytics   --video-src  rtsp://USER:PW@ADDRESS/STREAM --homography ../DATASETS/NAME/homography.json   --calibration ../DATASETS/NAME/calibration.json  --save-for-training EXPERIMENTS/raw/NAME/`, each recording adding a new file to the `raw` folder.
-4. Parse tracker data to Trajectron format: `poetry run process_data --src-dir EXPERIMENTS/raw/NAME --dst-dir EXPERIMENTS/trajectron-data/ --name NAME`
+4. Parse tracker data to Trajectron format: `poetry run process_data --src-dir EXPERIMENTS/raw/NAME --dst-dir EXPERIMENTS/trajectron-data/ --name NAME` Optionally, smooth tracks: `--smooth-tracks`
 5. Train Trajectron model `poetry run trajectron_train --eval_every 10 --vis_every 1 --train_data_dict NAME_train.pkl --eval_data_dict NAME_val.pkl --offline_scene_graph no --preprocess_workers 8 --log_dir EXPERIMENTS/models --log_tag _NAME --train_epochs 100 --conf EXPERIMENTS/config.json --batch_size 256 --data_dir EXPERIMENTS/trajectron-data `
 6. The run!
-    * On a video file (you can use a wildcard) `DISPLAY=:1 poetry run trapserv --remote-log-addr 100.69.123.91 --eval_device cuda:0 --detector ultralytics  --homography ../DATASETS/NAME/homography.json   --video-src ../DATASETS/NAME/*.mp4 --model_dir EXPERIMENTS/models/models_DATE_NAME/--smooth-predictions  --num-samples 3  --render-window --calibration ../DATASETS/NAME/calibration.json` (the DISPLAY environment variable is used here to running over SSH connection and display on local monitor)
-    * or on the RTSP stream. Which uses gstreamer to substantially reduce latency compared to the default ffmpeg bindings in OpenCV.
+    * On a video file (you can use a wildcard) `DISPLAY=:1 poetry run trapserv --remote-log-addr 100.69.123.91 --eval_device cuda:0 --detector ultralytics  --homography ../DATASETS/NAME/homography.json --eval_data_dict EXPERIMENTS/trajectron-data/hof2s-m_test.pkl  --video-src ../DATASETS/NAME/*.mp4 --model_dir EXPERIMENTS/models/models_DATE_NAME/--smooth-predictions  --smooth-tracks --num-samples 3  --render-window --calibration ../DATASETS/NAME/calibration.json` (the DISPLAY environment variable is used here to running over SSH connection and display on local monitor)
+    * or on the RTSP stream. Which uses gstreamer to substantially reduce latency compared to the default ffmpeg bindings in OpenCV.
+    * To just have a single trajectory pulled from distribution use `--full-dist`. Also try `--z_mode`.

trap/animation_renderer.py

@@ -277,7 +277,7 @@ class AnimationRenderer:
             self.video_sprite = pyglet.sprite.Sprite(img=img, batch=self.batch_bg)
             # transform to flipped coordinate system for pyglet
             self.video_sprite.y = self.window.height - self.video_sprite.height
-            self.video_sprite.opacity = 10
+            self.video_sprite.opacity = 90
         except zmq.ZMQError as e:
             # idx = frame.index if frame else "NONE"
             # logger.debug(f"reuse video frame {idx}")

trap/config.py

@@ -207,16 +207,20 @@ inference_parser.add_argument('--num-samples',
                     default=5)
 inference_parser.add_argument("--full-dist",
                     help="Trajectron.incremental_forward parameter",
-                    type=bool,
-                    default=False)
+                    action='store_true')
 inference_parser.add_argument("--gmm-mode",
                     help="Trajectron.incremental_forward parameter",
                     type=bool,
                     default=True)
 inference_parser.add_argument("--z-mode",
                     help="Trajectron.incremental_forward parameter",
-                    type=bool,
-                    default=False)
+                    action='store_true')
+inference_parser.add_argument('--cm-to-m',
+                              help="Correct for homography that is in cm (i.e. {x,y}/100). Should also be used when processing data",
+                              action='store_true')
+inference_parser.add_argument('--center-data',
+                              help="Center data around cx and cy. Should also be used when processing data",
+                              action='store_true')
 
 
 # Internal connections.

trap/plumber.py

@@ -1,6 +1,6 @@
 import atexit
 import logging
-from logging.handlers import SocketHandler
+from logging.handlers import SocketHandler, QueueHandler, QueueListener
 from multiprocessing import Event, Process, Queue
 import multiprocessing
 import signal
@@ -18,6 +18,7 @@ from trap.tracker import run_tracker
 logger = logging.getLogger("trap.plumbing")
 
 
+
 class ExceptionHandlingProcess(Process):
             
     def run(self):
@@ -45,25 +46,37 @@ def start():
     loglevel = logging.NOTSET if args.verbose > 1 else logging.DEBUG if args.verbose > 0  else logging.INFO
     # print(args)
     # exit()
-    logging.basicConfig(
-        level=loglevel,
-    )
-
-    # set per handler, so we can set it lower for the root logger if remote logging is enabled
-    root_logger = logging.getLogger()
-    [h.setLevel(loglevel) for h in root_logger.handlers]
 
     isRunning = Event()
     isRunning.set()
     
-
-
+    q = multiprocessing.Queue(-1)
+    queue_handler = QueueHandler(q)
+    stream_handler = logging.StreamHandler()
+    log_handlers = [stream_handler]
 
     if args.remote_log_addr:
         logging.captureWarnings(True)
-        root_logger.setLevel(logging.NOTSET)  # to send all records to cutelog
+        # root_logger.setLevel(logging.NOTSET)  # to send all records to cutelog
         socket_handler = SocketHandler(args.remote_log_addr, args.remote_log_port)
-        root_logger.addHandler(socket_handler)
+        socket_handler.setLevel(logging.NOTSET)
+        log_handlers.append(socket_handler)
+
+    queue_listener = QueueListener(q, *log_handlers, respect_handler_level=True)
+    queue_listener.start()
+
+    # root = logging.getLogger()
+    logging.basicConfig(
+        level=loglevel,
+        handlers=[queue_handler]
+    )
+
+    # root_logger = logging.getLogger()
+    # # set per handler, so we can set it lower for the root logger if remote logging is enabled
+    # [h.setLevel(loglevel) for h in root_logger.handlers]
+
+    
+        # queue_listener.handlers.append(socket_handler)
 
 
 

trap/prediction_server.py

@@ -119,14 +119,28 @@ def get_maps_for_input(input_dict, scene, hyperparams):
 def history_cm_to_m(history):
     return [(h[0]/100, h[1]/100) for h in history]
 
+# TODO)) variable. Now placeholders for hof2 dataset
+cx = 11.874955125
+cy = 7.186118765
+
 def prediction_m_to_cm(source):
     # histories_dict[t][node] 
     for t in source:
         for node in source[t]:
+            # source[t][node][:,0] += cx
+            # source[t][node][:,1] += cy
             source[t][node] *= 100
     # print(t,node, source[t][node])
     return source
 
+def offset_trajectron_dict(source, x, y):
+    # histories_dict[t][node] 
+    for t in source:
+        for node in source[t]:
+            source[t][node][:,0] += x
+            source[t][node][:,1] += y
+    return source
+
 class PredictionServer:
     def __init__(self, config: Namespace, is_running: Event):
         self.config = config
@@ -136,7 +150,7 @@ class PredictionServer:
             logger.warning("Running on CPU. Specifying --eval_device cuda:0 should dramatically speed up prediction")
 
         if self.config.smooth_predictions:
-            self.smoother = Smoother(window_len=4)
+            self.smoother = Smoother(window_len=12, convolution=True) # convolution seems fine for predictions
         
         context = zmq.Context()
         self.trajectory_socket: zmq.Socket = context.socket(zmq.SUB)
@@ -167,6 +181,7 @@ class PredictionServer:
         if not os.path.exists(config_file):
             raise ValueError('Config json not found!')
         with open(config_file, 'r') as conf_json:
+            logger.info(f"Load config from {config_file}")
             hyperparams = json.load(conf_json)
 
         # Add hyperparams from arguments
@@ -284,10 +299,15 @@ class PredictionServer:
                     # TODO: modify this into a mapping function between JS data an the expected Node format
                     # node = FakeNode(online_env.NodeType.PEDESTRIAN)
                     history = [[h['x'], h['y']] for h in track.get_projected_history_as_dict(frame.H, self.config.camera)]
-                    history = history_cm_to_m(history)
+                    if self.config.cm_to_m:
+                        history = history_cm_to_m(history)
+                    
                     history = np.array(history)
-                    x = history[:, 0]
-                    y = history[:, 1]
+                    x = history[:, 0] #- cx
+                    y = history[:, 1] #- cy
+                    if self.config.center_data:
+                        x -= cx
+                        y -= cy
                     # TODO: calculate dt based on input
                     vx = derivative_of(x, 0.1) #eval_scene.dt
                     vy = derivative_of(y, 0.1)
@@ -349,7 +369,7 @@ class PredictionServer:
                                                             maps,
                                                             prediction_horizon=self.config.prediction_horizon, # TODO: make variable
                                                             num_samples=self.config.num_samples, # TODO: make variable
-                                                            full_dist=self.config.full_dist, # "The model’s full sampled output, where z and y are sampled sequentially"
+                                                            full_dist=self.config.full_dist, # "The mol’des full sampled output, where z and y are sampled sequentially"
                                                             gmm_mode=self.config.gmm_mode, # "If True: The mode of the Gaussian Mixture Model (GMM) is sampled (see trajectron.model.mgcvae.py)"
                                                             z_mode=self.config.z_mode # "Predictions from the model’s most-likely high-level latent behavior mode" (see trajecton.models.components.discrete_latent:sample_p(most_likely_z=z_mode))
                                                             )
@@ -376,7 +396,12 @@ class PredictionServer:
                                                                                         )
             
             
-            prediction_dict, histories_dict, futures_dict = prediction_m_to_cm(prediction_dict), prediction_m_to_cm(histories_dict), prediction_m_to_cm(futures_dict)
+            # if self.config.center_data:
+            #     prediction_dict, histories_dict, futures_dict = offset_trajectron_dict(prediction_dict, cx, cy), offset_trajectron_dict(histories_dict, cx, cy), offset_trajectron_dict(futures_dict, cx, cy)
+
+            if self.config.cm_to_m:
+                # convert back to fit homography
+                prediction_dict, histories_dict, futures_dict = prediction_m_to_cm(prediction_dict), prediction_m_to_cm(histories_dict), prediction_m_to_cm(futures_dict)
             
             
             assert(len(prediction_dict.keys()) <= 1)
@@ -414,7 +439,7 @@ class PredictionServer:
             if self.config.predict_training_data:
                 logger.info(f"Frame prediction: {len(trajectron.nodes)} nodes & {trajectron.scene_graph.get_num_edges()} edges. Trajectron: {end - start}s")
             else:
-                logger.info(f"Total frame delay = {time.time()-frame.time}s ({len(trajectron.nodes)} nodes & {trajectron.scene_graph.get_num_edges()} edges. Trajectron: {end - start}s)")
+                logger.debug(f"Total frame delay = {time.time()-frame.time}s ({len(trajectron.nodes)} nodes & {trajectron.scene_graph.get_num_edges()} edges. Trajectron: {end - start}s)")
             
             if self.config.smooth_predictions:
                 frame = self.smoother.smooth_frame_predictions(frame)

trap/preview_renderer.py

@@ -153,15 +153,17 @@ class DrawnTrack:
             if ci >= len(self.shapes):
             # TODO: add color2
                 line = self.renderer.gradientLine(x, y, x2, y2, 3, color, color, batch=self.renderer.batch_anim)
-                line.opacity = 5
+                line = pyglet.shapes.Arc(x2, y2, 10, thickness=3, color=color, batch=self.renderer.batch_anim)
+                line.opacity = 20
                 self.shapes.append(line)
                 
             else:
                 line = self.shapes[ci-1]
                 line.x, line.y = x, y
                 line.x2, line.y2 = x2, y2
+                line.radius = int(exponentialDecay(line.radius, 2, 3, dt))
                 line.color = color
-                line.opacity = int(exponentialDecay(line.opacity, 180, 3, dt))
+                line.opacity = int(exponentialDecay(line.opacity, 180, 8, dt))
     
         # TODO: basically a duplication of the above,  do this smarter?
         # TODO: add intermediate segment

trap/process_data.py

@@ -6,18 +6,24 @@ import pandas as pd
 import dill
 import tqdm
 import argparse
+from typing import List 
+
+from trap.tracker import Smoother
 
 #sys.path.append("../../")
 from trajectron.environment import Environment, Scene, Node
 from trajectron.utils import maybe_makedirs
 from trajectron.environment import derivative_of
 
+FPS = 12
 desired_max_time = 100
 pred_indices = [2, 3]
 state_dim = 6
 frame_diff = 10
 desired_frame_diff = 1
-dt = 0.1 # dt per frame (e.g. 1/FPS)
+dt = 1/FPS # dt per frame (e.g. 1/FPS)
+smooth_window = FPS * 1.5 # see also tracker.py
+min_track_length = 10
 
 standardization = {
     'PEDESTRIAN': {
@@ -84,7 +90,7 @@ def augment(scene):
 # maybe_makedirs('trajectron-data')
 # for desired_source in [ 'hof2', ]:#  ,'hof-maskrcnn', 'hof-yolov8', 'VIRAT-0102-parsed', 'virat-resnet-keypoints-full']:
 
-def process_data(src_dir: Path, dst_dir: Path, name: str):
+def process_data(src_dir: Path, dst_dir: Path, name: str, smooth_tracks: bool, cm_to_m: bool, center_data: bool):
     print(f"Process data in {src_dir}, to {dst_dir}, identified by {name}")
     
     nl = 0
@@ -93,6 +99,31 @@ def process_data(src_dir: Path, dst_dir: Path, name: str):
     skipped_for_error = 0
     created = 0
 
+    smoother = Smoother(window_len=smooth_window, convolution=False) if smooth_tracks else None
+
+
+    files = list(src_dir.glob("*/*.txt"))
+    print(files)
+    all_data = pd.concat((pd.read_csv(f, sep='\t', index_col=False, header=None) for f in files), axis=0, ignore_index=True)
+    print(all_data.shape)
+    if all_data.shape[1] == 8:
+        all_data.columns = ['frame_id', 'track_id', 'l','t', 'w','h', 'pos_x', 'pos_y']
+    elif all_data.shape[1] == 9:
+        all_data.columns = ['frame_id', 'track_id', 'l','t', 'w','h', 'pos_x', 'pos_y', 'state']
+    else:
+        raise Exception("Unknown data format. Check column count")
+    
+    if cm_to_m:
+        all_data['pos_x'] /= 100
+        all_data['pos_y'] /= 100
+    
+    mean_x, mean_y = all_data['pos_x'].mean(), all_data['pos_y'].mean()
+    cx = .5 * all_data['pos_x'].min() + .5 * all_data['pos_x'].max()
+    cy = .5 * all_data['pos_y'].min() + .5 * all_data['pos_y'].max()
+    print(f"Dataset means: {mean_x=} {mean_y=}")
+    print(f"Dataset centers: {cx=} {cy=}")
+
+
     for data_class in ['train', 'val', 'test']:
         env = Environment(node_type_list=['PEDESTRIAN'], standardization=standardization)
         attention_radius = dict()
@@ -102,11 +133,13 @@ def process_data(src_dir: Path, dst_dir: Path, name: str):
         scenes = []
         split_id = f"{name}_{data_class}"
         data_dict_path = dst_dir / (split_id + '.pkl')
+        subpath = src_dir / data_class
 
         print(data_dict_path)
 
         
-        subpath = src_dir / data_class        
+
+            
         for file in subpath.glob("*.txt"):
                 print(file)
                 input_data_dict = dict()
@@ -132,12 +165,24 @@ def process_data(src_dir: Path, dst_dir: Path, name: str):
                 data['node_id'] = data['track_id'].astype(str)
                 data.sort_values('frame_id', inplace=True)
 
+                # cm to m
+                if cm_to_m:
+                    data['pos_x'] /= 100
+                    data['pos_y'] /= 100
+                
+                if center_data:
+                    data['pos_x'] -= cx
+                    data['pos_y'] -= cy
+
                 # Mean Position
 
                 print("Means: x:", data['pos_x'].mean(), "y:", data['pos_y'].mean())
 
-                data['pos_x'] = data['pos_x'] - data['pos_x'].mean()
-                data['pos_y'] = data['pos_y'] - data['pos_y'].mean()
+                # TODO)) If this normalization is here, it should also be in prediction_server.py
+                # data['pos_x'] = data['pos_x'] - data['pos_x'].mean()
+                # data['pos_y'] = data['pos_y'] - data['pos_y'].mean()
+                # data['pos_x'] = data['pos_x'] - cx
+                # data['pos_y'] = data['pos_y'] - cy
 
                 max_timesteps = data['frame_id'].max()
 
@@ -156,14 +201,19 @@ def process_data(src_dir: Path, dst_dir: Path, name: str):
 
 
                     node_values = node_df[['pos_x', 'pos_y']].values
+                    
 
-                    if node_values.shape[0] < 2:
+                    if node_values.shape[0] < min_track_length:
                         continue
 
                     new_first_idx = node_df['frame_id'].iloc[0]
 
                     x = node_values[:, 0]
                     y = node_values[:, 1]
+                    if smoother:
+                        x = smoother.smooth(x)
+                        y = smoother.smooth(y)
+
                     vx = derivative_of(x, scene.dt)
                     vy = derivative_of(y, scene.dt)
                     ax = derivative_of(vx, scene.dt)
@@ -209,6 +259,10 @@ def main():
     parser.add_argument("--src-dir", "-s", type=Path, required=True, help="Directory with tracker output in .txt files")
     parser.add_argument("--dst-dir", "-d", type=Path, required=True, help="Destination directory to store parsed .pkl files (typically 'trajectron-data')")
     parser.add_argument("--name", "-n", type=str, required=True, help="Identifier to prefix the output .pkl files with (result is NAME-train.pkl, NAME-test.pkl)")
+    parser.add_argument("--smooth-tracks", action='store_true', help=f"Enable smoother. Set to {smooth_window} frames")
+    parser.add_argument("--cm-to-m", action='store_true', help=f"If homography is in cm, convert tracked points to meter for beter results")
+    parser.add_argument("--center-data", action='store_true', help=f"Normalise around center")
     
     args = parser.parse_args()
-    process_data(**args.__dict__)
+    process_data(**args.__dict__)
+

trap/tools.py

@@ -60,7 +60,7 @@ def tracker_preprocess():
                 total += len(detections)
                 # detections = _yolov8_track(frame, model, imgsz=1440, classes=[0])
 
-                bar.set_description(f"[{video_nr}/{len(video_srcs)}] [{i}/{frame_count}] {str(video_path)} -- Detections {len(detections)}: {[d.conf for d in detections]} (so far {total})")
+                bar.set_description(f"[{video_nr}/{len(video_srcs)}] [{i}/{frame_count}] {str(video_path)} -- Detections {len(detections)}: {[d.track_id for d in detections]} (so far {total})")
             
                 for detection in detections:
                     track = tracks[detection.track_id]

trap/tracker.py

@@ -61,6 +61,7 @@ class Multifile():
     def __init__(self, srcs: List[Path]):
         self.srcs = srcs
         self.g = self.__iter__()
+        self.current_file = None
 
     @property
     def name(self):
@@ -68,6 +69,7 @@ class Multifile():
     
     def __iter__(self):
         for path in self.srcs:
+            self.current_file = path.name
             with path.open('r') as fp:
                 for l in fp:
                     yield l
@@ -124,6 +126,7 @@ class TrainingDataWriter:
                     # if t.history[-1].state != DetectionState.Lost
                     ])
         self.count += len(tracks)
+        
     
     def __exit__(self, exc_type, exc_value, exc_tb):
         # ... ignore exception (type, value, traceback)
@@ -148,14 +151,33 @@ class TrainingDataWriter:
 
         logger.info(f"Splitting gathered data from {sources.name}")
         # for source_file in source_files:
+        
         for name, line_nrs in lines.items():
             dir_path = self.path / name
             dir_path.mkdir(exist_ok=True)
             file = dir_path / 'tracked.txt'
             logger.debug(f"- Write {line_nrs} lines to {file}")
             with file.open('w') as target_fp:
+                max_track_id = 0
+                offset = 0
+                prev_file = None
                 for i in range(line_nrs):
-                    target_fp.write(sources.readline())
+                    line = sources.readline()
+                    current_file = sources.current_file
+                    if prev_file != current_file:
+                        offset = max_track_id
+                        
+                        logger.debug(f'{name} - update offset {offset} ({sources.current_file})')
+                        prev_file = current_file
+
+                    parts = line.split('\t')
+                    track_id = int(parts[1]) + offset
+                    
+                    if track_id > max_track_id:
+                        max_track_id = track_id
+                    
+                    parts[1] = str(track_id)
+                    target_fp.write("\t".join(parts))
                     
 
         
@@ -220,7 +242,8 @@ class Tracker:
 
         if self.config.smooth_tracks:
             logger.info("Smoother enabled")
-            self.smoother = Smoother()
+            fps = 12 # TODO)) make configurable, or get from cam
+            self.smoother = Smoother(window_len=fps*5, convolution=False)
         else:
             logger.info("Smoother Disabled (enable with --smooth-tracks)")
 
@@ -250,6 +273,9 @@ class Tracker:
         prev_frame_i = -1
         
         with TrainingDataWriter(self.config.save_for_training) as writer:
+            end_time = None
+            tracker_dt = None
+            w_time = None
             while self.is_running.is_set():
                 # this waiting for target_dt causes frame loss. E.g. with target_dt at .1, it
                 # skips exactly 1 frame on a 10 fps video (which, it obviously should not do)
@@ -259,9 +285,10 @@ class Tracker:
                 # time.sleep(max(0, prev_run_time - this_run_time + TARGET_DT))
                 # prev_run_time = time.time()
 
+                poll_time = time.time()
                 zmq_ev = self.frame_sock.poll(timeout=2000)
                 if not zmq_ev:
-                    logger.warn('skip poll after 2000ms')
+                    logger.warning('skip poll after 2000ms')
                     # when there's no data after timeout, loop so that is_running is checked
                     continue
 
@@ -269,7 +296,10 @@ class Tracker:
                 frame: Frame = self.frame_sock.recv_pyobj() # frame delivery in current setup: 0.012-0.03s
 
                 if frame.index > (prev_frame_i+1):
-                    logger.warn(f"Dropped {frame.index - prev_frame_i - 1} frames ({frame.index=}, {prev_frame_i=})")
+                    logger.warning(f"Dropped {frame.index - prev_frame_i - 1} frames ({frame.index=}, {prev_frame_i=}) -- poll time {start_time-poll_time:.5f}")
+                    if tracker_dt:
+                        logger.warning(f"last loop took {tracker_dt} (finished {start_time - end_time:0.5f} ago, writing took {w_time-end_time} and finshed {start_time - w_time} ago).. {writer.path}")
+
 
                 
                 prev_frame_i = frame.index
@@ -283,7 +313,7 @@ class Tracker:
 
 
                 if self.config.detector == DETECTOR_YOLOv8:
-                    detections: [Detection]  = _yolov8_track(frame, self.model, classes=[0])
+                    detections: [Detection]  = _yolov8_track(frame, self.model, classes=[0], imgsz=[1152, 640])
                 else :
                     detections: [Detection] = self._resnet_track(frame.img, scale = 1)
                 
@@ -327,17 +357,27 @@ class Tracker:
 
                 self.trajectory_socket.send_pyobj(frame)
                     
-                current_time = time.time()
-                logger.debug(f"Trajectories: {len(active_tracks)}. Current frame delay = {current_time-frame.time}s (trajectories: {current_time - start_time}s)")
+                end_time = time.time()
+                tracker_dt = end_time - start_time
+                
+
+                # having {end_time-frame.time} creates incidental delay... don't know why, maybe because of send?. So add n/a for now
+                # or is it {len(active_tracks)}     or  {tracker_dt}
+                # logger.debug(f"Trajectories: n/a. Current frame delay = n/a s (trajectories:s)")
                 
                 # self.trajectory_socket.send_string(json.dumps(trajectories))
                 # provide a {ID: {id: ID, history: [[x,y],[x,y],...]}}
                 # TODO: provide a track object that actually keeps history (unlike tracker)
 
-                #TODO calculate fps (also for other loops to see asynchonity)
+                # TODO calculate fps (also for other loops to see asynchonity)
                 # fpsfilter=fpsfilter*.9+(1/dt)*.1    #trust value in order to stabilize fps display
+                
                 writer.add(frame, active_tracks.values())
                 
+                w_time = time.time()
+                
+                
+                
                 
             
         logger.info('Stopping')
@@ -396,8 +436,19 @@ def run_tracker(config: Namespace, is_running: Event):
 
 class Smoother:
 
-    def __init__(self, window_len=2):
-        self.smoother = ConvolutionSmoother(window_len=window_len, window_type='ones', copy=None)
+    def __init__(self, window_len=6, convolution=False):
+        # for some reason this smoother messes the predictions. Probably skews the points too much??
+        if convolution:
+            self.smoother = ConvolutionSmoother(window_len=window_len, window_type='ones', copy=None)
+        else:
+            # "Unlike Kalman filtering, which focuses on predicting and updating the current state using historical measurements, Kalman smoothing enhances the accuracy of past state values"
+            # see https://medium.com/@shahalkp1/kalman-smoothing-using-tsmoothie-0175260464e5
+            self.smoother = KalmanSmoother(component='level_trend_season',  component_noise={'level':0.03, 'season': .02, 'trend':0.04},n_seasons = 2, copy=None)
+
+    
+    def smooth(self, points: List[float]):
+        self.smoother.smooth(points)
+        return self.smoother.smooth_data[0]
         
     
     def smooth_frame_tracks(self, frame: Frame) -> Frame: