msight-vision 0.1.0__py3-none-any.whl

msight_vision/localizer.py

@@ -0,0 +1,32 @@
+class LocalizerBase:
+    def __init__(self):
+        pass
+
+    def localize(self):
+        raise NotImplementedError(
+            "This method should be overridden by subclasses")
+
+
+class HashLocalizer(LocalizerBase):
+    """Hash-based localizer for 2D images.
+    This localizer looks up the pixel values in a hash table to find the corresponding location in the image.
+    """
+
+    def __init__(self, lat_map, lon_map):
+        super().__init__()
+        self.lat_map = lat_map
+        self.lon_map = lon_map
+
+    def localize(self, detection2d_result):
+        # Perform localization using the model and config
+        # This is a placeholder implementation
+        for obj in detection2d_result.object_list:
+            # Assuming obj has a method to get the pixel coordinates
+            bottom_center_x = int(obj.pixel_bottom_center[0])
+            bottom_center_y = int(obj.pixel_bottom_center[1])
+            lat = self.lat_map[bottom_center_y, bottom_center_x,]
+            lon = self.lon_map[bottom_center_y, bottom_center_x,]
+            obj.lat = lat
+            obj.lon = lon
+            # print(f"Object {obj.class_id} localized to lat: {lat}, lon: {lon}")
+        return detection2d_result

msight_vision/msight_core/__init__.py

@@ -0,0 +1,6 @@
+from .detection import YoloOneStageDetectionNode
+from .fusion import FuserNode
+from .tracking import SortTrackerNode
+from .viewer import RoadUserListViewerNode, DetectionResults2DViewerNode
+from .warper import WarperMatrixUpdaterNode
+from .state_estimation import FiniteDifferenceStateEstimatorNode

msight_vision/msight_core/detection.py

