Commit 76c0aea9 authored by Jadracka's avatar Jadracka
Browse files

Added Helmert transformation function

parent a1970c4a
......@@ -9,7 +9,7 @@ Created on Fri May 21 11:21:32 2021
import numpy as np
#import sys
#import string
import math as m
#import math as m
import config as cg
import functions as fc
import MainCode as MC
......@@ -34,3 +34,25 @@ if MC.Two_epochs:
measured_distances_in_lines_E1 = MC.measured_distances_in_lines_E1
sorted_measured_points_in_lines_E1 = MC.sorted_measured_points_in_lines_E1
#A_matrix = np.array([[], []], np.double)
#m = 5
#n = 6
#A_matrix = np.zeroes([m,n])
#print(A_matrix.shape)
count_IFM_measurements = sum([len(v)+1 for k, v in\
measured_distances_in_lines.items()])\
- len(measured_distances_in_lines)
count_Pico_measurements = 4
count_Pol_measurements = (sum([len(v) for k, v in Pol_measurements.items()]))*3
count_all_observations = count_IFM_measurements + count_Pico_measurements \
+ count_Pol_measurements
unknowns = []
for instrument in Pol_measurements:
keys = list(Pol_measurements[instrument].keys())
unknowns = list(set(unknowns + keys))
unknowns.sort()
print(unknowns)
print(count_all_observations)
\ No newline at end of file
# -*- coding: utf-8 -*-
"""
Created on Tue Jun 1 16:23:20 2021
@author: jbarker
"""
""" SET BACKEND A magical trick from
https://stackoverflow.com/questions/47356726/
fix-matplotlib-not-installed-as-framework-error-w-out-changing-matplotlib-con"""
#import matplotlib as mpl
#mpl.use('TkAgg')
from collections import namedtuple
import numpy as np
#import matplotlib.pyplot as plt
#import scipy as sp
import math
#import sys
import functions as fc
def X_Rotation(alpha):
Rxc = math.cos(alpha)
Rxs = math.sin(alpha)
Rx = np.array([[1, 0, 0], [0, Rxc, Rxs], [0, -Rxs, Rxc]])
return Rx
def Y_Rotation(beta):
Ryc = math.cos(beta)
Rys = math.sin(beta)
Ry = np.array([[Ryc, 0, -Rys], [0, 1, 0], [Rys, 0, Ryc]])
return Ry
def Z_Rotation(gamma):
Rzc = math.cos(gamma)
Rzs = math.sin(gamma)
Rz = np.array([[Rzc, Rzs, 0], [-Rzs, Rzc, 0], [0, 0, 1]])
return Rz
def dX_Rotation(alpha):
Rxc = math.cos(alpha)
Rxs = math.sin(alpha)
Rdx = np.array([[0, 0, 0], [0, -Rxs, Rxc], [0, -Rxc, -Rxs]])
return Rdx
def dY_Rotation(beta):
Ryc = math.cos(beta)
Rys = math.sin(beta)
Rdy = np.array([[-Rys, 0, -Ryc], [0, 0, 0], [Ryc, 0, -Rys]])
return Rdy
def dZ_Rotation(gamma):
Rzc = math.cos(gamma)
Rzs = math.sin(gamma)
Rdz = np.array([[-Rzs, Rzc, 0], [-Rzc, -Rzs, 0], [0, 0, 0]])
return Rdz
def Rotation_matrix(alpha, beta, gamma):
R = np.array(X_Rotation(alpha)*Y_Rotation(beta)*Z_Rotation(gamma))
return R
def Helmert_transformation(T, R, x):
"""3D Helmert transformation with known transformation Key
(Rotation matrix parameters and Translation vector)"""
X = T + R.dot(x)
return X
def Helmert_aproximate_parameters(From,To):
identicals = list(set(To.keys()) & set(From.keys()))
if len(identicals) > 3:
# MAKE BETTER CHOICE ON POINTS WHEN YOU HAVE TIME, IF YOU WANT... PLEASE
point1_To = np.array(To[identicals[0]])
point2_To = np.array(To[identicals[-1]])
point3_To = np.array(To[identicals[len(identicals)//2]])
point1_To_original = point1_To.copy().transpose()
#// truncating division = rounds result to lower int
point1_From = np.array(From[identicals[0]])
point2_From = np.array(From[identicals[-1]])
point3_From = np.array(From[identicals[len(identicals)//2]])
point1_From_original = point1_From.copy().transpose()
# Translation of the points to have origin in point 1:
point2_To = point2_To - point1_To
point3_To = point3_To - point1_To
point1_To = (0,0,0)
point2_From = point2_From - point1_From
point3_From = point3_From - point1_From
point1_From = (0,0,0)
# Calculating the aproximate parameters
# (based on Angle between planes paper)
# First rotation angle calculations
psi_To = math.atan2(point2_To[1],point2_To[0])
psi_From = math.atan2(point2_From[1],point2_From[0])
# Applying the calculated angle to point 2 and 3
FirstZrotation2_To = (Z_Rotation(psi_To)).dot(point2_To).transpose()
FirstZrotation3_To = (Z_Rotation(psi_To)).dot(point3_To.transpose())
FirstZrotation2_From = (Z_Rotation(psi_From)).dot(point2_To.transpose())
FirstZrotation3_From = (Z_Rotation(psi_From)).dot(point3_To.transpose())
# Calculating second rotation angle from the first rotated coordinates
fi_To = math.atan2(FirstZrotation2_To[1],FirstZrotation2_To[2])
fi_From = math.atan2(FirstZrotation2_From[1],FirstZrotation2_From[2])
# Applying the calculated angle to point 3
SecondYrotation3_To = (Y_Rotation(fi_To)).dot(
FirstZrotation3_To.transpose())
SecondYrotation3_From = (Y_Rotation(fi_From)).dot(
FirstZrotation3_From.transpose())
# Calculating third rotation angle
theta_To = math.atan2(SecondYrotation3_To[1],SecondYrotation3_To[0])
theta_From = math.atan2(SecondYrotation3_From[1],
SecondYrotation3_From[0])
# Using all three angles, I calculate the full rotation matrix for From To
R_To = Z_Rotation(theta_To).dot(Y_Rotation(fi_To)).dot(Z_Rotation(psi_To))
R_From = Z_Rotation(theta_From).dot(Y_Rotation(fi_From)).dot(
Z_Rotation(psi_From))
# The translation vector is calculated by rotating the original
# point1_From to the "To" coordinate frame. By substracting the rotated
# point1_From from point1_To we get the translation vector.
R = R_To.transpose().dot(R_From)
Translation = point1_To_original - R.dot(point1_From_original)
# Euler rotation angles
alpha = math.atan2(R[1,2],R[2,2])
beta = - math.atan(R[0,0])
gamma = math.atan2(R[0,1],R[0,0])
R_angles = (alpha, beta, gamma)
else:
print('Not enough identical points for transformation calculations.')
return R, Translation, R_angles# RX + T
To = {'Point1': (3970673.003, 1018563.740, 4870369.178),
'Point2': (3970667.574, 1018565.195, 4870373.010),
'Point3': (3970659.461, 1018571.269, 4870377.881),
'Point4': (3970654.604, 1018577.517, 4870380.020),
'Point5': (3970650.090, 1018580.577, 4870382.774),
'Point6': (3970646.096, 1018581.683, 4870385.620)
}
From = {'Point1': (744970.551, 1040944.109, 224.592),
'Point2': (744966.969, 1040938.331, 224.390),
'Point3': (744958.051, 1040931.492, 224.057),
'Point4': (744950.344, 1040928.731, 223.676),
'Point5': (744945.677, 1040924.795, 223.472),
'Point6': (744943.006, 1040920.538, 223.352)
}
R0, T0, R_angles0 = Helmert_aproximate_parameters(From,To)
print(R0, T0, R_angles0)
""" Testing of aproximate parameters
From_transed = {}
for point in From:
From_transed[point] = Translation + R.dot(np.array(From[point]).transpose())
distance12 = fc.slope_distance(From['Point1'],From[point])
distance121 = fc.slope_distance(tuple(From_transed['Point1']),tuple(From_transed[point]))
print(distance12, distance121)
print(From_transed)"""
def Build_TFromT(x,From,identicals):
TFrom = np.array([]) #Transformed "From" coords
for i in range(len(identicals)):
PointID = identicals[i] #string
From_ith = np.asarray(From[PointID]) # array
R = Rotation_matrix(x[4],x[5],x[6])
T = np.array([x[0],x[1],x[2]])
TFrom_ith = T + x[3] * R.dot(From_ith)
TFrom = np.append(TFrom,TFrom_ith)
return TFrom
def Build_A(x,From,identicals):
# x (TX,TY,TZ,q,alpha,beta,gamma)
equation_count = 3 * len(identicals)
RX = X_Rotation(x[4])
RY = Y_Rotation(x[5])
RZ = Z_Rotation(x[6])
dRX = dX_Rotation(x[4])
dRY = dY_Rotation(x[5])
dRZ = dZ_Rotation(x[6])
R = Rotation_matrix(x[4],x[5],x[6])
A = np.zeros((equation_count,7))
for i in range(len(identicals)):
iii = 3*i
PointID = identicals[i] #string
From_ith = np.asarray(From[PointID]) # array
A[iii,0] = 1
A[iii+1,1] = 1
A[iii+2,2] = 1
D_alpha = dRX.dot(RY).dot(RZ).dot(From_ith)
D_beta = RX.dot(dRY).dot(RZ).dot(From_ith)
D_gamma = RX.dot(RY).dot(dRZ).dot(From_ith)
D_q = R.dot(From_ith)
A[iii,3] = D_q[0]
A[iii+1,3] = D_q[1]
A[iii+2,3] = D_q[2]
A[iii,4] = D_alpha[0]
A[iii+1,4] = D_alpha[1]
A[iii+2,4] = D_alpha[2]
A[iii,5] = D_beta[0]
A[iii+1,5] = D_beta[1]
A[iii+2,5] = D_beta[2]
A[iii,6] = D_gamma[0]
A[iii+1,6] = D_gamma[1]
A[iii+2,6] = D_gamma[2]
return A
def Helmert_LSM(R0,T0,R_angles0,From,To):
identicals = list(set(To.keys()) & set(From.keys()))
equation_count = 3*len(identicals) #This is Anthony's fault!
dx = np.empty(7)
x = np.array([T0[0],T0[1],T0[2],1.0,R_angles0[0],R_angles0[1],R_angles0[2]])
To_ith = To[PointID] #tuple
vI = np.empty((equation_count))
vII = np.empty((equation_count))
ToT = np.empty((equation_count)) # Transposed "To" coordinates
for i in range(len(identicals)):
PointID = identicals[i] #string
To_ith = To[PointID] #tuple
From_ith = From[PointID] #tuple
ToT[3*i] = To_ith[0]
ToT[3*i+1] = To_ith[1] # MAKE IT NICE! THIS IS GROSS
ToT[3*i+2] = To_ith[2]
return T,R,q,Transformed_From
\ No newline at end of file
......@@ -448,10 +448,5 @@ if Two_epochs:
print('All lines measured in Epoch 0 were also measured in Epoch 1.')
del line, all_lines_measured_same
point = fc.ParD_Hz(Pol_measurements_cart['Instrument_0']['Girder_10'],
Pol_measurements_cart['Instrument_0']['Girder_12'])
instr = fc.ParD_Hz(Pol_measurements_cart['Instrument_0']['Girder_10'],
Pol_measurements_cart['Instrument_0']['Girder_12'],'I')
print(point, instr)
print('End of the script')
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment