Rotation.py

from scipy.spatial.transform import Rotation as R
from scipy.optimize import minimize
import pandas as pd
import numpy as np
import trimesh
import vispy
vispy.use("glfw")
from vispy import scene
from vispy.app import run
from scipy.optimize import least_squares
from collections import defaultdict

import sys
import pandas as pd
import numpy as np
import matplotlib.pyplot as plt
import csv

import pandas as pd
import numpy as np

vM3 = "vector_M3_CS"
vp  = "vector_probeCS"
vf  = "vector_fit"
vectorlst = [vM3,vp,vf]
masklst = [1,4,4,7,8,10]
#example:
#N X     Y       Z       X       Y      Z       A       X       Y       sx    sy    theta date
#2,0.0 , 331.0 , 261.7 , 868.0 , 52.0 , 169.0 , -46.0 , 20.53 , 19.84 , 4.0 , 4.0 , 1.1 , 20250212164236
Fiducial_solutions = []
filename = sys.argv[1]
#with open(MESApath + "/" + dataset[dataindex] + "/" + filelist[0], newline='') as csvfile:
with open(filename, newline='') as csvfile:
    fieldreader = csv.reader(csvfile, delimiter=',', quotechar="|", quoting=csv.QUOTE_NONNUMERIC)
    for row in fieldreader:
        Fiducial_solutions.append(row)

Fidunamelst = ["F"+str(int(Fiducial_solutions[i][0])) for i in range(0,3)]
Fidunamelst_FLP = ["F"+str(int(Fiducial_solutions[i][0])) for i in range(3,6)]

Fidudict = defaultdict(dict)
for i in range(len(Fidunamelst)):
    for j in range(len(vectorlst)):
        Fidudict[Fidunamelst[i]][vectorlst[j]] = Fiducial_solutions[i][masklst[j*2]:masklst[j*2+1]]
            
Fidudict_FLP = defaultdict(dict)
for i in range(len(Fidunamelst_FLP)):
    for j in range(len(vectorlst)):
        Fidudict_FLP[Fidunamelst_FLP[i]][vectorlst[j]] = Fiducial_solutions[i+3][masklst[j*2]:masklst[j*2+1]]

def fourthpoint(Fidlst,Fiddict):
    print(Fiddict)
    if "F3" in Fidlst and "F4" in Fidlst:
        F3f =  np.append(np.array(Fiddict["F3"][vf]),0)
        F4f =  np.append(np.array(Fiddict["F4"][vf]),0)
        F3 = np.array(Fiddict["F3"][vp]) + F3f
        F4 = np.array(Fiddict["F4"][vp]) + F4f
        dist = F4 - F3 #edge
        normal = np.array([-dist[1],dist[0],0.])
        Center = F3 + .5*(dist-240./330.*normal)
        if "F1" in Fidlst:
            F1f =  np.append(np.array(Fiddict["F1"][vf]),0)
            F1 = np.array(Fiddict["F1"][vp]) + F1f
            F2 = F1 + dist*550./330.
        else:
            F2f =  np.append(np.array(Fiddict["F2"][vf]),0)
            F2 = np.array(Fiddict["F2"][vp]) + F2f
            F1 = F2 - dist*550./330.
        
    else:
        F1f =  np.append(np.array(Fiddict["F1"][vf]),0)
        F2f =  np.append(np.array(Fiddict["F2"][vf]),0)
        F1 = np.array(Fiddict["F1"][vp]) + F1f
        F2 = np.array(Fiddict["F2"][vp]) + F2f
        dist = F1 - F2 #edge
        normal = np.array([-dist[1],dist[0],0.])
        Center = F1 + .5*(-dist-240./550.*normal)
        if "F3" in Fidlst:
            F3f =  np.append(np.array(Fiddict["F3"][vf]),0)
            F3 = np.array(Fiddict["F3"][vp]) + F3f
            F4 = F3 + dist*330./550.
        else:
            F4f =  np.append(np.array(Fiddict["F4"][vf]),0)
            F4 = np.array(Fiddict["F4"][vp]) + F4f
            F3 = F4 - dist*330./550.
    tiltangleagainstedge = 90-180./np.pi*np.arccos(np.dot(np.transpose([1,0,0]),normal)/(np.linalg.norm(normal))) #Ideal 22.5-5=17.5 degrees
    return [F1,F2,F3,F4,Center,tiltangleagainstedge]

Fiducoordinates = fourthpoint(Fidunamelst,Fidudict)
Fiducoordinates_FLP = fourthpoint(Fidunamelst_FLP,Fidudict_FLP)


# Load the CSV data, assuming the file is in the same directory or provide full path
#df = pd.read_csv("MARC1_16_Fiducial_solutions.csv", sep=";", encoding='unicode_escape')
#df1 = pd.read_csv("M3_MARC1_1_20241009.csv", sep=";", encoding='unicode_escape')
#df2 = pd.read_csv("M3_MARC1_1_20241017_Flp.csv", sep=";", encoding='unicode_escape')