@@ -0,0 +1,103 @@
+from msight_core.nodes import DataProcessingNode, NodeConfig
+from msight_core.data import ImageData, DetectionResultsData
+import yaml
+from pathlib import Path
+import numpy as np
+from .. import YoloDetector, HashLocalizer, ClassicWarperWithExternalUpdate
+import torch
+import time
+from msight_core.utils import get_redis_client
+
+
+device = "cuda" if torch.cuda.is_available() else "cpu"
+
+def load_locmaps(loc_maps_path):
+    """
+    Load localization maps from the specified path.
+    :param loc_maps_path: path to the localization maps in the config file
+    :return: localization maps
+    """
+    result = {key: np.load(item) for key, item in loc_maps_path.items()}
+    return result
+
+class YoloOneStageDetectionNode(DataProcessingNode):
+    default_configs = NodeConfig(
+        publish_topic_data_type=DetectionResultsData
+    )
+    def __init__(self, configs, det_configs_path):
+        super().__init__(configs)
+        self.det_config_path = Path(det_configs_path)
+        self.detector = None
+        with open(self.det_config_path, "r") as f:
+            self.det_config = yaml.safe_load(f)
+        self.no_warp = self.det_config['warper_config']['no_warp']            
+        self.model_path = self.det_config["model_config"]["ckpt_path"]
+        self.confthre = self.det_config["model_config"]["confthre"]
+        self.nmsthre = self.det_config["model_config"]["nmsthre"]
+        self.class_agnostic_nms = self.det_config["model_config"]["class_agnostic_nms"]
+        self.logger.info(f"Initializing YoloOneStageDetectionNode with model path: {self.model_path}, no_warp: {self.no_warp}, confthre: {self.confthre}, nmsthre: {self.nmsthre}, class_agnostic_nms: {self.class_agnostic_nms}")
+        self.detector = YoloDetector(model_path=Path(self.model_path), device=device, confthre=self.confthre, nmsthre=self.nmsthre, fp16=False, class_agnostic_nms=self.class_agnostic_nms)
+        loc_maps_path = self.det_config["loc_maps"]
+        loc_maps = load_locmaps(loc_maps_path)
+        self.localizers = {key: HashLocalizer(item['x_map'], item['y_map']) for key, item in loc_maps.items()}
+        if not self.no_warp:
+            self.warper = ClassicWarperWithExternalUpdate()
+            self.warper_matrix_redis_prefix = self.det_config["warper_config"]["redis_prefix"]
+        else:
+            self.warper_matrix_redis_prefix = None
+        self.include_sensor_data_in_result = self.det_config["det_config"].get("include_sensor_data_in_result", False)
+        self.sensor_type = self.det_config["det_config"].get("sensor_type", "fisheye")
+
+    def get_warp_matrix_from_redis(self, sensor_name):
+        redis_client = get_redis_client()
+        key = self.warper_matrix_redis_prefix + f":{sensor_name}"
+        warp_matrix_str = redis_client.get(key)
+        
+        if warp_matrix_str is None:
+            self.logger.warning(f"No warp matrix found in Redis for sensor: {sensor_name}")
+            return None
+
+        # Decode bytes to string if needed
+        if isinstance(warp_matrix_str, bytes):
+            warp_matrix_str = warp_matrix_str.decode()
+
+        # Remove brackets and convert back to numpy array
+        warp_matrix = np.array(eval(warp_matrix_str))
+        
+        # You may need to reshape it, for example to 3x3 if that's your matrix size
+        warp_matrix = warp_matrix.reshape((3, 3))  # adjust the shape if your matrix is different
+        
+        return warp_matrix
+
+
+    def process(self, data: ImageData):
+        self.logger.info(f"Processing image data from sensor: {data.sensor_name}, frame: {data.frame_id}")
+        start = time.time()
+        image = data.to_ndarray()
+        sensor_name = data.sensor_name
+        frame_id = data.frame_id
+        # cv2.imshow("image", image)
+        # cv2.waitKey(1)
+        # print(image.shape)
+        timestamp = data.capture_timestamp
+        if not self.no_warp:
+            # print(f"Image shape before warping: {image.shape}")
+            warping_matrix = self.get_warp_matrix_from_redis(sensor_name)
+            image = self.warper.warp(image, warping_matrix)
+            # print(f"Image shape after warping: {image.shape}")
+        # cv2.imshow("image", image)
+        # cv2.waitKey(1)
+        # print(image.shape)
+        result = self.detector.detect(image, timestamp, self.sensor_type)
+        localizer = self.localizers[sensor_name]
+        localizer.localize(result)
+        self.logger.info(f"Detection completed in {time.time() - start:.2f} seconds for sensor: {sensor_name}")
+        def is_number(val):
+            return isinstance(val, (int, float, np.number)) and np.isfinite(val)
+        result.object_list = [obj for obj in result.object_list if is_number(obj.lat) and is_number(obj.lon)]
+        raw_sensor_data = None
+        if self.include_sensor_data_in_result:
+            raw_sensor_data = data
+        detection_result_data = DetectionResultsData(result, sensor_frame_id=data.frame_id, capture_timestamp=data.capture_timestamp, creation_timestamp=time.time(), sensor_name=sensor_name, raw_sensor_data=raw_sensor_data)
+        # print(f"Detection results: {detection_result_data.to_dict()}")
+        return detection_result_data

msight_vision/msight_core/fusion.py

@@ -0,0 +1,64 @@
+from msight_core.nodes import DataProcessingNode, NodeConfig
+from msight_core.data import RoadUserListData, DetectionResultsData
+from pathlib import Path
+import yaml
+import importlib.util
+import sys
+import copy
+
+def load_class_from_file(file_path: str, class_path: str):
+    module_name = Path(file_path).stem
+    spec = importlib.util.spec_from_file_location(module_name, file_path)
+    if spec is None:
+        raise ImportError(f"Cannot load module from {file_path}")
+    module = importlib.util.module_from_spec(spec)
+    sys.modules[module_name] = module
+    spec.loader.exec_module(module)
+
+    # Resolve class from class_path
+    components = class_path.split(".")
+    cls = module
+    for comp in components[1:]:  # Skip the module name
+        cls = getattr(cls, comp)
+    return cls
+
+class FuserNode(DataProcessingNode):
+    default_configs = NodeConfig(
+        publish_topic_data_type=RoadUserListData
+    )
+    def __init__(self, configs, fusion_configs_path):
+        super().__init__(configs)
+        self.config_file_path = Path(fusion_configs_path)
+        with open(self.config_file_path, "r") as f:
+            self.fusion_configs = yaml.safe_load(f)
+        self.fuser_class_path = self.fusion_configs["fuser_config"]["class_path"]
+        self.fuser_file_path = self.fusion_configs["fuser_config"]["file_path"]
+        self.sensor_list = self.fusion_configs["fuser_config"]["sensor_list"]
+        FuserClass = load_class_from_file(self.fuser_file_path, self.fuser_class_path)
+        self.fuser = FuserClass()
+        self.buffer = {sensor: None for sensor in self.sensor_list}
+        assert configs.sensor_name is not None, "sensor_name must be provided in configs for the fusion node."
+        self.sensor_name = self.configs.sensor_name
+
+    def process(self, data: DetectionResultsData):
+        self.buffer[data.sensor_name] = data.detection_result
+        # print(data.detection_result)
+        buffer = copy.copy(self.buffer)
+        self.logger.info(f"Processing detection results from sensor: {data.sensor_name}")
+        sensor_name = data.sensor_name
+        if sensor_name not in self.sensor_list:
+            raise ValueError(f"Sensor {sensor_name} not in configured sensor list: {self.sensor_list}")
+
+        for _, detection_result in buffer.items():
+            if detection_result is None:
+                return None
+        self.logger.info(f"All data received. Fusing detection results for sensor.")
+        fused_result = self.fuser.fuse(buffer)
+        road_user_list_data = RoadUserListData(
+            road_user_list=fused_result,
+            capture_timestamp=data.capture_timestamp,
+            sensor_name=self.sensor_name
+        )
+        self.buffer = {sensor: None for sensor in self.sensor_list}  # Reset buffer after fusion
+        # print(road_user_list_data)
+        return road_user_list_data

