SA-Kenan/04_Spurerkennung/dev/dev_Linefitting_and_angle.py

59 lines
1.4 KiB
Python
Raw Normal View History

2022-05-09 20:30:26 +02:00
# -*- coding: utf-8 -*-
"""
Created on Fri Mar 4 19:02:17 2022
@author: Kenan
Skript um Geradengleichung und winkel der geraden aus einer menge von punkten zu bestimmen
"""
import numpy as np
from matplotlib import pyplot as plt
# KOSY: Y: Nach oben, X: Nach rechts
# create array: column: x y radius colornumber
points = np.array([[2,3,5,1],
[7,6,5,1]])
points = np.array([[1,1,5,1],
[1,2,5,1],
[2,2,5,1],
[3,1,5,1],
[4,1,5,1],
[5,1,5,1],
[6,1,5,1],
[5,2,5,1],
[4,3,5,1],
[5,5,5,1],
[7,5,5,1],
[2,6,5,1]])
x = points[:,0] # x coordinates
y = points[:,1] # y coordinates
deg = 1 # Degree of the fitting polynomial
[m, c] = np.polyfit(x,y,deg) # m*x+c = g(x)
#Winkelberechnung mit atan2. Mathematisch. Zur X-Achse. Positiv gegen UZS.
x_1=1
y_1=m*x_1
alpha_atan2 = np.arctan2(y_1,x_1)*180/np.pi
print(f"alpha atan2: {round(alpha_atan2,2)}°")
# winkelberechnung selbst. Zur Y-Achse aber
alpha = np.arctan(1/m) # Winkel zur Y-Achse in [rad]
alpha_deg = alpha * 180/np.pi # Winkel in [°]
print(f"alpha: {round(alpha_deg,2)}°")
f1 = plt.plot(x,y, 'o')
plt.grid(True)
x_values_to_plot = np.linspace(0,7,100)
y_fitted_line = m*x_values_to_plot+c
plt.plot(x_values_to_plot, y_fitted_line)