F1p = Fiducoordinates[0]#[363.8814295180317, -93.03808816209627, -169]
F2p = Fiducoordinates[1]#[888.9748350639564, 70.77346183385109, -169]
F3p = Fiducoordinates[2]#[397.49594195551896, 168.96975051421472, -169]
F4p = Fiducoordinates[3]#[712.5519852830738, 267.25668051178314, -169]
Center = Fiducoordinates[4]#[712.5519852830738, 267.25668051178314, -169]
tiltangleagainstedge = Fiducoordinates[5]#[712.5519852830738, 267.25668051178314, -169]

F1p_Flp = Fiducoordinates_FLP[0]#[880.9063,  6.0495, -169]
F2p_Flp = Fiducoordinates_FLP[1]#[356.4885, 171.9683, -169]
F3p_Flp = Fiducoordinates_FLP[2]#[388.9710, -90.0549, -169]
F4p_Flp = Fiducoordinates_FLP[3]#[703.6218,-189.6063, -169]
Center_Flp = Fiducoordinates_FLP[4]#[703.6218,-189.6063, -169]
#exit()

fig, ax1 = plt.subplots()
ax1.plot([F1p[0],F2p[0],F3p[0],F4p[0],Center[0]],[F1p[1],F2p[1],F3p[1],F4p[1],Center[1]],"o")
ax1.plot([F1p_Flp[0],F2p_Flp[0],F3p_Flp[0],F4p_Flp[0],Center_Flp[0]],[F1p_Flp[1],F2p_Flp[1],F3p_Flp[1],F4p_Flp[1],Center_Flp[1]],"o")
ax1.set_aspect("equal")
plt.show()

# Define the objective function to minimize alignment error
def alignment_error(quat,P1,P2):
    rotation = R.from_quat(quat)  # Create a rotation from the quaternion
    transformed_points = rotation.apply(P1)  # Apply rotation to A
    return np.linalg.norm(transformed_points - P2)  # Compute alignment error

current_iteration = [0]  # Store the current iteration count

def callback(x):
    current_iteration[0] += 1
    print(f"Iteration {current_iteration[0]}: Current Parameters: {x}")
    optimal_rotation = R.from_quat(x)  # Convert optimized quaternion to Rotation object
    rotation_angle = optimal_rotation.as_euler('zxy', degrees=True)[0]  # Extract angle around z-axis
    print("Rotation Angle around z-axis (degrees):", rotation_angle)
    #if current_iteration[0] >= 2:  # Stop after 2 iterations
    #    return True  # Signal to stop optimization

def optiquat(P1,P2):
    # Initial guess for quaternion (identity rotation)
    initial_quat = R.from_euler('y', 140, degrees=True).as_quat()

    # Run optimization
    result = minimize(alignment_error, initial_quat, args=(P1, P2), callback=callback)
    optimal_quat = result.x  # Optimized quaternion

    # Display results
    print("Optimal Quaternion:", optimal_quat)
    # After optimization
    optimal_rotation = R.from_quat(optimal_quat)  # Convert optimized quaternion to Rotation object

    # Extract the rotation angle in degrees
    rotation_angle = optimal_rotation.as_euler('zxy', degrees=True)[0]  # Extract angle around z-axis

    print("Optimal Quaternion:", optimal_quat)
    print("Rotation Angle around z-axis (degrees):", rotation_angle)

    axis = optimal_rotation.as_rotvec() / np.linalg.norm(optimal_rotation.as_rotvec())  # normalized axis
    angle = optimal_rotation.magnitude()  # angle in radians
    # Convert angle to degrees if needed
    angle_degrees = np.degrees(angle)

    print("Rotation axis:", axis)
    #print("Rotation angle (radians):", angle)
    print("Rotation angle (degrees):", angle_degrees)
    print("Angle against edge (degrees):",tiltangleagainstedge)
    print("Center: ", Center)
    print("Center_Flp: ", Center_Flp)
    print("diff Centers: ", Center-Center_Flp)
    print(angle_degrees,",",tiltangleagainstedge,",",Center[0],",",Center[1],",",Center[2],",",Center_Flp[0],",",Center_Flp[1],",",Center_Flp[2])
    return axis , angle

#axisangleM3 = optiquat(A,B)
#optimal_quat = result.x
#optimal_rotation = R.from_quat(optimal_quat)
#target_rotation = R.from_rotvec(axisangleM3[0] * -1*axisangleM3[1])
#rotated_A_final= target_rotation.apply(points2Allshift)

#pointsMAG1 = points1Allshift[0:4] #magnet
#pointsMAG2 = rotated_A_final[4:8] #magnet

pointsMAG1 = np.array([F1p    ,F2p    ,F3p    ,F4p    ])
pointsMAG2 = np.array([F1p_Flp,F2p_Flp,F3p_Flp,F4p_Flp])