msight_vision/msight_core/state_estimation.py

@@ -0,0 +1,38 @@
+from msight_core.nodes import DataProcessingNode, NodeConfig
+from msight_core.data import RoadUserListData
+from .. import FiniteDifferenceStateEstimator
+from pathlib import Path
+import yaml
+import time
+
+class  FiniteDifferenceStateEstimatorNode(DataProcessingNode):
+    default_configs = NodeConfig(
+        publish_topic_data_type=RoadUserListData
+    )
+    def __init__(self, configs, state_estimation_configs_path):
+        super().__init__(configs)
+        self.config_file_path = Path(state_estimation_configs_path)
+        with open(self.config_file_path, "r") as f:
+            self.state_estimation_configs = yaml.safe_load(f)
+        self.frame_rate = self.state_estimation_configs["state_estimator_config"].get("frame_rate", 5)
+        self.frame_interval = self.state_estimation_configs["state_estimator_config"].get("frame_interval", 1)
+        self.dist_threshold = self.state_estimation_configs["state_estimator_config"].get("dist_threshold", 4)
+        self.state_estimator = FiniteDifferenceStateEstimator(
+            frame_rate=self.frame_rate,
+            frame_interval=self.frame_interval,
+            dist_threshold=self.dist_threshold
+        )
+
+    def process(self, data: RoadUserListData) -> RoadUserListData:
+        self.logger.info(f"Processing road user list data from sensor: {data.sensor_name}")
+        start = time.time()
+        road_user_list = data.road_user_list
+        result = self.state_estimator.estimate(road_user_list)
+        # road_user_list_data = RoadUserListData(
+        #     road_user_list=result,
+        #     capture_timestamp=data.capture_timestamp,
+        #     sensor_name=data.sensor_name
+        # )
+        data.road_user_list = result
+        self.logger.info(f"State Estimation completed in {time.time() - start:.2f} seconds for sensor: {data.sensor_name}")
+        return data

msight_vision/msight_core/tracking.py

@@ -0,0 +1,31 @@
+from msight_core.nodes import DataProcessingNode, NodeConfig
+from msight_core.data import RoadUserListData
+from .. import SortTracker
+from pathlib import Path
+import yaml
+import time
+
+
+class SortTrackerNode(DataProcessingNode):
+    default_configs = NodeConfig(
+        publish_topic_data_type=RoadUserListData
+    )
+    def __init__(self, configs, tracking_configs_path):
+        super().__init__(configs)
+        self.config_file_path = Path(tracking_configs_path)
+        with open(self.config_file_path, "r") as f:
+            self.tracking_configs = yaml.safe_load(f)
+        self.tracker = SortTracker()
+        
+
+    def process(self, data: RoadUserListData) -> RoadUserListData:
+        self.logger.info(f"Processing road user list data from sensor: {data.sensor_name}")
+        start = time.time()
+        road_user_list = data.road_user_list
+        # print(road_user_list)
+        tracking_result = self.tracker.track(road_user_list)
+        data.road_user_list = tracking_result
+        self.logger.info(f"Tracking completed in {time.time() - start:.2f} seconds for sensor: {data.sensor_name}")
+        return data
+
+    

