graph.py

import os
import numpy as np
import plotly.graph_objects as go

# Directory containing pose matrices
directory = os.path.expanduser("/home/ma1614/zero123/zero123/dataset/ob_in_cam")

# List to store all pose matrices
pose_matrices = []

# Iterate over each file in the directory
for filename in sorted(os.listdir(directory)):
    if filename.endswith(".txt"):
        file_path = os.path.join(directory, filename)
        
        # Read the content of the file
        with open(file_path, "r") as file:
            lines = file.readlines()
        
        # Convert the pose data into a NumPy array
        pose_matrix = []
        for line in lines:
            pose_matrix.append([float(x) for x in line.strip().split()])
        
        # Append to the list as a NumPy array
        pose_matrices.append(np.array(pose_matrix))

# Function to calculate azimuth, elevation, and radius
def calculate_pose_properties(pose_matrix):
    # Extract the translation vector (position)
    translation = pose_matrix[:3, 3]
    x, y, z = translation
    
    # Radius (distance from origin)
    r = np.sqrt(x**2 + y**2 + z**2)
    
    # Azimuth (angle in the XY plane)
    azimuth = np.arctan2(y, x)  # in radians
    
    # Elevation (angle above the XY plane)
    elevation = np.arctan2(z, np.sqrt(x**2 + y**2))  # in radians
    
    # Convert azimuth and elevation to degrees
    azimuth_deg = np.degrees(azimuth)
    elevation_deg = np.degrees(elevation)
    
    return x, y, z, r, azimuth_deg, elevation_deg

# Process all pose matrices
pose_properties = []
for idx, pose_matrix in enumerate(pose_matrices):
    x, y, z, r, azimuth, elevation = calculate_pose_properties(pose_matrix)
    pose_properties.append((x, y, z, r, azimuth, elevation))

# Extract values for plotting
x_vals = [x for x, _, _, _, _, _ in pose_properties]
y_vals = [y for _, y, _, _, _, _ in pose_properties]
z_vals = [z for _, _, z, _, _, _ in pose_properties]

# Last point details
last_x, last_y, last_z, _, last_azimuth, last_elevation = pose_properties[-1]

# Create the plot
fig = go.Figure()



# Add all points as a scatter plot
fig.add_trace(go.Scatter3d(
    x=x_vals,
    y=y_vals,
    z=z_vals,
    mode='markers+text',
    marker=dict(size=5, color='blue'),
    text=[f"Az: {azimuth:.1f}°<br>El: {elevation:.1f}°<br>R: {r:.2f}" 
          for _, _, _, r, azimuth, elevation in pose_properties],
    textposition='top center',
    name='Pose Points'
))

# Highlight the last point
fig.add_trace(go.Scatter3d(
    x=[last_x],
    y=[last_y],
    z=[last_z],
    mode='markers+text',
    marker=dict(size=10, color='red'),
    # text=f"Az: {last_azimuth:.1f}°<br>El: {last_elevation:.1f}°<br>R: {r:.2f}",
    textposition='top center',
    name='Last Pose'
))

# Set axis labels and title
fig.update_layout(
    title="Interactive Pose Visualization",
    scene=dict(
        xaxis_title='X Axis',
        yaxis_title='Y Axis',
        zaxis_title='Z Axis'
    ),
    legend=dict(
        x=0,
        y=1
    )
)

# Show the plot
fig.show()