Untitled

import pandas as pd
import numpy as np

def parse_covariance_and_calculate_trace(covariance_str):
    # Remove parentheses and split by comma
    covariance_values = covariance_str.strip("()").split(", ")
    # Convert to numpy array of floats
    covariance_array = np.array(covariance_values, dtype=float)
    # Reshape to 6x6 matrix
    covariance_matrix = covariance_array.reshape(6, 6)
    # Calculate the trace
    trace = np.trace(covariance_matrix)
    return trace

# Load your CSV
csv_path = 'jackal_velocity_controller-odom.csv'  # Update this path to your CSV file
df = pd.read_csv(csv_path)

# Apply the function to calculate traces for pose.covariance and twist.covariance
df['pose_covariance_trace'] = df['pose.covariance'].apply(parse_covariance_and_calculate_trace)
df['twist_covariance_trace'] = df['twist.covariance'].apply(parse_covariance_and_calculate_trace)

covariance_columns = [col for col in df.columns if 'covariance' in col and 'trace' not in col]

# Remove the original covariance matrix columns
df= df.drop(columns=covariance_columns)

df = df.drop(columns=['header.seq', 'header.stamp.nsecs', 'header.stamp.secs'])
#  Ensure the DataFrame is sorted by Time
df = df.sort_values(by='Time')
# df=df.drop(columns=['header.seq', 'header.stamp.nsecs','header.stamp.secs' ])

# Filter rows, keeping only those 100ms apart
filtered_rows = [True]  # Keep the first row
last_time_kept = df['Time'].iloc[0]

for current_time in df['Time'][1:]:
    if current_time - last_time_kept >= 0.1:
        filtered_rows.append(True)
        last_time_kept = current_time
    else:
        filtered_rows.append(False)

df_filtered = df[filtered_rows]

# Reset index of the filtered DataFrame
df = df_filtered.reset_index(drop=True)

# Save the modified DataFrame back to a new CSV file
output_csv_path = 'filtered'+csv_path+'.csv'  # Update this path for the output CSV file
df.to_csv(output_csv_path, index=False)

print("Processing complete. The output CSV with trace values is saved.")