msight_vision/msight_core/viewer.py

@@ -0,0 +1,55 @@
+from msight_core.nodes import SinkNode
+from msight_core.data import RoadUserListData, DetectionResultsData
+from msight_base.visualizer import Visualizer
+# from pathlib import Path
+# import yaml
+from msight_base import Frame
+import cv2
+
+
+class RoadUserListViewerNode(SinkNode):
+    def __init__(self, configs, basemap_path, with_traj=True, show_heading=False):
+        super().__init__(configs)
+        self.basemap_path = basemap_path
+        self.visualizer = Visualizer(basemap_path)
+        self.with_traj = with_traj
+        self.show_heading = show_heading
+        self.step=0
+
+
+    def on_message(self, data: RoadUserListData):
+        self.logger.info(f"Received road user list data from sensor: {data.sensor_name}")
+        road_user_list = data.road_user_list
+        if not self.with_traj:
+            # print(f"fused result: {len(fused_result)} objects")
+            vis_image = self.visualizer.render_roaduser_points(road_user_list)
+            cv2.imshow(self.name, vis_image)
+            cv2.waitKey(1)
+            return
+        result_frame = Frame(self.step)  
+        for obj in road_user_list:
+            # print(obj.traj_id)
+            result_frame.add_object(obj)
+        vis_img = self.visualizer.render(result_frame, with_traj=self.with_traj, show_heading=self.show_heading)
+        cv2.imshow(self.name, vis_img)
+        cv2.waitKey(1)
+        self.step += 1
+
+class DetectionResults2DViewerNode(SinkNode):
+    def __init__(self, configs):
+        super().__init__(configs)
+
+    def on_message(self, data: DetectionResultsData):
+        # print(data)
+        self.logger.info(f"Received detection results from sensor: {data.sensor_name}")
+        raw_image_data = data.raw_sensor_data
+        decoded_image = raw_image_data.to_ndarray()
+        detection_result = data.detection_result
+        for obj in detection_result.object_list:
+            # print(obj.box, obj.pixel_bottom_center)
+            x1, y1, x2, y2 = map(int, obj.box)
+            px, py = map(int, obj.pixel_bottom_center)
+            cv2.circle(decoded_image, (px, py), 5, (0, 0, 255), -1)
+            cv2.rectangle(decoded_image, (x1, y1), (x2, y2), (0, 255, 0), 2)
+        cv2.imshow(self.name, decoded_image)
+        cv2.waitKey(1)

msight_vision/msight_core/warper.py

