#!/usr/bin/env python # coding: utf-8 # In[4]: import pandas as pd import numpy as np import scipy as sci import matplotlib.pyplot as plt # In[5]: data = pd.read_csv("/mnt/flashblade/agis/pose_data.csv", header=None,index_col=[0,1]) data.drop(columns=[201], inplace=True) # Drop accidental NaNs # In[6]: #mi = pd.MultiIndex.from_frame(data) data.sort_index(level=[0,1], ascending=[1, 1], inplace=True) # In[7]: data # In[8]: #data.loc[(slice('Camera::NE_'), slice(0)), :] #data.xs((0,1), level=1) data.loc[(['Camera::NE_E_WIDE','Camera::SE_E_WIDE'],)] #data.loc[(slice('Camera::NE_E_WIDE':'Camera::SE_E_WIDE'), slice(None)), : ] # In[9]: # handy constants ANY = slice(None) idx = pd.IndexSlice #sensor_ids = ['Camera::SE_E_WIDE', 'Camera::NE_E_WIDE'] sensor_ids = ANY dimensions = [0,1] selected_indices = idx[ sensor_ids, dimensions ] sliced = data.loc[selected_indices, :] sliced # In[ ]: # In[10]: cam_array = np.array(sliced.values) # In[11]: cam_array.shape # In[ ]: # In[ ]: # In[12]: # mean = np.mean(test_cam_array, axis=1) # In[ ]: # In[13]: cov = np.cov(cam_array.astype(np.double)) # In[14]: cov.shape np.mean(cov) # In[15]: fig = plt.imshow(cov/np.median(cov)) plt.colorbar(fig) # In[16]: cameras = data.index.to_frame().loc[:,0].values cameras = np.unique(cameras) # In[17]: cameras # In[18]: def slice_data(cameras, dimensions, data): """ Accepts a list of cameras and a list of dimensions. You can use ANY instead of a list, if you want all of the cameras. """ idx = pd.IndexSlice sensor_ids = cameras dimensions = dimensions selected_indices = idx[ sensor_ids, dimensions ] sliced = data.loc[selected_indices, :] return sliced # In[19]: def covariance_for_camera(camera, data): sliced = slice_data([camera], ANY, data) sliced_array = np.array(sliced.values) cov = np.cov(sliced_array.astype(np.double)) return cov # In[ ]: # In[20]: positive_cor = [] negative_cor = [] for camera in cameras: print(camera) idx = pd.IndexSlice cov = covariance_for_camera(camera, data) print(cov[0,0]/cov[0,1]) #sb = axs[i].imshow(cov/np.median(cov)) fig = plt.imshow(cov) plt.colorbar(fig) plt.show() if cov[0,1]>0: positive_cor.append(camera) else: negative_cor.append(camera) # In[21]: positive_cor # In[22]: negative_cor # In[23]: def hat(vector_3d): x = vector_3d[0] y = vector_3d[1] z = vector_3d[2] hatted = np.array( [ [0, -z, y], [z, 0, -x], [-y, x, 0], ] ) return hatted # In[24]: cross = hat([1,2,3]) print(cross) # In[25]: np.arange(0,1,1) # In[78]: def calc_error(az, el, base_cov, dimension, distance): vec = np.array([np.sin(el)*np.cos(az), np.sin(el)*np.sin(az), np.cos(el)]) * distance h = hat(vec) vec_cov = -np.matmul(h, base_cov) vec_cov = np.matmul(vec_cov, h) z_var = vec_cov[dimension, dimension] z_stdev = np.sqrt(z_var) z_error_cm = z_stdev*3*100 return z_error_cm def plot_errors_for_horizontal_angle_range(cov): distance_meters = 120 angles_az = np.arange(-70, 70, 1) / 180 * np.pi # angles_el = np.arange(0, 90, 5) / 180 * np.pi angles_el = np.zeros((1,1)) angles_el[0,0] = 90 / 180 * np.pi az, el = np.meshgrid(angles_az, angles_el) errors_x = np.zeros((len(angles_el), len(angles_az))) errors_y = np.zeros((len(angles_el), len(angles_az))) errors_z = np.zeros((len(angles_el), len(angles_az))) print("3*sigma extrinsic rotation (deg): {}".format( np.sqrt(np.diag(cov))*3/np.pi*180)) for i in np.arange(0,len(angles_el), 1): for j in np.arange(0,len(angles_az), 1): errors_x[i,j] = calc_error(az[i,j], el[i,j], cov, dimension=0, distance=distance_meters) errors_y[i,j] = calc_error(az[i,j], el[i,j], cov, dimension=1, distance=distance_meters) errors_z[i,j] = calc_error(az[i,j], el[i,j], cov, dimension=2, distance=distance_meters) #print("az {}, el {}, error {}, i {}, j {}".format(az[i,j], el[i,j], errors[i,j], i, j)) ax = plt.subplot(111, projection='polar') ax.set_thetamin(-90) ax.set_thetamax(90) ax.set_theta_zero_location("N") ax.plot(az[0,:], errors_x[0,:], 'r', label='Point error x (cm)') ax.plot(az[0,:], errors_y[0,:], 'g', label='Point error y (cm)') ax.plot(az[0,:], errors_z[0,:], 'b', label='Point error z (cm)') ax.set_xlabel('Direction angle (deg) on the x-y plane.') ax.set_ylabel('3D point error, 3*sigma (cm) at {} m'.format(distance_meters)) ax.set( ylim=(0, 10)) ax.legend() plt.show() # ax.plot(az, errors, linewidth=1, linestyle='solid', polar=True) # plt.show # ax.fill(az, errors, 'b', alpha=0.1) # #ax.contourf(az, el, errors) # In[79]: # RADAR PLOTS # *********** for camera in cameras: print(camera) idx = pd.IndexSlice cov = covariance_for_camera(camera, data) # cov = np.array( # [ # [1.3539e-06, 1.3539e-06, 0], # [1.3539e-06, 1.3539e-06, 0], # [0, 0, 0], # ] # ) plot_errors_for_horizontal_angle_range(cov) # fig = plt.imshow(cov) # plt.colorbar(fig) # plt.show() # In[75]: def covariance_for_dimensions(dimensions, data): idx = pd.IndexSlice sensor_ids = ANY selected_indices = idx[ sensor_ids, dimensions ] sliced = data.loc[selected_indices, :] sliced_array = np.array(sliced.values) cov = np.cov(sliced_array.astype(np.double)) return cov # In[22]: c = covariance_for_dimensions([0,1], data) # In[ ]: # In[80]: north_south = ['Camera::NE_N_WIDE', 'Camera::NW_N_WIDE', 'Camera::N_N_FISHEYE', 'Camera::N_N_NARROW', # 'Camera::SE_N_REARVIEW', # 'Camera::SW_N_REARVIEW', 'Camera::SE_S_WIDE', 'Camera::SW_S_WIDE', 'Camera::S_S_FISHEYE', 'Camera::S_S_NARROW', # 'Camera::NE_S_REARVIEW', # 'Camera::NW_S_REARVIEW' ] # In[81]: north_south # In[152]: def covariance_for(cameras_, dimensions_, data): sliced_ = slice_data(cameras_, dimensions_, data) sliced_array_ = np.array(sliced_.values) cov_ = np.cov(sliced_array_.astype(np.double)) return cov_ c = covariance_for(ANY, [0], data) # c = covariance_for(north_south, [1], data) c.shape # In[153]: 1/np.diag(c) # In[154]: diag_factor = np.diag(np.sqrt(1/np.diag(c))) # In[155]: cor = np.matmul(diag_factor, c) cor = np.matmul(cor, diag_factor) # In[156]: fig = plt.imshow(cor, vmin=-1, vmax=1, cmap='coolwarm') plt.colorbar(fig) # In[157]: plt.bar(cameras, cor[0,:]) # In[ ]: # In[ ]: # In[ ]: