diff --git a/navipy/processing/SVP_function.py b/navipy/processing/SVP_function.py new file mode 100755 index 0000000000000000000000000000000000000000..b48f7a3f77d1c3c91bdbf93e6d004523f56026dc --- /dev/null +++ b/navipy/processing/SVP_function.py @@ -0,0 +1,183 @@ +import numpy as np +import pandas as pd + + + +def ird(ref_svp,scene): + """ + compute the rotational image difference between a reference memorized snapshot (SVP) and actual position + + : scene + : ref_svp: snapshot memorized reference (denormalized image) + : return: a Serie with angle and image rotation difference value + : typer:pd.Series + """ + + rms=pd.Series(index=['x','y','z','irdf','angle',]) + + angles=range(0,ref_svp.shape[1]+1,1) + + # for this image do the rotation and found the minimum + + image_rot=pd.Series(index=angles) + image_rot.name='irdf' + + for ang in angles: + + # rotation of the image for one degree angle + out = np.roll(scene,ang,1) + + # difference between view position and reference snapshot + diff_rot=out-ref_svp + + + # calculate Root Mean Square + + diff_val=np.sqrt(np.mean(diff_rot[:,:,:3]**2)) + + image_rot[ang]=diff_val + + rms['angle']=image_rot.argmin() + rms['irdf']=image_rot.min() + rms['x']=rms.irdf*np.cos(rms.angle) + rms['y']=rms.irdf*np.sin(rms.angle) + rms['z']=np.nan + + return rms + + + + +def weighted_ird(scene,mem_scenes): + + '''Return an homing vector direction based on an Image rotational difference weighted between some reference snapshots + + : param scene: actual scene , np.array + : param mem_scenes: list of set of views + : returns: dx,dy,dz,dyaw,dpitch,droll. + : rtypes : pd.Series + ''' + + df_svp= pd.DataFrame(index=range(0,len(mem_scenes)),columns=['irdf','angle']) + + for i in range(0,len(mem_scenes)): + + df_svp.loc[i]=ird(mem_scenes[i],scene) + + best_svp=pd.Series(index=['svp_nb','irdf']) + best_svp.loc['svp_nb']=np.argmin(df_svp.irdf) + best_svp['irdf']=min(df_svp.irdf) + + + # Take the best svp irdf and make the ratio for each others that gives the weighted irdf + + w_svp=pd.DataFrame(index=df_svp.index,columns=['w']) + + for i in df_svp.index : + w=best_svp['irdf']/df_svp.irdf[i] + w_svp.loc[i,['w']]=w + + + # Weighting of the vector direction based on circular statistics + + Heading=pd.Series(data=complex()) + j=complex(0,1) + + H=list() + + for i in df_svp.index: + + t=w_svp.loc[i]*np.exp(np.deg2rad(df_svp.loc[i,['angle']][0])*j) + H.append(t) + + Heading=np.sum(H) + + Hdeg=np.angle(Heading, deg=True) + + move_vector=pd.Series(index=['dx','dy','dz','dyaw','dpitch','droll']) + + + move_vector['dx']=np.cos((Hdeg)*np.pi/180) + + move_vector['dy']=np.sin((Hdeg)*np.pi/180) + + move_vector['dz']=0 + move_vector['dyaw','dpitch','droll']=[np.nan,np.nan,np.nan] + + + return move_vector + +def nposorient_around_ref(mydb,position_df,ref_pos,nb_snapshot,radius,blender_view): + + '''Return set of views around and oriented towards memorized location + :param mydb: Database environment + :param position_df: dataframe with the positions of the grid + :param ref_pos: df with x, y and z position of the reference snapshot + :param nb_snapshot: number of wanted set of views (multiple of 2) + :param radius: distance from memorized location to take snapshots + :param blender_view: viewing axis camera id y=90, if x=0 + :returns: list of reoriented image array, snaphots positions + :rtypes: array of np.array, pd.DataFrame + ''' + + + svp_all=pd.DataFrame(columns=['x','y','z','frame']) + nsvp_image=[] + + + + # angle of rotation + ang_deg=360/nb_snapshot + ang=np.deg2rad(ang_deg) + + # make the different view + + for i in range(0,nb_snapshot): + + x=ref_pos.x+radius*np.cos(ang*i) + y=ref_pos.y+radius*np.sin(ang*i) + z=ref_pos.z + + + svp_frame=pd.Series(index=['frame','x','y','z']) + + distance_arr=pd.Series(index=position_df.index) + + for index, pos in position_df.dropna().iterrows(): + + distance = (pos.x-x)**2 + distance += (pos.y-y)**2 + distance += (pos.z-z)**2 + + distance_arr[index]=distance + + svp_frame.frame=int(distance_arr.dropna().argmin()) + svp_frame.x=position_df.x[svp_frame.frame] + svp_frame.y=position_df.y[svp_frame.frame] + svp_frame.z=position_df.z[svp_frame.frame] + + svp_all.loc[i]=[svp_frame.x,svp_frame.y,svp_frame.z,int(svp_frame.frame)] + + reoriented_mem=[] + rot=list() + + for i,j in svp_all.iterrows(): + + ide=int(j.frame) + + image=mydb.scene(rowid=ide) + + alpha=np.floor(np.rad2deg(np.arctan2((ref_pos.y-j.y),(ref_pos.x-j.x)))) + + alpha=alpha+blender_view##because y view on blender + rot.append(alpha) + alpha=int(alpha) + + + svp_reorient=np.roll(image,alpha,1) + reoriented_mem.append(svp_reorient) + + + return reoriented_mem,svp_all + +