@@ -0,0 +1,98 @@
+from msight_core.nodes import SinkNode, NodeConfig
+from msight_core.data import ImageData
+import cv2
+import numpy as np
+import time
+from msight_core.utils import get_redis_client
+from pathlib import Path
+import yaml
+import threading
+
+class WarperMatrixUpdaterNode(SinkNode):
+    def __init__(self, configs, warper_configs_path):
+        '''
+        The constructor for the WarperMatrixUpdaterNode class.
+        :param standard_image_path: Path to the standard image used for warping.
+        :param update_interval: Interval in steps between the updates.
+        :param time_threshold: Time threshold in seconds to trigger an update.
+        :param redis_prefix: Redis prefix for storing the warp matrix.
+        '''
+        super().__init__(configs)
+        self.warper_config_path = Path(warper_configs_path)
+        self.detector = None
+        with open(self.warper_config_path, "r") as f:
+            self.warper_config = yaml.safe_load(f)
+        self.update_interval = self.warper_config["warper_config"]["update_interval"]
+        self.time_threshold = self.warper_config["warper_config"]["time_threshold"]
+        self.redis_prefix = self.warper_config["warper_config"]["redis_prefix"]
+        standard_images_paths = self.warper_config["warper_config"]["std_imgs"]
+        self.standard_images = {sensor_name: cv2.imread(img_path) for (sensor_name, img_path) in standard_images_paths.items()}
+        self.steps = {sensor_name: 0 for sensor_name in standard_images_paths}
+        self.last_update_times = {sensor_name: time.time() for sensor_name in standard_images_paths}
+
+    def get_warp_matrix_between_two_image(self, im1, im2):
+
+        # Convert images to grayscale
+        im1_gray = cv2.cvtColor(im1, cv2.COLOR_BGR2GRAY)
+        im2_gray = cv2.cvtColor(im2, cv2.COLOR_BGR2GRAY)
+
+        # Find size of image1
+        # sz = im1.shape
+
+        # Define the motion model
+        warp_mode = cv2.MOTION_HOMOGRAPHY
+
+        # Define 2x3 or 3x3 matrices and initialize the matrix to identity
+        if warp_mode == cv2.MOTION_HOMOGRAPHY:
+            warp_matrix = np.eye(3, 3, dtype=np.float32)
+        else:
+            warp_matrix = np.eye(2, 3, dtype=np.float32)
+
+        # Specify the number of iterations.
+        number_of_iterations = 500
+
+        # Specify the threshold of the increment
+        # in the correlation coefficient between two iterations
+        termination_eps = 1e-10
+
+        # Define termination criteria
+        criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT,
+                    number_of_iterations,  termination_eps)
+
+        # Run the ECC algorithm. The results are stored in warp_matrix.
+        (cc, warp_matrix) = cv2.findTransformECC(
+            im1_gray, im2_gray, warp_matrix, warp_mode, criteria)
+        return warp_matrix
+    
+    def update_warp_matrix(self, image, sensor_name):
+        standard_img = self.standard_images[sensor_name]
+        warp_matrix = self.get_warp_matrix_between_two_image(
+            standard_img, image)
+        return warp_matrix
+    
+    def update_warp_matrix_in_redis(self, warp_matrix, sensor_name):
+        redis_client = get_redis_client()
+        # Convert the warp matrix to a string and store it in Redis
+        warp_matrix_str = np.array2string(warp_matrix, separator=',')
+        redis_client.set(self.redis_prefix + f":{sensor_name}", warp_matrix_str)
+        # self.logger.info("Warp matrix updated in Redis.")
+
+    def on_message(self, data: ImageData):
+        sensor_name = data.sensor_name
+        self.logger.debug(f"receive one image for {sensor_name}")
+        # self.logger.info(f"step {self.steps[sensor_name]}")
+        if self.steps[sensor_name] % self.update_interval == 0 or time.time() - self.last_update_times[sensor_name] > self.time_threshold:
+            def _update():
+                self.logger.info(f"Updating parameter for {sensor_name}")
+                start = time.time()
+                encoded_image = data.encoded_image
+                image = cv2.imdecode(encoded_image, cv2.IMREAD_COLOR)
+                warp_matrix = self.update_warp_matrix(image, sensor_name)
+                self.update_warp_matrix_in_redis(warp_matrix, sensor_name)
+                self.logger.info(f"Updated warp matrix for sensor: {sensor_name} in {time.time() - start:.2f} seconds")
+            x = threading.Thread(target=_update)
+            x.setDaemon(True)
+            x.start()
+        self.steps[sensor_name] += 1
+        self.steps[sensor_name] = self.steps[sensor_name] % self.update_interval
+        self.last_update_times[sensor_name] = time.time()

msight_vision/state_estimator.py

