Merge branch 'charlotte' into develop

navipy/processing/SVP_function.py

+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
+
+