cdf.py

import numpy as np
from show import visualize_trajectory,visualize_trajectory_comparison
import matplotlib.pyplot as plt

def compute_cdf(data):
    sorted_data = np.sort(data)
    cdf = np.linspace(0, 1, len(sorted_data))
    return sorted_data, cdf

def compute_errors(baseline_x, baseline_y, filtered_x, filtered_y):

    errors = np.sqrt((baseline_x - filtered_x)**2 + (baseline_y - filtered_y)**2)
    return errors

def plot_cdf_of_errors(x_values, y_values, max_delta=0.6, max_skip=3, label="Error CDF", color=None):
    """Compute and plot the CDF of errors between baseline and filtered trajectory"""

    baseline_x_interp, baseline_y_interp = visualize_trajectory(x_values, y_values)

    errors = compute_errors(baseline_x_interp, baseline_y_interp, x_values, y_values)

    num_points = len(errors)

    mean_error = np.mean(errors)

    rmse = np.sqrt(np.mean(errors**2))

    errors_sorted, errors_cdf = compute_cdf(errors)

    plt.plot(errors_sorted, errors_cdf, 
             label=f"{label} (ME={mean_error:.3f}m, RMSE={rmse:.3f}m)",
             color=color)

    print(f"{label}: total points = {num_points}/1460, mean error = {mean_error:.3f} m, RMSE = {rmse:.3f} m")

    return mean_error, rmse, num_points