@@ -0,0 +1,121 @@
+from typing import List
+from msight_base import RoadUserPoint, TrajectoryManager
+from geopy.distance import geodesic
+import numpy as np
+
+
+class StateEstimatorBase:
+    def estimate(self, road_user_point_list: List[RoadUserPoint]) -> List[RoadUserPoint]:
+        """
+        Estimate the state of road users based on the provided list of RoadUserPoint instances.
+        :param road_user_point_list: List of RoadUserPoint instances to estimate the state from.
+        :return: Estimated state of road users.
+        """
+        raise NotImplementedError("StateEstimatorBase is an abstract class and cannot be instantiated directly.")
+    
+class FiniteDifferenceStateEstimator(StateEstimatorBase):
+    def __init__(self, frame_rate=5, frame_interval=1, dist_threshold=2):
+        """
+        Initialize the finite difference state estimator.
+        :param frame_rate: Frame rate of the video stream.
+        :param frame_interval: Interval the two object to calculate the difference, the two neighbor objects has interval 0.
+        :param heading_mask_area: Area to mask the heading estimation (optional).
+        """
+        self.frame_rate = frame_rate
+        self.frame_interval = frame_interval
+        self.trajectory_manager = TrajectoryManager(max_frames=100)
+        self.dist_threshold = dist_threshold
+
+    # def calc_heading(self, obj: RoadUserPoint, anchor: RoadUserPoint, scale):
+        
+    def get_anchor_point(self, obj):
+        traj = obj.traj
+        if len(traj.steps) <= 1:
+            None
+        if len(traj.steps) < self.frame_interval + 2:
+            anchor_index = 0
+        else:
+            anchor_index = len(traj.steps) - self.frame_interval - 1
+        anchor_step = traj.steps[anchor_index]
+        anchor = traj.step_to_object_map[anchor_step]
+        return anchor
+    
+    def calc_xy_difference(self, obj: RoadUserPoint, anchor: RoadUserPoint, scale="latlon"):
+        """
+        Calculate the difference between the object and the anchor point in the specified scale, sign is persistent.
+        :param
+        obj: RoadUserPoint instance representing the object.
+        :param anchor: RoadUserPoint instance representing the anchor point.
+        :param scale: Scale of the coordinates, either "latlon", "utm" or "meters". ("meters" and "utm" are equivalent)
+        :return: Difference between the object and the anchor point in meters.
+        """
+        if scale == "latlon":
+            lat1, lon1 = obj.x, obj.y
+            lat2, lon2 = anchor.x, anchor.y
+            # Calculate the distance in meters using geodesic
+            dx = geodesic((lat1, lon1), (lat2, lon1)).meters
+            if lat1 < lat2:
+                dx = -dx
+            dy = geodesic((lat1, lon1), (lat1, lon2)).meters
+            if lon1 < lon2:
+                dy = -dy    
+        elif scale in ["utm", "meters"]:
+            # just take difference
+            dx = obj.x - anchor.x
+            dy = obj.y - anchor.y
+        else:
+            raise ValueError("Invalid scale. Use 'latlon', 'utm' or 'meters'.")
+        return dx, dy
+    
+    def calc_heading(self, dx: float, dy: float, temporal_distance, fallback) -> float:
+        """
+        Calculate the heading of the object based on the difference in x and y coordinates.
+        :param
+        dx: Difference in x coordinates.
+        :param dy: Difference in y coordinates.
+        :param temporal_distance: Temporal distance between the two points.
+        :return: Heading of the object in degree.
+        """
+        # clock-wise, in degree
+        # north: 0, east: 90, north-east: 45
+        if temporal_distance > self.dist_threshold:
+            return fallback
+        heading = np.arctan2(dy, dx) * 180 / np.pi
+        return heading
+    
+    def calc_speed(self, obj: RoadUserPoint, anchor: RoadUserPoint, temporal_distance) -> float:
+        """
+        Calculate the speed of the object based on the difference in x and y coordinates and temporal distance.
+        :param
+        obj: RoadUserPoint instance representing the object.
+        :param anchor: RoadUserPoint instance representing the anchor point.
+        :param temporal_distance: Temporal distance between the two points.
+        :return: Speed of the object in meters per second.
+        """
+        
+        step_now = obj.frame_step
+        step_anchor = anchor.frame_step
+        time_difference = (step_now - step_anchor) * 1 / self.frame_rate  # in seconds
+        speed = (temporal_distance / time_difference) if time_difference > 0 else 0.0
+        return speed
+
+
+
+    def estimate(self, road_user_point_list: List[RoadUserPoint], scale="latlon") -> List[RoadUserPoint]:
+        """
+        Estimate the state of road users based on the provided list of RoadUserPoint instances.
+        :param road_user_point_list: List of RoadUserPoint instances to estimate the state from.
+        :param scale: Scale of the coordinates, either "latlon", "utm" or "meters". ("meters" and "utm" are equivalent)
+        :return: Estimated state of road users.
+        """
+        self.trajectory_manager.add_list_as_new_frame(road_user_point_list)
+        for obj in road_user_point_list:
+            anchor = self.get_anchor_point(obj)
+            if anchor is None:
+                continue
+            dx, dy = self.calc_xy_difference(obj, anchor, scale)
+            temporal_distance = (dx**2 + dy**2)**0.5
+            obj.heading = self.calc_heading(dx, dy, temporal_distance, anchor.heading) 
+            obj.speed = self.calc_speed(obj, anchor, temporal_distance)
+
+        return road_user_point_list