plot.py

import plotly.graph_objs as go
import numpy as np

def load_colmap_images_txt(file_path):
    xs, ys, zs = [], [], []
    with open(file_path, 'r') as f:
        for line in f:
            parts = line.strip().split()
            if len(parts) == 9:
                # Format: idx qw qx qy qz x y z 1
                x, y, z = map(float, parts[4:7])
                xs.append(x)
                ys.append(y)
                zs.append(z)
    return np.array(xs), np.array(ys), np.array(zs)

def plot_trajectory_html(xs, ys, zs, output_html="trajectory_plot.html"):
    fig = go.Figure(data=[go.Scatter3d(
        x=xs, y=ys, z=zs,
        mode='lines+markers',
        marker=dict(size=4, color='blue'),
        line=dict(width=2, color='black')
    )])
    fig.update_layout(
        scene=dict(
            xaxis_title='X',
            yaxis_title='Y',
            zaxis_title='Z'
        ),
        title='3D Trajectory from COLMAP images.txt',
        width=800,
        height=600
    )
    fig.write_html(output_html)
    print(f"Saved 3D trajectory to: {output_html}")

# === Usage ===
images_txt_path = "result/images.txt"  # Replace with actual path
xs, ys, zs = load_colmap_images_txt(images_txt_path)
plot_trajectory_html(xs, ys, zs, output_html="colmap_trajectory.html")