I have the following script. currently the movement is calculated with a fixed FPS value, but the translations and rotation is actually calculated with interpolatet timestamps and its poses. Does it make sense to adjust the script and use the read timestamps instead of FPS and how to do it: import os

import numpy as np
import pandas as pd

from src.raw_dataset_configs import CoordinateDataKeys, RawDataFolder
from src.selector_configs import SelectedData

FPS = 10
TOOL_LENGTH_ESTIMATION = 100

def pre_filter_unrecorded_nir(
    image_list: list[str], raw_coordinates_dataframe: pd.DataFrame, margin: float = 0.5
):
    start_timestamp = int(
        (raw_coordinates_dataframe["timestamp"].min() + margin) * 1000000
    )
    end_timestamp = int(
        (raw_coordinates_dataframe["timestamp"].max() - margin) * 1000000
    )

    return [
        image_name
        for image_name in image_list
        if start_timestamp < int(os.path.splitext(image_name)[0]) < end_timestamp
    ]


def get_interpolated_data(image_name: str, raw_coordinates_dataframe: pd.DataFrame, offset: float = 0.0):



    timestamp = float(os.path.splitext(image_name)[0]) / 1000000 + offset
    timestamps = raw_coordinates_dataframe["timestamp"]

    # Find the two closest timestamps
    closest_indices = timestamps.sub(timestamp).abs().nsmallest(2).index
    closest_timestamps = timestamps[closest_indices]
    # Interpolate data for these timestamps
    if len(closest_timestamps) == 2:
        lower_idx, upper_idx = closest_indices
        lower_time, upper_time = closest_timestamps
        ratio = (timestamp - lower_time) / (upper_time - lower_time)
        interpolated_data = raw_coordinates_dataframe.iloc[lower_idx] + (raw_coordinates_dataframe.iloc[upper_idx] - raw_coordinates_dataframe.iloc[lower_idx]) * ratio
    else:
        interpolated_data = raw_coordinates_dataframe.iloc[closest_indices[0]]

    return interpolated_data

def match_images_dataframes(image_list, raw_coordinates_dataframe: pd.DataFrame, offset: float = 0.0) -> pd.DataFrame:
    interpolated_data_list = [get_interpolated_data(image_name, raw_coordinates_dataframe, offset) for image_name in image_list]

    matched_data = pd.DataFrame(interpolated_data_list)
    return matched_data.assign(image_name=image_list).reset_index(drop=True)


# def get_closest_timestamp_data(
#     image_name: str, raw_coordinates_dataframe: pd.DataFrame, offset: float = 0.0
# ) -> int:
#     timestamp = float(os.path.splitext(image_name)[0]) / 1000000 + offset
#     return int(raw_coordinates_dataframe["timestamp"].sub(timestamp).abs().idxmin())
#
#
# def match_images_dataframes(
#     image_list: list[str], raw_coordinates_dataframe: pd.DataFrame, offset: float = 0.0
# ) -> pd.DataFrame:
#     row_list = [
#         get_closest_timestamp_data(image_name, raw_coordinates_dataframe, offset)
#         for image_name in image_list
#     ]
#
#     matched_data = raw_coordinates_dataframe.iloc[row_list]
#
#     return matched_data.assign(image_name=image_list).reset_index(drop=True)


def filter_quality_recorded_trackers(
    coordinate_dataframe: pd.DataFrame,
    quality_threshold: float = 0.5,
    verbose: bool = True,
) -> pd.DataFrame:
    coordinate_dataframe = coordinate_dataframe[
        coordinate_dataframe["quality"] > quality_threshold
    ]

    if verbose:
        print(
            f"Filtering trackers quality... Keeping {coordinate_dataframe.shape[0]} frames"
        )

    return coordinate_dataframe


def filter_blurry(
    coordinate_dataframe: pd.DataFrame,
    movement_threshold_mm: float = 7,
    verbose: bool = True,
) -> pd.DataFrame:
    rotation_dataframe = coordinate_dataframe[
        CoordinateDataKeys.TOOL_ROTATION_SPEED
    ].to_numpy()

    translation_dataframe = coordinate_dataframe[
        CoordinateDataKeys.TOOL_TRANSLATION_SPEED
    ].to_numpy()

    estimated_movement = (
        np.linalg.norm(translation_dataframe, axis=1) * 1000
        + np.linalg.norm(rotation_dataframe, axis=1) * TOOL_LENGTH_ESTIMATION
    ) / FPS

    coordinate_dataframe["estimated_movement"] = estimated_movement

    coordinate_dataframe = coordinate_dataframe[
        coordinate_dataframe["estimated_movement"] < movement_threshold_mm
    ]

    if verbose:
        print(
            f"Filtering blurry images... Keeping {coordinate_dataframe.shape[0]} frames"
        )

    return coordinate_dataframe