pointsMAG1center = Center#np.mean(pointsMAG1,axis=0)
pointsMAG2center = Center_Flp#np.mean(pointsMAG2,axis=0)

pointsMag1shift = pointsMAG1 - pointsMAG1center#centervector
pointsMag2shift = pointsMAG2 - pointsMAG2center#centervector

axisangleM3 = optiquat(pointsMag1shift,pointsMag2shift)
#optimal_quat = result.x
#optimal_rotation = R.from_quat(optimal_quat)
target_rotation = R.from_rotvec(axisangleM3[0] * -1.*axisangleM3[1])
rotated_A_final= target_rotation.apply(pointsMag2shift)

#rotated_A_final = rotated_A_final + pointsMAG2center

#rotated_A_final = optimal_rotation.apply(points1M3shift)

#print("final difference", points2M3shift-rotated_A_final)

##################################


scene_trimesh = trimesh.Scene()
# Create a Vispy canvas
canvas = scene.SceneCanvas(keys='interactive', show=True)
view = canvas.central_widget.add_view()

# Set view parameters
view.camera = 'turntable'  # 'arcball', 'fly'
view.camera.fov = 45

# Set the initial camera position and center
initial_center  =pointsMAG2[2]#np.array([0,0,0])
view.camera.center = initial_center
station1 = initial_center
station2 = initial_center
view.camera.distance = 200  # Distance from the center point

camera_positions = {
    '1': {'center': (station1[0], station1[1], station1[2]), 'distance': 500},
    '2': {'center': (station2[0], station2[1], station2[2]), 'distance': 500},
    '3': {'center': (initial_center[0], initial_center[1], initial_center[2]), 'distance': 200},
}

camera_tools = {
    'p': {'center': (station1[0], station1[1], station1[2]), 'distance': 500},
}

def on_key_press(event):
    key = event.key.name
    if key in camera_positions:
        # Get the camera position corresponding to the pressed key
        pos = camera_positions[key]
        # Update the camera center and distance
        view.camera.center = pos['center']
        view.camera.distance = pos['distance']
    if key == "P":
        if view.camera.fov == 45:
            view.camera.fov = 0   # Orthographic view
        else:
            view.camera.fov = 45  # Perspective view

# Connect the key press event to the handler
canvas.events.key_press.connect(on_key_press)

# Create marker visuals for hit points
"""
clusteroverlap = points1M3shift#np.array([[0,0,100],[0,100,0],[100,0,0]])

for point in clusteroverlap:
    box = trimesh.creation.box(extents=[50, 50, 50])
    scene_trimesh.add_geometry(box)
    box.apply_translation(point)
    vertices, faces = box.vertices, box.faces
    mesh = scene.visuals.Mesh(vertices=vertices, faces=faces, color=(0,1,0), shading='smooth')
    view.add(mesh)

clusteroverlap = points2Allshift#np.array([[0,0,100],[0,100,0],[100,0,0]])

for point in clusteroverlap:
    box = trimesh.creation.box(extents=[50, 50, 50])
    scene_trimesh.add_geometry(box)
    box.apply_translation(point)
    vertices, faces = box.vertices, box.faces
    mesh = scene.visuals.Mesh(vertices=vertices, faces=faces, color=(1,0,0), shading='smooth')
    view.add(mesh)
"""
clusteroverlap = rotated_A_final#np.array([[0,0,100],[0,100,0],[100,0,0]])

for point in clusteroverlap:
    box = trimesh.creation.box(extents=[50, 50, 50])
    scene_trimesh.add_geometry(box)
    box.apply_translation(point)
    vertices, faces = box.vertices, box.faces
    mesh = scene.visuals.Mesh(vertices=vertices, faces=faces, color=(0,1,1), shading='smooth')
    view.add(mesh)

clusteroverlap = pointsMag1shift#np.array([[0,0,100],[0,100,0],[100,0,0]])
i = 0.
for point in clusteroverlap:
    i = i + 1
    c=.1*i
    box = trimesh.creation.box(extents=[50, 50, 50])
    scene_trimesh.add_geometry(box)
    box.apply_translation(point)
    vertices, faces = box.vertices, box.faces
    mesh = scene.visuals.Mesh(vertices=vertices, faces=faces, color=(1-c,0,c), shading='smooth')
    view.add(mesh)

clusteroverlap = pointsMag2shift#np.array([[0,0,100],[0,100,0],[100,0,0]])
i = 0.
for point in clusteroverlap:
    i = i + 1
    c=.2*i
    box = trimesh.creation.box(extents=[50, 50, 50])
    scene_trimesh.add_geometry(box)
    box.apply_translation(point)
    vertices, faces = box.vertices, box.faces
    mesh = scene.visuals.Mesh(vertices=vertices, faces=faces, color=(0,1,c), shading='smooth')
    view.add(mesh)


if __name__ == '__main__':
    run()