# def _synchronise_fiducials_timestamps(
#     selected_data: SelectedData, triangulated_points: pd.DataFrame
# ) -> pd.DataFrame:
#     timestamps = selected_data.coordinate_dataframe["timestamp"].to_numpy()
#
#     return triangulated_points[triangulated_points["time_stamp"].isin(timestamps)]
#

import pandas as pd


def interpolate_data_at_timestamp(timestamp: float, dataframe: pd.DataFrame, fiducial_indexes: list) -> pd.DataFrame:
    interpolated_frames = []

    for fiducial_index in fiducial_indexes:
        # Filter the dataframe for the specific fiducial_index
        df_filtered = dataframe[dataframe['fiducial_index'] == fiducial_index]

        # Find the two closest distinct timestamps for the specific fiducial_index
        closest_indices = df_filtered['time_stamp'].sub(timestamp).abs().nsmallest(2).index
        closest_timestamps = df_filtered.loc[closest_indices, 'time_stamp']

        if len(closest_timestamps) == 2:
            lower_idx, upper_idx = closest_indices
            lower_time, upper_time = closest_timestamps

            # Check if lower_time and upper_time are equal to avoid division by zero
            if lower_time == upper_time:
                interpolated_data = df_filtered.loc[lower_idx]
            else:
                ratio = (timestamp - lower_time) / (upper_time - lower_time)
                interpolated_data = df_filtered.loc[lower_idx] + (
                            df_filtered.loc[upper_idx] - df_filtered.loc[lower_idx]) * ratio
        else:
            interpolated_data = df_filtered.loc[closest_indices[0]]

        # Add the DataFrame to the list
        interpolated_frames.append(interpolated_data.to_frame().T)

    # Combine all interpolated frames
    combined_interpolated_data = pd.concat(interpolated_frames, axis=0)
    return combined_interpolated_data


def _synchronise_fiducials_timestamps(selected_data, triangulated_points: pd.DataFrame) -> pd.DataFrame:
    timestamps = selected_data.coordinate_dataframe["timestamp"].to_numpy()
    fiducial_indexes = set(triangulated_points["fiducial_index"])

    interpolated_data_frames = []

    for timestamp in timestamps:
        interpolated_data = interpolate_data_at_timestamp(timestamp, triangulated_points, fiducial_indexes)
        interpolated_data_frames.append(interpolated_data)

    # Combine all interpolated data frames
    all_interpolated_data = pd.concat(interpolated_data_frames, axis=0).reset_index(drop=True)

    return all_interpolated_data


def _get_fiducial_dataframe(
    synchronised_triangulated_points: pd.DataFrame, fiducial_index: int
) -> pd.DataFrame:
    fiducial_dataframe = synchronised_triangulated_points[
        synchronised_triangulated_points["fiducial_index"] == fiducial_index
    ]

    fiducial_dataframe = fiducial_dataframe[["time_stamp", "x", "y", "z"]]
    fiducial_dataframe = fiducial_dataframe.drop_duplicates(
        subset=["time_stamp"], keep="first"
    )

    return fiducial_dataframe.rename(
        columns={
            "x": CoordinateDataKeys.FIDUCIALS_3D.get(fiducial_index)[0],
            "y": CoordinateDataKeys.FIDUCIALS_3D.get(fiducial_index)[1],
            "z": CoordinateDataKeys.FIDUCIALS_3D.get(fiducial_index)[2],
        }
    )


def add_fiducial_coordinates(
    selected_data: SelectedData, verbose: bool = False
) -> None:
    if verbose:
        print("Joining fiducials...")

    triangulated_points_file = os.path.join(
        selected_data.data_folder, RawDataFolder.TRIANGULATED_POINTS
    )
    triangulated_points = pd.read_csv(triangulated_points_file, delimiter=r"\s+")

    synchronised_triangulated_points = _synchronise_fiducials_timestamps(
        selected_data, triangulated_points
    )



    for fiducial_index in range(4):
        fiducial_dataframe = _get_fiducial_dataframe(
            synchronised_triangulated_points, fiducial_index
        )

        selected_data.coordinate_dataframe = pd.merge(
            selected_data.coordinate_dataframe,
            fiducial_dataframe,
            left_on="timestamp",
            right_on="time_stamp",
            how="inner",
            validate="one_to_one",
        ).drop(columns=["time_stamp"])