Ce serveur Gitlab sera éteint le 30 juin 2020, pensez à migrer vos projets vers les serveurs gitlab-research.centralesupelec.fr et gitlab-student.centralesupelec.fr !

...
 
Commits (1)
......@@ -17,4 +17,15 @@
</node>
<node pkg="drone_project" name="detectVanish" type="image_proc.py">
</node>
<node pkg="drone_project" name="image_processor_stairs_round1" type="stairs_entrance_target.py">
<remap from="/drone_img" to="/bebop/image_raw/compressed"/>
</node>
<node pkg="drone_project" name="drone_vs_stairs_round1" type="go_to_stairs.py">
</node>
<node pkg="drone_project" name="image_analyzer3" type="stairs_target_up.py">
<remap from="/drone_img" to="/bebop/image_raw/compressed"/>
</node>
<node pkg="drone_project" name="drone_vs_stairs_round2" type="get_up_stairs.py">
</node>
</launch>
\ No newline at end of file
......@@ -17,6 +17,11 @@
<remap from="/curr_in" to="/bebop/odom" />
<remap from="/reset_cmd" to="/reset_cmd_z"/>
</node>
<node pkg="drone_project" name="ComputeAlt" type="compute_alt.py">
<remap from="/tar_in" to="/vel_in_alt"/>
<remap from="/vel_out" to="/vel_out_alt"/>
<remap from="/curr_in" to="/bebop/odom" />
</node>
<node pkg="drone_project" name="sendCommand" type="send_cmd.py">
</node>
</launch>
\ No newline at end of file
......@@ -24,9 +24,12 @@ class Sequencer(object):
self.doors_seq_list = ["doors",[["curveMotion"],["curveMotion"]],[["checkDoors","turnAng"],["checkDoors"]]]
self.init_seq_list = ["init",[ ["resetCmd"],[] ] ]
self.hallway_seq_list = ["hallway",[["detectVanish","computeTarX","computeTarY","computeTarZ"], [""] ] ]
self.stairs_seq_list = ["stairs",[["image_processor_stairs_round1","drone_vs_stairs_round1"],["drone_vs_stairs_round1"]],[["image_processor_stairs_round2","drone_vs_stairs_round2"],[""]]]
self.doors_seq = Sequence(self.doors_seq_list)
self.init_seq = Sequence(self.init_seq_list)
self.hallways_seq = Sequence(self.hallway_seq_list)
self.stairs_seq = Sequence(self.stairs_seq_list)
#self.new_seq_list = []
#self.new_seq = Sequence(self.new_seq_list)
......@@ -40,6 +43,7 @@ class Sequencer(object):
self.actv_publisher.publish(msg)
self.doors_seq = Sequence(self.doors_seq_list)
self.hallways_seq = Sequence(self.hallway_seq_list)
self.stairs_seq = Sequence(self.stairs_seq_list)
#self.new_seq = Sequence(self.new_seq_list)
else:
self.loop_pub.publish(3)
......@@ -53,6 +57,8 @@ class Sequencer(object):
#self.new_seq.seq_fun()
if(self.mode == self.hallways_seq.get_mode()):
self.hallways_seq.seq_fun()
if(self.mode == self.stairs_seq.get_mode()):
self.stairs_seq.seq_fun()
def main(args):
rospy.init_node('Sequencer', anonymous=True)
sc = Sequencer()
......
import numpy as np
from sklearn.cluster import DBSCAN
import pandas as pd
import tools
def clusterize_parallel(linesArray,eps,min_sample):
max_clusters = 10
normal_vector = linesArray[:,5:7]
try:
db = DBSCAN(eps, min_sample).fit(normal_vector)
labels = db.labels_
n_clusters_ = max(set(labels))
clusters = pd.Series([linesArray[labels == i] for i in xrange(0,n_clusters_+1)])
tab_ab=[]
for cluster in clusters:
a = np.mean(cluster[:,5])
b = np.mean(cluster[:,6])
tab_ab.append([a,b])
return (np.array(tab_ab),clusters, labels)
except Exception as e:
print("Error")
def clusterize_xy(linesArray,eps,min_sample):
#first, we compute the array of midlles of each segment
midXY = tools.findMiddlePointsArray(linesArray)
# we will clusterize on y(x) so we write y first
mid=np.concatenate((np.expand_dims(midXY[:,1],axis=1),np.expand_dims(midXY[:,0],axis=1)), axis =1)
try:
db = DBSCAN(eps, min_sample).fit(mid)
labels = db.labels_
n_clusters_ = max(set(labels))
clusters = pd.Series([linesArray[labels == i] for i in xrange(0,n_clusters_+1)])
return(clusters,labels)
except Exception as e:
print("Error")
def clusterize_offsets(linesArray,eps,min_samples):
vecteur_c = linesArray[:,7]
n_lines = np.size(vecteur_c)
X = vecteur_c.reshape(n_lines,1)
db = DBSCAN(eps, min_samples).fit(X)##
labels = db.labels_ #-1
nb_cluster = max(labels)
i_list = list(np.arange(nb_cluster+1))
gathered_on_C = pd.Series([linesArray[labels == i] for i in i_list])
tab_c=[]
for cluster in gathered_on_C:
c = np.mean(cluster[:,7])
tab_c.append(c)
# we add the well detected lines (label == -1) at the end:
if -1 in set(labels):
well_detected_lines = linesArray[labels == -1]
new_indexes = list(np.arange(nb_cluster+1 , nb_cluster+len(well_detected_lines)+1))
k = nb_cluster + 1
for line in well_detected_lines:
tab_c.append(line[7])
i_list.append(k)
k = k+1
s1 = pd.Series([[line] for line in well_detected_lines],index = new_indexes)
gathered_on_C = gathered_on_C.append(s1)
return (tab_c, i_list, gathered_on_C, labels)
\ No newline at end of file
#!/usr/bin/env python
"""
This node will just forward data from /vel_in to /vel_out
It will get the target velocity from the topic /vel_in
and republishes it to /vel_out
"""
import numpy as np
import rospy
import sys
from projectTools import GenTools
from Regulator import RegulatorClass
from std_msgs.msg import Float32,Int32
class ZCommand(RegulatorClass):
def __init__(self):
super(ZCommand,self).__init__(0.0,0,0)# p=0
#The Z is not like the others,we need to just publish the data to the out node, we will override the read_tar
def read_tar(self,ros_data):
#if(self.activation == 0):
# return
self.cmd = ros_data.data
#self.cmd_publisher.publish(ros_data.data)
def read_val(self,ros_data):
self.cmd = self.dc.computeCommand(self.currVal,self.targVal)
if(self.data_rec == 1):
self.cmd_publisher.publish(self.cmd)
self.data_rec = 0
def main(args):
rospy.init_node('computeLinZ', anonymous=True)
sc = ZCommand()
#rospy.init_node('send_command', anonymous=True)
rospy.spin()
if __name__ == '__main__':
main(sys.argv)
import cv2
import matplotlib.pyplot as plt
import numpy as np
import cluster
############################# OUTILS DE DESSIN ################################
def segments(img, nameString, lines, color,thickness,lsd):
drawn_img=lsd.drawSegments(img,lines)
def draw_line (screen, nameString, line, color, thickness):
H, W = screen.shape[:2]
a=line[5]
b=line[6]
c=line[7]
T=[]
def f(x,y):
return(a*x+b*y+c)
if f(0,0)*f(W,0)<0: #croise le bord haut de la fenetre
x=(-c/a)
y=0
T.append(x,y)
if f(W,0)*f(W,H)<0: #croise le bord droit de la fenetre
x=W
y=(-c-a*W)/b
T.append(x,y)
if f(0,H)*f(W,H)<0: #croise le bord en bas
x=(-c-b*H)/a
y=H
T.append(x,y)
if f(0,0)*f(0,H)<0:
x=0
y=-c/b
T.append(x,y)
cv2.line(screen, (int(T[0]),int(T[1])),(int(T[2]),int(T[3])), color, thickness, lineType=8, shift=0)
def draw_lines(drawn_img, nameString, lines, color, thickness):
for line in lines:
drawn_img = draw_line(drawn_img, nameString, line,color, thickness)
def draw_segments_couleur_angle(drawn_img, lines, tab_ab, labels, clusters_AB,thickness):
i=0
for line in lines:
label_i=labels[i]#+1
color=cluster_color(tab_ab,label_i)
drawn_img = cv2.line(drawn_img, (int(line[0]),int(line[1])),(int(line[2]),int(line[3])), color, thickness, lineType=8, shift=0)
i+=1
def draw_segments_couleur_angle_par_cluster(drawn_img, lines, tab_ab, labels, clusters_AB,thickness):
i=0
for label in labels:#on parcourt les labels
color=cluster_color(tab_ab,label)
lines_to_plot=clusters_AB[label]
for line in lines_to_plot:#on parcourt le cluster
drawn_img = cv2.line(drawn_img, (int(line[0]),int(line[1])),(int(line[2]),int(line[3])), color, thickness, lineType=8, shift=0)
def draw_segments(drawn_img, lines, color,thickness):
i=0
for line in lines:
drawn_img = cv2.line(drawn_img, (int(line[0]),int(line[1])),(int(line[2]),int(line[3])), color, thickness, lineType=8, shift=0)
i+=1
def draw_segment(drawn_img, line, color_tab, thickness):
drawn_img = cv2.line(drawn_img, (int(line[0]),int(line[1])),(int(line[2]),int(line[3])), color_tab, thickness, lineType=8, shift=0)
def points(img, pointsArray, radius = 3, color = 255,thickness=-1):#j'ai supprime l'argument nameString en deuxieme position
drawn_img = img
for x,y in pointsArray:
drawn_img = cv2.circle(img, (int(x),int(y)), radius, color, thickness)
#cv2.circle(img, center, radius, color[, thickness[, lineType[, shift]]])
####################### CLUSTERS #########################################################################
def clusters(vecteur_normalise,labels,desc_string,a=None, b=None):
#utilise plot, cas general
fig, ax = plt.subplots()
ax.scatter(vecteur_normalise[:,0], vecteur_normalise[:,1], c=labels)
if desc_string == "ab":
ax.set_xlabel('a norm')
ax.set_ylabel('b norm')
plt.title('repartion b en fonction de a')
elif desc_string == "c":
ax.set_xlabel('c')
ax.set_ylabel('c')
title = 'repartion de c sur le cluster ab, a=' + str(a) + ' b=' +str(b)
plt.title(title)
else:
print("vous devez quel type de cluster il faut afficher, rentrez ab ou c - en precisant a et b dans ce cas")
plt.show()
def empty_cluster(desc_string="ab", a=None, b=None):
#preciser le type de cluster: "ab"(par defaut) ou "c"
#cree le fond
if desc_string == "ab":
name = "repartion b en fonction de a"
x_name = "a norm"
y_name = "b norm"
if desc_string == "c":
name = "repartion de c sur le cluster ab, a=" + str(a) + " b=" +str(b)
x_name = "c"
y_name = "c"
font = cv2.FONT_HERSHEY_SIMPLEX
fontScale = 1
fontColor = 0
lineType = 2#cv2.LINE_AA
height, width = 200, 200
mid = height//2
if desc_string=="ab":
x_mid = mid
y_mid = mid
if desc_string=="c":
x_mid = mid
y_mid = mid
empty_window(name)
img_plot = empty_white_img (height,width)
#on commence par nommer l'axe y et faire pivoter l'image
cv2.putText(img_plot, y_name, (70,x_mid-20), font, fontScale, fontColor, lineType)
rows,cols,color_type = img_plot.shape
M = cv2.getRotationMatrix2D((cols/2,rows/2),90,1)
img_plot = cv2.warpAffine(img_plot,M,(cols,rows))
# on place les autres elements
mid = height//2
cv2.arrowedLine(img_plot, (x_mid, y_mid), (680, y_mid), 0, thickness=1, line_type=8, shift=0, tipLength=0.1)
cv2.arrowedLine(img_plot, (x_mid, y_mid), (x_mid,680), 0, thickness=1, line_type=8, shift=0, tipLength=0.1)
cv2.putText(img_plot, x_name, (width-200, y_mid+30), font, fontScale, fontColor, lineType) #img,text,coord,font
return name, img_plot
def cluster_plot(vecteur_normalise,labels, image_plot, name, desc_string="ab"):
#remplit le fond avec les donnees
height, width = 200, 200
mid = height//2
if desc_string=="ab":
x_mid = mid-50
y_mid = mid
scale = 80
cluster_plot = image_plot.copy()
if desc_string=="c":
x_mid = mid
y_mid = mid
scale = 800
cluster_plot = image_plot
n = vecteur_normalise.shape[0]
radius = 3
thickness=-1
for i in range (n):
cluster_plot = cv2.circle(cluster_plot,(int(vecteur_normalise[i,0]*scale)+x_mid,int(vecteur_normalise[i,1]*scale)+y_mid), radius, colors(labels[i]+1),thickness)
cv2.imshow(name,cluster_plot)# ou return cluster_plot
def cluster_plot_bis(vecteur_normalise,cluster_AB, tab_ab, labels, image_plot, name, desc_string="ab"):
#remplit le fond avec les donnees
height, width = 200, 200
mid = height//2
if desc_string=="ab":
x_mid = mid-50
y_mid = mid
scale = 80
cluster_plot = image_plot.copy()
if desc_string=="c":
x_mid = mid
y_mid = mid
scale = 80
cluster_plot = image_plot
n = vecteur_normalise.shape[0]
radius = 3
thickness=-1
n=np.max(labels)
i=0
for label_i in labels:
if label_i>-1:
cluster_AB_i=cluster_AB[label_i]
color=cluster_color(tab_ab,label_i)
vecteur_plot=cluster_AB_i[:,5:7]
for vecteur_normalises in vecteur_plot:
cluster_plot = cv2.circle(cluster_plot,(int(vecteur_normalises[0]*scale)+2*x_mid,int(vecteur_normalises[1]*scale)+y_mid), radius, color,thickness)
image_plot=cluster_plot
cv2.imshow(name,cluster_plot)# ou return cluster_plot*
cv2.waitKey(10)
def cluster_ab(vecteur_normalise,labels, image_plot, window_name):
cluster_plot(vecteur_normalise,labels, image_plot, window_name, "ab")
def cluster_ab_bis(vecteur_normalise,cluster_AB, tab_ab,labels, image_plot, window_name):
cluster_plot_bis(vecteur_normalise,cluster_AB, tab_ab, labels, image_plot, window_name, "ab")
def cluster_c(vecteur_normalise,labels, a, b):
vecteur_normalise = np.apply_along_axis(np.divide,0,vecteur_normalise,np.max(np.abs(vecteur_normalise)))#est ce que ca ne modifierait pas le tableau initial?
name, img_plot = empty_cluster("c", a, b)
cluster_plot(vecteur_normalise,labels, img_plot, name, "c")
def couleur():
colors=np.zeros((180,3))
for i in range (60):
colors[i][2]=255-i*4
colors[i+30][1]=4*i
colors[179-i-30][1]=4*i
colors[179-i][0]=255-4*i
return(colors)
def colors_angle(angle):
colors = couleur()
color=tuple((colors[angle,0],colors[angle,1],colors[angle,2]))
return(color)
def cluster_color(tab_ab,i):
a_moy=tab_ab[i,0]
b_moy=tab_ab[i,1]
if abs(a_moy)>0.01:
teta=int((np.arctan(b_moy/a_moy)*180)/(np.pi))
color=colors_angle(teta)
else:
color=colors_angle(1)
return(color)
####################### GESTION DES FENETRES #####################
def empty_white_img(height,width):
blank_image = np.ones((height,width,3), np.float32)*255#initialement np.uint8 #cv2.waitKey(0)np.float32?
return blank_image
def empty_black_img(height,width):
blank_image = np.zeros((height,width,3), np.float32)#initialement np.uint8 #cv2.waitKey(0)np.float32?
return blank_image
def copy_img(img):
drawn_img = cv2.cvtColor(img,cv2.COLOR_GRAY2RGB)#a sortir pour pouvoir afficher plusieur
return drawn_img
def empty_window(nameString):
cv2.namedWindow(nameString, cv2.WINDOW_KEEPRATIO)
def resize_window(nameString, img):
screen_res = 1280.0, 720.0
scale_width = screen_res[0] / img.shape[1]
scale_height = screen_res[1] / img.shape[0]
scale = min(scale_width, scale_height)
window_width = int(img.shape[1] * scale)
window_height = int(img.shape[0] * scale)
cv2.namedWindow(nameString, cv2.WINDOW_NORMAL)
cv2.resizeWindow(nameString, window_width, window_height)
return window_width, window_height
#################### GESTION colors GENERALE #######################
def colors(index):
c = [(0, 0, 0), (255, 0, 0), (0, 255, 0), (0, 0, 255), (255, 255, 0), (0, 255, 255), (255, 0, 255), (120, 120, 120), (120, 50, 0), (50, 120, 0), (0, 50, 120), (120, 0, 50), (0,120,50),(50, 0, 120)]
if index >= len(c):
return c[0]
else:
return c[index]
def colors_table(labels):
return [colors(label + 1) for label in labels]
def different_window(img, nameString, function, argArray, color, thickness):
drawn_img = copy_img(img)
function(drawn_img, argArray, color, thickness)
resize_window(nameString,img)
cv2.imshow(nameString,drawn_img)
\ No newline at end of file
#!/usr/bin/env python
"""
This node will read the stairs data and compute the desired command to send it to the /vel_in
"""
import numpy as np
import rospy
import sys
from activation_class import NodeActivate
from projectTools import GenTools, DroneCommand
from Regulator import RegulatorClass
from std_msgs.msg import Float32,Int32,String
import math
class ComputeTarStairsUp(NodeActivate):
def __init__(self):
super(ComputeTarStairsUp,self).__init__("drone_vs_stairs_round2")
self.dcx = DroneCommand(2.0,0,0.05)#initialement 0.1
self.dcang = DroneCommand(1.,0,0.0)#initialement 0.001
self.dcy = DroneCommand(1.0,0.0,0.0)#nerveux
self.dcz = DroneCommand(1.0,0,0)
self.stairs_presence = rospy.Subscriber("/stairs_presence",Int32,self.stairs_detection)#
#self.ready_to_get_up = rospy.Subscriber("/ready_to_get_up",Int32,self.position)
self.st_ctr_x = rospy.Subscriber("/stairs_center_x_relative",Float32,self.lateralPositionCheck)
self.st_ctr_y = rospy.Subscriber("/stairs_center_y_relative",Float32,self.callback)
self.low_st_y = rospy.Subscriber("/lowest_stair_y_relative",Float32,self.verticalPositionCheck)
self.st_ang = rospy.Subscriber("/stairs_angle_rad",Float32,self.directionCheck)
self.xcmd_publisher = rospy.Publisher("/vel_in_x",Float32,queue_size=1)
self.ycmd_publisher = rospy.Publisher("/vel_in_y",Float32,queue_size=1)
#self.ycmd_publisher = rospy.Publisher("/vel_out_y",Float32,queue_size=1)
self.angcmd_publisher = rospy.Publisher("/vel_in_z",Float32,queue_size=1)
self.zcmd_publisher = rospy.Publisher("/vel_in_alt",Float32,queue_size=1)
#self.deact_publisher = rospy.Publisher("/stairs_over",Int32,queue_size=1)
#TESTERS:
#self.lat_pos = rospy.Publisher("/lat_pos_ok",Int32,queue_size=1)
#self.dir_ok = rospy.Publisher("/dir_ok",Int32,queue_size=1)
#self.vert_pos_ok = rospy.Publisher("/get_up",Int32,queue_size=1)
self.stairs_detected = 1#0 enleve pour le test
#self.ready = 0#1#0enleve pour le test
self.directionOK = False
self.lateralPositionOK = False
self.verticalPositionOK = False
self.center_diff_x = 0 # >0 si escaliers a droite
# self.center_diff_y = 0 # >0 si escalier au fond de l'image
self.lowest_st_y = 0
self.diff_angle = 0
self.delta_angle = 0.02#3 # empirique
self.delta_centerX = 0.01 # a faire varier pour optimiser, normalement 0.04 devrait suffir
# self.delta_centerY = 0.2 # qqch >= 0
self.delta_Y = 0.25 # tiers inferieur de l'image
self.xcmd = 0
self.ycmd = 0
self.angcmd = 0
self.zcmd = 0
#self.deact_publisher.publish(0)
#NODE ACTIVATION CONTROL
def stairs_detection(self,ros_data):
self.stairs_detected = ros_data.data
#def position(self,ros_data):
# self.ready = ros_data.data
#NODE DEACTIVATION WHEN STAIRS OVER -- NOT USED YET
#def de_activation(self,ros_data):
# rec_msg = ros_data.data
# if rec_msg == 0:# pas d'escaliers ou pas ready
# self.deactivate = 1
# if rec_msg == 1:
# self.deactivate = 0
def turn(self):
self.angcmd = self.dcx.computeCommand(self.diff_angle,0.0)*0.1 #turn
self.xcmd = 0.0
def shift(self):
#center_diff_x > 0 <=> se decaler vers la droite, soit ycmd positif
if self.directionOK:
self.ycmd = self.dcy.computeCommand(self.center_diff_x,0.0)*0.7 #, self.dcy.computeCommand(self.diff_angle,0.0)) #shift(centerdiffX)
else:
self.ycmd = self.dcy.computeCommand(self.center_diff_x,0.0)
def goforwards(self):
self.angcmd = 0.0
self.ycmd = 0.0
self.xcmd = 0.25 #une marche par seconde
def stop(self):
self.angcmd = 0.0
self.ycmd = 0.0
self.xcmd = 0.0
self.zcmd = 0.0
def directionCheck(self,ros_data):
if self.stairs_detected:# and self.ready:
self.diff_angle = ros_data.data - math.pi/2
self.directionOK = abs(self.diff_angle) < self.delta_angle
if not(self.directionOK) :
self.turn()
# self.shift()#sur proposition d'Adrien
else:
self.angcmd = 0.0
def lateralPositionCheck(self, ros_data):
if self.stairs_detected:# and self.ready:
self.center_diff_x = ros_data.data
self.lateralPositionOK = abs(self.center_diff_x) < self.delta_centerX
if not(self.lateralPositionOK) :
self.shift()
def verticalPositionCheck(self, ros_data):
self.zcmd = 0.
self.lowest_st_y = ros_data.data
self.verticalPositionOK = self.lowest_st_y > 0.3
if self.directionOK and self.lateralPositionOK and not(self.verticalPositionOK): #> self.delta_Y:
self.zcmd = 0.35
#si on avance de 25cm on monte de 20cm car normes:
# Hauteur de marches : entre 17 et 21 cm.
# Giron de marche : entre 21 et 27 cm.
def callback(self,ros_data):
if self.stairs_detected:# and self.ready:
if self.directionOK and self.lateralPositionOK:
#if self.lateralPositionOK:
self.goforwards()
#if not(self.verticalPositionOK):
# self.zcmd = 0.25
#saturation
self.angcmd = GenTools.setMax(self.angcmd,0.3)
#self.ycmd = GenTools.setMax(self.ycmd,0.08)
self.ycmd = GenTools.setMax(self.ycmd,0.1)
#je ne mets pas de else stop pour pouvoir monter les escaliers jusqu'au bout mais ce serait interessant de tester
self.angcmd_publisher.publish(self.angcmd)
self.ycmd_publisher.publish(self.ycmd)
self.xcmd_publisher.publish(self.xcmd)
self.zcmd_publisher.publish(self.zcmd)
#self.positionOK_publisher.publish(self.deactivate)
#self.lat_pos.publish(self.lateralPositionOK)
#self.dir_ok.publish(self.directionOK)
else:#a supprimer aprs, c'est juste pour voir #ca ne marche pas dans la vraie vie a mon avis
self.stop()
def main(args):
rospy.init_node('drone_vs_stairs_round2', anonymous=True)
sc = ComputeTarStairsUp()
rospy.spin()
if __name__ == '__main__':
main(sys.argv)
\ No newline at end of file
#!/usr/bin/env python
"""
This node will read the stairs data and compute the desired command to send it to the /vel_in
"""
import numpy as np
import rospy
import sys
from activation_class import NodeActivate,returnResp
from projectTools import GenTools, DroneCommand
from Regulator import RegulatorClass
from std_msgs.msg import Float32, Int32, String
import math
class ComputeStairsTar(NodeActivate):
def __init__(self):
super(ComputeStairsTar,self).__init__("drone_vs_stairs_round1")
self.dcx = DroneCommand(1.0,0,0.05)
self.dcang = DroneCommand(1.0,0.,0)#-? I=0.5
self.stairs_presence = rospy.Subscriber("/stairs_presence",Int32,self.de_activation)
self.st_ctr_x = rospy.Subscriber("/stairs_center_x_relative",Float32,self.directionCheck)
self.st_ctr_y = rospy.Subscriber("/stairs_center_y_relative",Float32,self.computeStairsCommand)
self.angcmd_publisher = rospy.Publisher("/vel_in_ang",Float32,queue_size=1)
self.xcmd_publisher = rospy.Publisher("/vel_in_x",Float32,queue_size=1)
self.positionOK_publisher = rospy.Publisher("/ready_to_get_up",Int32,queue_size=1)
self.center_diff_x = 0 # >0 si escaliers a droite
self.center_diff_y = 0 # >0 si escalier au fond de l'image
self.delta_centerX = 0.005 # a faire varier pour optimiser
self.delta_centerY = 0.3 # cinquieme inferieur de l'ecran #plus etait trop restrictif
self.facingstairs = False
self.xcmd = 0
self.angcmd = 0
self.no_stairs = 0
self.ready_to_get_up = 0
self.positionOK_publisher.publish(0)
def de_activation(self,ros_data):
#if not(self.ready_to_get_up):
rec_msg = ros_data.data
if rec_msg == 0:
self.no_stairs = 1
#self.positionOK_publisher.publish(self.deactivate)
if rec_msg == 1:
self.no_stairs = 0
def directionCheck(self,ros_data):
#if not(self.ready_to_get_up):
self.center_diff_x = ros_data.data
self.facingstairs = abs(self.center_diff_x) < self.delta_centerX
def computeStairsCommand(self,ros_data):
#if not(self.ready_to_get_up):
self.center_diff_y = ros_data.data
if self.no_stairs == 0:
if not(self.facingstairs):
self.angcmd = self.dcx.computeCommand(self.center_diff_x,0.0) #turn(centerdiffX) #on veut un odg de 0.3 environ
self.xcmd = .1 #on vise 0.1 en odg
else:
self.angcmd = 0.0 #stay still
self.xcmd = .1
if self.center_diff_y - self.delta_centerY > 0: # if lowest stair close:
self.xcmd = 0.0
self.ready_to_get_up = 1
self.send_conf() # ready to get up
self.angcmd = GenTools.setMax(self.angcmd,0.3)
self.angcmd_publisher.publish(self.angcmd)
self.xcmd_publisher.publish(self.xcmd)
self.positionOK_publisher.publish(self.ready_to_get_up)
def main(args):
rospy.init_node('drone_vs_stairs_round1', anonymous=True)
sc = ComputeStairsTar()
rospy.spin()
if __name__ == '__main__':
main(sys.argv)
\ No newline at end of file
......@@ -13,10 +13,12 @@ class collectData:
self.xcmd_subscriber = rospy.Subscriber("/vel_out_x", Float32, self.read_vel_x)
self.ycmd_subscriber = rospy.Subscriber("/vel_out_y", Float32, self.read_vel_y)
self.zcmd_subscriber = rospy.Subscriber("/vel_out_z", Float32, self.read_vel_z)
self.angcmd_subscriber = rospy.Subscriber("/vel_out_alt", Float32, self.read_vel_alt)
self.cmd_publisher = rospy.Publisher("/cmd_vel",Twist,queue_size = 1)
self.xcmd = 0
self.ycmd = 0
self.zcmd = 0
self.altcmd = 0
def read_vel_x(self,ros_data):
#Just updating values
......@@ -33,11 +35,16 @@ class collectData:
self.zcmd = ros_data.data
self.send_twist()
def read_vel_alt(self,ros_data):
#update and sendData Here
self.altcmd = ros_data.data
self.send_twist()
def send_twist(self):
my_cmd = Twist()
my_cmd.linear.x = self.xcmd
my_cmd.linear.y = self.ycmd
my_cmd.linear.z = 0
my_cmd.linear.z = self.altcmd
my_cmd.angular.x = 0
my_cmd.angular.y = 0
my_cmd.angular.z = self.zcmd
......
#!/usr/bin/env python
from __future__ import print_function
import sys
import rospy
import cv2
from std_msgs.msg import String, Int32, Float32
from sensor_msgs.msg import CompressedImage, Image
from cv_bridge import CvBridge, CvBridgeError
import display
import tools
import cluster
import numpy as np
import math
from activation_class import NodeActivate
class ImageProcessorStairs(NodeActivate):
def __init__(self):
super(ImageProcessorStairs,self).__init__("image_processor_drone_vs_stairs_round1")
# subscribers
self.image_sub = rospy.Subscriber("/drone_img",CompressedImage, self.callback, queue_size = 1, buff_size = 2**22)
#self.deactivation = rospy.Subscriber("/ready_to_get_up",Int32,self.deactivate)
# publishers
self.image_all_lines_pub = rospy.Publisher("/image_topic_all_lines", Image,queue_size = 1)
self.image_stairs_pub = rospy.Publisher("/image_topic_stairs_lines", Image,queue_size = 1)
self.st_ctr_x_pub = rospy.Publisher("/stairs_center_x_relative", Float32, queue_size=1)
self.st_ctr_y_pub = rospy.Publisher("/stairs_center_y_relative", Float32, queue_size=1)
self.st_ang_pub = rospy.Publisher("/stairs_angle_rad", Float32, queue_size=1)
self.stairs_pres = rospy.Publisher("/stairs_presence", Int32, queue_size=1)
# initialisations
self.bridge = CvBridge()
#self.node_deactivated = 0
self.see_stairs = 1 #juste pour ne pas crasher des le debut
self.stairs_pres.publish(1)
self.st_ctr_x_pub.publish(0.0)
self.st_ctr_y_pub.publish(0.0)
self.st_ang_pub.publish(math.pi/2)
#def deactivate(self,ros_data):
# self.node_deactivated = ros_data.data
def noStairs(self):
#if not(self.node_deactivated):
self.see_stairs = 0
self.stairs_pres.publish(0)
def callback(self,data):
#if not(self.node_deactivated):
try:
frame = self.bridge.compressed_imgmsg_to_cv2(data, "bgr8")
# RESHAPE
heigth, width, depth = frame.shape
W = 1000.
imgscale = 0.6#W/width
newX,newY = frame.shape[1]*imgscale, frame.shape[0]*imgscale
newframe = cv2.resize(frame, (int(newX), int(newY)))
grayVid = cv2.cvtColor(newframe, cv2.COLOR_BGR2GRAY)
# DETECT SEGMENTS
lsdVid = cv2.createLineSegmentDetector(0)
segmentsArray = lsdVid.detect(grayVid)[0]
# CONVERTIT LES SEGMENTS EN LIGNES
linesArray = tools.findParamSegments(segmentsArray)
if str(linesArray) == "no stairs" or np.size(linesArray) == 0:
self.noStairs()
else:
# NE GARDE QUE LES LIGNES LES PLUS LONGUES
lmin = 30*imgscale
long_lines = tools.segmentsLongerThan(linesArray, lmin)
if np.size(long_lines)==0:
self.noStairs()
else:
drawn_image_long_lines = display.copy_img(grayVid)
display.draw_segments(drawn_image_long_lines,long_lines,(255,0,0),2)
image_long_lines = self.bridge.cv2_to_imgmsg(drawn_image_long_lines, encoding="bgr8")
self.image_all_lines_pub.publish(image_long_lines)
# CLUSTERISE SUR AB
epsAB=0.05 #initiallement 0.05
min_sampleAB=2
tab_ab, clustersAB, labels = cluster.clusterize_parallel(long_lines,epsAB,min_sampleAB)
stairs_label = tools.findLabelStairs(tab_ab, clustersAB, labels,0.8,2.4)
if stairs_label == "NoStairs":
self.noStairs()
else:
a_st,b_st = tab_ab[stairs_label]
stairs_angle = np.arctan2(b_st,a_st)
cluster_AB_i = clustersAB[stairs_label]
# CLUSTERISE SUR XY
clusterbis,labelbis = cluster.clusterize_xy(cluster_AB_i,50*imgscale,4)#initialement minsample =2, j'ai fait ca pour eviter de se prndre les rembardes
T=[]
for cluster_ in clusterbis :
taille=np.size(cluster_)
T.append(taille)
if T == [0] or T==[]:
self.noStairs()
else:
i_max = T.index(max(T))
print(i_max)
stairs_lines = clusterbis[i_max]
#LOWER STAIRS
length = len(stairs_lines)
lowest_st_nb = 4
if length > lowest_st_nb:
lowest_stairs_lines = stairs_lines[length-lowest_st_nb:length,:]
else:
lowest_stairs_lines = stairs_lines
low_x_moy, low_y_moy = tools.findMiddle(lowest_stairs_lines)
x_moy, y_moy = tools.findMiddle(stairs_lines)
drawn_image_stairs = display.copy_img(grayVid)
display.draw_segments(drawn_image_stairs, stairs_lines, (255,0,0), 2)
display.draw_segments(drawn_image_stairs, lowest_stairs_lines, (0,255,0), 3)
display.points(drawn_image_stairs, np.array([[x_moy,y_moy]]), 5, np.array([255,0,0]),-1)
display.points(drawn_image_stairs, np.array([[low_x_moy,low_y_moy]]), 5, np.array([0,255,0]),-1)#affiche le centre de l'escalier en blanc
display.points(drawn_image_stairs, np.array([[newX/2,newY/2]]), 3, np.array([0,0,0]),-1)
display.draw_segments(drawn_image_stairs, np.array([[newX/2,0,newX/2,newY],[0,0.8*newY,newX,0.8*newY]]), (0,0,0), 1)
display.draw_segments(drawn_image_stairs, np.array([[low_x_moy,0,low_x_moy,newY],[0,low_y_moy,newX,low_y_moy]]), (0,0,255), 2)
image_stairs = self.bridge.cv2_to_imgmsg(drawn_image_stairs, encoding="bgr8")
self.image_stairs_pub.publish(image_stairs)
self.st_ctr_x_pub.publish(low_x_moy/newX - 0.5)#est-ce vraiment necessaire?
self.st_ctr_y_pub.publish(low_y_moy/newY - 0.5)
self.st_ang_pub.publish(stairs_angle)
self.stairs_pres.publish(1)
except CvBridgeError as e:
pass
def main(args):
rospy.init_node('image_processor_drone_vs_stairs_round1', anonymous=True)
ia = ImageProcessorStairs()
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
if __name__ == '__main__':
main(sys.argv)
#!/usr/bin/env python
from __future__ import print_function
import sys
import rospy
import cv2
from std_msgs.msg import String, Int32, Float32
from sensor_msgs.msg import CompressedImage, Image
from cv_bridge import CvBridge, CvBridgeError
import display
import tools
import cluster
import numpy as np
import math
from activation_class import NodeActivate
class ImageProcessorStairsUp(NodeActivate):
def __init__(self):
super(ImageProcessorStairsUp,self).__init__("image_processor_drone_vs_stairs_round2")
# subscribers
#self.ready = rospy.Subscriber("/ready_to_get_up",Int32,self.activate)
self.image_sub = rospy.Subscriber("/drone_img",CompressedImage, self.callback, queue_size = 1, buff_size = 2**22)
# publishers
self.image_pub_all = rospy.Publisher("image_topic_all_lines", Image,queue_size = 1)
self.image_pub_stairs = rospy.Publisher("/image_topic_stairs_lines", Image,queue_size = 1)
self.image_pub_merged = rospy.Publisher("image_topic_merged_lines", Image,queue_size = 1)
self.image_pub_trpz_lin = rospy.Publisher("image_topic_trapeze_lin", Image,queue_size = 1)
self.image_pub_trpz_svr = rospy.Publisher("image_topic_trapeze_svr", Image,queue_size = 1)
self.st_ctr_x_pub = rospy.Publisher("/stairs_center_x_relative", Float32, queue_size=1)
self.st_ctr_y_pub = rospy.Publisher("/stairs_center_y_relative", Float32, queue_size=1)
self.low_st_y_pub = rospy.Publisher("/lowest_stair_y_relative", Float32, queue_size=1)
self.st_ang_pub = rospy.Publisher("/stairs_angle_rad", Float32, queue_size=1)
self.stairs_pres = rospy.Publisher("/stairs_presence", Int32, queue_size=1)
#self.node_active = 0
# trois estimateurs du centre des escaliers:
self.current_st_ctr_x = 0
self.old_st_ctr_x = 0
self.current_st_ctr_x_trpz_lin = 0
self.old_st_ctr_x_trpz_lin = 0
self.current_st_ctr_x_trpz_svr = 0
self.old_st_ctr_x_trpz_svr = 0
self.alpha = 0.2
# enregistrement de ces estimateurs et de leur correction (lissage)
self.center_stairs = np.array([])
self.corr_center_stairs = np.array([])
self.center_trpz_lin = np.array([])
self.corr_center_trpz_lin = np.array([])
self.center_trpz_svr = np.array([])
self.corr_center_trpz_svr = np.array([])
# initialisations:
self.bridge = CvBridge()
self.see_stairs = 1 #juste pour ne pas crasher des le debut
self.stairs_pres.publish(1)
self.st_ctr_x_pub.publish(0.0)
self.st_ctr_y_pub.publish(0.0)
self.low_st_y_pub.publish(0.0)
self.st_ang_pub.publish(math.pi/2)
#def activate(self,ros_data):
# self.node_active = ros_data.data
def noStairs(self):
#if self.node_active == 1:
self.see_stairs = 0
self.stairs_pres.publish(0)
def callback(self,data):
#if self.node_active == 1:
try:
frame = self.bridge.compressed_imgmsg_to_cv2(data, "bgr8")
# RESHAPE
heigth, width, depth = frame.shape
W = 1000.
imgscale = 1.0# W/width
newX,newY = frame.shape[1]*imgscale, frame.shape[0]*imgscale
newframe = cv2.resize(frame, (int(newX), int(newY)))
grayVid = cv2.cvtColor(newframe, cv2.COLOR_BGR2GRAY)
# DETECT SEGMENTS
lsdVid = cv2.createLineSegmentDetector(0)
segmentsArray = lsdVid.detect(grayVid)[0]
# CONVERTIT LES SEGMENTS EN LIGNES
linesArray = tools.findParamSegments(segmentsArray)
if str(linesArray) == "no stairs" or np.size(linesArray) == 0:
self.noStairs()
else:
# NE GARDE QUE LES LIGNES LES PLUS LONGUES
lmin = 30*imgscale
long_lines = tools.segmentsLongerThan(linesArray, lmin)
if np.size(long_lines)==0:
self.noStairs()
else:
# CLUSTERISE SUR AB
epsAB = 0.05
min_sampleAB = 2
tab_ab, clustersAB, labels = cluster.clusterize_parallel(long_lines,epsAB,min_sampleAB)
stairs_label = tools.findLabelStairs(tab_ab, clustersAB, labels,1.4,1.7)
if stairs_label == "NoStairs":
self.noStairs()
else:
cluster_AB_i = clustersAB[stairs_label]
# CLUSTERISE SUR XY
clusterbis,labelbis = cluster.clusterize_xy(cluster_AB_i,50*imgscale,4)#initialement minsample =2, j'ai fait ca pour eviter de se prndre les rembardes
T=[]
for cluster_ in clusterbis :
taille=np.size(cluster_)
T.append(taille)
if T == [0] or T==[]:
self.noStairs()
else:
i_max = T.index(max(T))
stairs_lines = clusterbis[i_max]
#LOWER STAIRS
length = len(stairs_lines)
lowest_st_nb = 4
if length > lowest_st_nb:
lowest_stairs_lines = stairs_lines[length-lowest_st_nb:length,:]
else:
lowest_stairs_lines = stairs_lines
a_st,b_st = np.mean(lowest_stairs_lines[:,5]),np.mean(lowest_stairs_lines[:,6])
stairs_angle = np.arctan2(b_st,a_st)
low_x_moy, low_y_moy = tools.findMiddle(lowest_stairs_lines)
# MERGE LINES
epsC = 5.0
min_sampleC = 2
merged_lines = np.empty((0, 8), dtype=np.float32)
mean_c, i_list, clustersABC, labels_c = cluster.clusterize_offsets(stairs_lines,epsC,min_sampleC)#clusterizeoffsets pas opti
new_lines = tools.mergeClusterSerieAfterC(i_list, clustersABC, labels_c)
merged_lines = np.concatenate((merged_lines,new_lines),axis = 0)
#L = np.mean(merged_lines[:,4])
#cond = np.greater(merged_lines[:,4],L)
#merged_lines = merged_lines[cond]
if np.size(merged_lines)<16: # 2 lines
self.noStairs()
else:
# SELECTION DE LA DERNIERE MARCHE
Ymax = tools.findLowestStair(stairs_lines)
#ESTIMATION DU CENTRE PAR LES MILIEUX DES SEGMENTS FUSIONNES
x_moy, y_moy = tools.findMiddle(merged_lines)
self.current_st_ctr_x = self.old_st_ctr_x - self.alpha*(self.old_st_ctr_x - x_moy)
self.old_st_ctr_x = self.current_st_ctr_x
self.center_stairs = np.append(self.center_stairs, x_moy)
self.corr_center_stairs = np.append(self.corr_center_stairs, self.current_st_ctr_x)
#ESTIMATION DU CENTRE PAR TRAPEZE DES MOINDRES CARRES
trapeze_lines, leftPTS, rightPTS, mid_line = tools.findTrapeze(merged_lines,'lin')
x_moy_trpz_lin, y_moy_trpz_lin = tools.findMiddle(trapeze_lines)
self.current_st_ctr_x_trpz_lin = self.old_st_ctr_x_trpz_lin - self.alpha*(self.old_st_ctr_x_trpz_lin - x_moy_trpz_lin)
self.old_st_ctr_x_trpz_lin = self.current_st_ctr_x_trpz_lin
self.center_trpz_lin = np.append(self.center_trpz_lin, x_moy_trpz_lin)
self.corr_center_trpz_lin = np.append(self.corr_center_trpz_lin, self.current_st_ctr_x_trpz_lin)
#ESTIMATION DU CENTRE PAR TRAPEZE DES MOINDRES CARRES
trapeze_lines_svr, leftPTS, rightPTS, mid_line = tools.findTrapeze(merged_lines,'svr')
x_moy_trpz_svr, y_moy_trpz_svr = tools.findMiddle(trapeze_lines_svr)
self.current_st_ctr_x_trpz_svr = self.old_st_ctr_x_trpz_svr - self.alpha*(self.old_st_ctr_x_trpz_svr - x_moy_trpz_svr)
self.old_st_ctr_x_trpz_svr = self.current_st_ctr_x_trpz_svr
self.center_trpz_svr = np.append(self.center_trpz_svr, x_moy_trpz_svr)
self.corr_center_trpz_svr = np.append(self.corr_center_trpz_svr, self.current_st_ctr_x_trpz_svr)
drawn_image_all_lines = display.copy_img(grayVid)
display.draw_segments(drawn_image_all_lines,long_lines,(255,0,0),2)
image_all_lines = self.bridge.cv2_to_imgmsg(drawn_image_all_lines, encoding="bgr8")
drawn_image_stairs = display.copy_img(grayVid)
display.draw_segments(drawn_image_stairs, stairs_lines, (255,0,0), 2)
display.draw_segments(drawn_image_stairs, lowest_stairs_lines, (0,0,255), 3)
display.points(drawn_image_stairs, np.array([[x_moy,y_moy]]), 5, np.array([0,0,0]),-1)#affiche le centre de l'escalier en blanc
display.points(drawn_image_stairs, np.array([[newX/2,newY/2]]), 3, np.array([0,0,0]),-1)
image_stairs = self.bridge.cv2_to_imgmsg(drawn_image_stairs, encoding="bgr8")
drawn_image_merged = display.copy_img(grayVid)
display.draw_segments(drawn_image_merged,merged_lines,(255,0,0),2)
display.points(drawn_image_merged, np.array([[x_moy_trpz_svr,y_moy_trpz_svr]]), 5, np.array([100,100,0]),-1)
display.points(drawn_image_merged, np.array([[x_moy_trpz_lin,y_moy_trpz_lin]]), 5, np.array([0,100,100]),-1)
display.points(drawn_image_merged, np.array([[x_moy,y_moy]]), 5, np.array([0,0,0]),-1)#affiche le centre de l'escalier en blanc
display.draw_segments(drawn_image_merged, np.array([[newX/2,0,newX/2,newY],[0,0.8*newY,newX,0.8*newY]]), (0,0,0), 2)
display.draw_segments(drawn_image_merged, np.array([[x_moy,0,x_moy,newY]]), (0,0,255), 2)
display.draw_segments(drawn_image_merged, np.array([[0,Ymax,newX,Ymax]]), (0,150,200), 2)
image_merged = self.bridge.cv2_to_imgmsg(drawn_image_merged, encoding="bgr8")
drawn_image_trpz_lin = display.copy_img(grayVid)
display.draw_segments(drawn_image_trpz_lin,trapeze_lines,(255,0,0),2)
display.draw_segments(drawn_image_trpz_lin,mid_line,(255,255,0),4)
display.points(drawn_image_trpz_lin, leftPTS, 2, np.array([0,255,0]),5)
display.points(drawn_image_trpz_lin, rightPTS, 2, np.array([0,0,255]),5)
display.points(drawn_image_trpz_lin, np.array([[x_moy_trpz_lin,y_moy_trpz_lin]]), 5, np.array([0,100,100]),-1)
display.points(drawn_image_trpz_lin, np.array([[x_moy,y_moy]]), 5, np.array([0,0,0]),-1)#affiche le centre de l'escalier en blanc
display.points(drawn_image_trpz_lin, np.array([[newX/2,newY/2]]), 3, np.array([0,0,0]),-1)
image_trpz_lin = self.bridge.cv2_to_imgmsg(drawn_image_trpz_lin, encoding="bgr8")
drawn_image_trpz_svr = display.copy_img(grayVid)
display.draw_segments(drawn_image_trpz_svr,trapeze_lines_svr,(255,0,0),2)
display.draw_segments(drawn_image_trpz_svr,mid_line,(255,255,0),4)
display.points(drawn_image_trpz_svr, leftPTS, 2, np.array([0,255,0]),5)
display.points(drawn_image_trpz_svr, rightPTS, 2, np.array([0,0,255]),5)
display.points(drawn_image_trpz_svr, np.array([[x_moy_trpz_svr,y_moy_trpz_svr]]), 5, np.array([100,100,0]),-1)
display.points(drawn_image_trpz_svr, np.array([[newX/2,newY/2]]), 3, np.array([0,0,0]),-1)
image_trpz_svr = self.bridge.cv2_to_imgmsg(drawn_image_trpz_svr, encoding="bgr8")
self.image_pub_all.publish(image_all_lines)
self.image_pub_stairs.publish(image_stairs)
self.image_pub_merged.publish(image_merged)
self.image_pub_trpz_lin.publish(image_trpz_lin)
self.image_pub_trpz_svr.publish(image_trpz_svr)
self.st_ctr_x_pub.publish(x_moy/newX - 0.5)#est-ce vraiment necessaire?
self.st_ctr_y_pub.publish(y_moy/newY - 0.5)
self.low_st_y_pub.publish(Ymax/newY - 0.5)
self.st_ang_pub.publish(stairs_angle)
#self.stairs_data_pub.publish(str(stairs_angle)+"_"+str(meanX) +"_"+str(newX/2)+"_"+str(meanY) +"_"+str(newY/2)+"_"+str(Ymax))#permet de connaitre le centre
self.stairs_pres.publish(1)
except CvBridgeError as e:
pass
def saveVal(self):
np.savetxt("/home/qsdfgh/projetDrones2A/centre_esc.csv", self.center_stairs, delimiter=",")
np.savetxt("/home/qsdfgh/projetDrones2A/centre_esc_corr.csv", self.corr_center_stairs, delimiter=",")
np.savetxt("/home/qsdfgh/projetDrones2A/centre_trpz_lin.csv", self.center_trpz_lin, delimiter=",")
np.savetxt("/home/qsdfgh/projetDrones2A/centre_trpz_lin_corr.csv", self.corr_center_trpz_lin, delimiter=",")
np.savetxt("/home/qsdfgh/projetDrones2A/centre_trpz_svr.csv", self.center_trpz_svr, delimiter=",")
np.savetxt("/home/qsdfgh/projetDrones2A/centre_trpz_svr_corr.csv", self.corr_center_trpz_svr, delimiter=",")
def main(args):
rospy.init_node('image_processor_drone_vs_stairs_round2', anonymous=True)
ia = ImageProcessorStairsUp()
try:
rospy.spin()
ia.saveVal()
except KeyboardInterrupt:
ia.saveVal()
print("Shutting down")
ia.saveVal()
cv2.destroyAllWindows()
if __name__ == '__main__':
main(sys.argv)
\ No newline at end of file
from __future__ import division
import numpy as np
from scipy.stats import linregress
from sklearn import linear_model #donnait la methode des moindre carres et jugee inadaptee
import sklearn.svm
import collections
import pandas as pd
import math
from statistics import median
from operator import truediv
def sign(x):
if x > 0:
return 1.
elif x < 0:
return -1.
elif x == 0:
return 1.
else:
return x
def findLength(x1,x2,y1,y2):
return math.sqrt((x1-x2)**2 + (y1-y2)**2)
def findParamSegment(segment):
x1 = segment[0]
y1 = segment[1]
x2 = segment[2]
y2 = segment[3]
b =-(x2-x1)
a =(y2-y1)
c =-(a*x1+b*y1)
norm = math.sqrt(a**2+b**2)*sign(b) #b sera toujours positif pour eviter toute redondance
if norm!=0:
return(np.array([a/norm,b/norm,c/norm]))
def findParamSegments(linesArray):
if linesArray is None:
return "no stairs"
n = np.shape(linesArray)[0]
x1 = linesArray[:,0,0]
y1 = linesArray[:,0,1]
x2 = linesArray[:,0,2]
y2 = linesArray[:,0,3]
L = np.sqrt(np.square(x1 -x2) + np.square(y1-y2))
A = (y2-y1)
B = -(x2-x1)
C = -(A*x1+B*y1)
normalVector = np.concatenate((np.expand_dims(A,axis=1),np.expand_dims(B,axis=1)), axis =1)
norm = np.linalg.norm(normalVector, axis =1)*np.sign(B) # B positif
# on supprime DEJA les lignes de norme nulle
linesArray = linesArray.reshape(n,4)
linesArray = linesArray[norm != 0]
normalVector = normalVector[norm != 0]
L = L[norm !=0]
C = C[norm !=0]
norm = norm[norm !=0]
lines = np.concatenate((linesArray, np.expand_dims(L,axis=1), np.apply_along_axis(np.divide, 0, normalVector,norm), np.expand_dims(np.divide(C,norm),axis=1)),axis=1)
return lines
def segmentsLongerThan(linesArray, lmin):
cond = np.greater(linesArray[:,4],lmin)
longLines = linesArray[cond]
return longLines
def mergeLines(lines):
if np.shape(lines)[0]>1:
a = np.mean(lines[:,5])
b = np.mean(lines[:,6])
c = np.mean(lines[:,7])
if b > 0.1:
x = np.concatenate((lines[:,0],lines[:,2]),axis= 0)
x1 = np.min(x)
x2 = np.max(x)
y1 = -(a*x1+c)/b # calcul potentiellement dangereux en cas de ligne verticale
y2 = -(a*x2+c)/b
else: # alors a est suffisamment grand
y = np.concatenate((lines[:,1],lines[:,3]),axis= 0)
y1 = np.min(y)
y2 = np.max(y)
x1 = -(b*y1+c)/a
x2 = -(b*y2+c)/a
l = findLength(x1,x2,y1,y2)
extremCoord = [x1,y1,x2,y2,l,a,b,c]
return extremCoord
else:
return lines[0]
def mergeClusterSerieAfterC(i_list, gathered_on_C,labels_c): # a appliquer a une sortie de lustering
mergedLinesArray = []
for i in i_list:
mergedLinesArray.append(mergeLines(gathered_on_C[i]))
return np.array(mergedLinesArray)
def findUpperSide(parallelLinesArray): # a appliquer a un ensemble de lignes paralleles
ymin1 = np.min(parallelLinesArray[:,1])
arg_min1 = np.argmin(parallelLinesArray[:,1])
ymin2 = np.min(parallelLinesArray[:,3])
arg_min2 = np.argmin(parallelLinesArray[:,3])
ymin = min(ymin1,ymin2)
if ymin == ymin1:
arg_min = arg_min1
else :
arg_min = arg_min2
return parallelLinesArray[arg_min].astype('float64')
def findLowerSide(parallelLinesArray): # a appliquer a un ensemble de lignes paralleles
ymax1 = np.max(parallelLinesArray[:,1])
arg_max1 = np.argmax(parallelLinesArray[:,1])
ymax2 = np.max(parallelLinesArray[:,3])
arg_max2 = np.argmax(parallelLinesArray[:,3])
ymax = max(ymax1,ymax2)
if ymax == ymax1:
arg_max = arg_max1
else :
arg_max = arg_max2
return parallelLinesArray[arg_max].astype('float64')
def findLinesIntersectionPoint(line1,line2):
a1=line1[0]
b1=line1[1]
c1=line1[2]
a2=line2[0]
b2=line2[1]
c2=line2[2]
y = (a1*c2 - a2*c1)/(b1*a2-b2*a1)
x = ((b1-b2)*y + c1-c2)/(a2-a1)
return(x,y)
def findLineIntersectionPoints(line,linesArray):
n = np.shape(linesArray)[0]
a=line[0]
b=line[1]
c=line[2]
A=linesArray[5]
B=linesArray[6]
C=linesArray[7]
y = (a*C - A*c)/(b*A-B*a)
x = ((b*np.ones((n,1))-B)*y + c*np.ones((n,1))-C)/(A-a*np.ones((n,1)))
return np.concatenate((x,y),axis=1)
def linearRegressionCoeff(pointsArray):
regr = linear_model.LinearRegression()
Xarray = pointsArray[:,0]
Yarray = pointsArray[:,1]
l = np.size(Xarray)
if (abs(max(Xarray)-min(Xarray))) > (abs(max(Yarray)-min(Yarray))):
#si la droite est globalement horizontale, LinearRegression ne bug pas
regr.fit(Xarray.reshape((l, 1)),Yarray)
m = regr.coef_[0]#regr.coef_ est un tableau a 1 dim avec linear reg et a 2 dim avec svm
p = regr.intercept_
#y=mx+p
div = math.sqrt(1+m**2)
a = -m/div
b = 1/div#b toujours positif
c = -p/div
else: #si la droite est globalement verticale, LinearRegression bug
regr.fit(Yarray.reshape((l, 1)),Xarray)
m = regr.coef_[0]
p = regr.intercept_
#x=my+p
div = math.sqrt(1+m**2)*sign(m)#b doit toujours etre positif
a = -1/div
b = m/div#b toujours positif
c = p/div
print(m,p)
return [a,b,c]
def linearRegressionSVRCoeff(pointsArray):
regr = sklearn.svm.SVR(kernel='linear', C=100, epsilon=30)
Xarray = pointsArray[:,0]
Yarray = pointsArray[:,1]
l = np.size(Xarray)
#if (abs(max(Xarray)-min(Xarray))) > (abs(max(Yarray)-min(Yarray)))*1.2:
# #si la droite est globalement horizontale, LinearRegression ne bug pas
# #facteur 1.2 empirique, rajoute pour le cas ou l'escalier prend toute la largeur de l'image
# regr.fit(Xarray.reshape((l, 1)),Yarray)
# m = regr.coef_[0][0]#regr.coef_ est un tableau a 1 dim avec linear reg et a 2 dim avec svm
# p = regr.intercept_[0]
# #y=mx+p
# div = math.sqrt(1+m**2)
# a = -m/div
# b = 1/div#b toujours positif
# c = -p/div
#else: #si la droite est globalement verticale, LinearRegression bug
regr.fit(Yarray.reshape((l, 1)),Xarray)
m = regr.coef_[0][0]
p = regr.intercept_[0]
#x=my+p
div = math.sqrt(1+m**2)*sign(m)#b doit toujours etre positif
a = -1/div
b = m/div#b toujours positif
c = p/div
print(m,p)
return [a,b,c]
def findLeftRightPoints(linesArray):
tableauRangPointDroit = np.greater(linesArray[:,2],linesArray[:,0])#1 si x2 a droite
rightXarray = np.where(tableauRangPointDroit,linesArray[:,2],linesArray[:,0])
rightYarray = np.where(tableauRangPointDroit,linesArray[:,3],linesArray[:,1])
leftXarray = np.where(tableauRangPointDroit,linesArray[:,0],linesArray[:,2])
leftYarray = np.where(tableauRangPointDroit,linesArray[:,1],linesArray[:,3])
rightPointsArray = np.concatenate((np.expand_dims(rightXarray,axis=1),np.expand_dims(rightYarray,axis=1)),axis=1)
leftPointsArray = np.concatenate((np.expand_dims(leftXarray,axis=1),np.expand_dims(leftYarray,axis=1)),axis=1)
return leftPointsArray, rightPointsArray
#return leftPointsArray.astype('float32'), rightPointsArray.astype('float32')
def findTrapeze(mergedLinesArray,chosen_regression_string = 'lin'): #'lin' ou 'svr'
n = np.shape(mergedLinesArray)[0]
leftPointsArray, rightPointsArray = findLeftRightPoints(mergedLinesArray)
midPointsArray = findMiddlePointsArray(mergedLinesArray)
midLineCoef = linearRegressionCoeff(midPointsArray)
goodlinesLeftPoints = leftPointsArray
goodlinesRightPoints = rightPointsArray
# ELIMINE LES POINTS DONT LES MILIEUX SONT TROP LOINS DE LA VALEUR THEORIQUE
#estimated_mid = findLineIntersectionPoints(midLineCoef, mergedLinesArray)
#mid_deviation = midPointsArray - estimated_mid
#mid_mean = np.mean(mid_deviation)#should be 0
#mid_variance = np.var((mid_deviation), axis = 1)
#print("mid mean, var")
#print(mid_mean,mid_variance)
#cond_gch = np.greater(mid_deviation)
# ELIMINE LES POINTS QUI SE SITUENT A UNE DISTANCE TROP GRANDE DE LA MOYENNE
#goodlinesLeftPoints = np.empty((0, 2), dtype=np.float32)
#goodlinesRightPoints = np.empty((0, 2), dtype=np.float32)
#for i in range(n):
# center_strs = findLinesIntersectionPoint(midLineCoef,mergedLinesArray[i,5:])[0]#x coordinate of the theoretical center of the stair i
# print("i,left,mid,right")
# print(i,leftPointsArray[i,0],center_strs,rightPointsArray[i,0])
# if leftPointsArray[i,0] < center_strs*0.5 and rightPointsArray[i,0] > center_strs*1.5:
# goodlinesLeftPoints = np.concatenate((goodlinesLeftPoints,np.expand_dims(leftPointsArray[i],axis=0)),axis=0)
# goodlinesRightPoints = np.concatenate((goodlinesRightPoints,np.expand_dims(rightPointsArray[i],axis=0)),axis=0)
middleLine = np.expand_dims(np.concatenate((findLinesIntersectionPoint(midLineCoef, mergedLinesArray[0,5:]),findLinesIntersectionPoint(midLineCoef, mergedLinesArray[n-1,5:]))),axis=0)
if chosen_regression_string == 'lin':
leftSide = linearRegressionCoeff(goodlinesLeftPoints)
rightSide = linearRegressionCoeff(goodlinesRightPoints)
elif chosen_regression_string == 'svr':
leftSide = linearRegressionSVRCoeff(goodlinesLeftPoints)
rightSide = linearRegressionSVRCoeff(goodlinesRightPoints)
upperSide = findUpperSide(mergedLinesArray)
lowerSide = findLowerSide(mergedLinesArray)
upperSide = upperSide[5:]
lowerSide = lowerSide[5:]
vertex = []
vertex.append(findLinesIntersectionPoint(rightSide, upperSide))#en haut a droite
vertex.append(findLinesIntersectionPoint(leftSide, upperSide))#en haut a gauche
vertex.append(findLinesIntersectionPoint(leftSide, lowerSide))#en bas a gauche
vertex.append(findLinesIntersectionPoint(lowerSide, rightSide))#en bas a droite
rightline = [vertex[0][0],vertex[0][1],vertex[3][0],vertex[3][1]]
leftline = [vertex[1][0],vertex[1][1],vertex[2][0],vertex[2][1]]
if chosen_regression_string=='svr':
epsilonSVR = 30
upperline = [vertex[0][0]-epsilonSVR,vertex[0][1],vertex[1][0]+epsilonSVR,vertex[1][1]]
lowerline = [vertex[2][0]-epsilonSVR,vertex[2][1],vertex[3][0]+epsilonSVR,vertex[3][1]]
r1 = [vertex[0][0]-epsilonSVR,vertex[0][1],vertex[3][0]-epsilonSVR,vertex[3][1]]
r2 = [vertex[0][0]+epsilonSVR,vertex[0][1],vertex[3][0]+epsilonSVR,vertex[3][1]]
l1 = [vertex[1][0]-epsilonSVR,vertex[1][1],vertex[2][0]-epsilonSVR,vertex[2][1]]
l2 = [vertex[1][0]+epsilonSVR,vertex[1][1],vertex[2][0]+epsilonSVR,vertex[2][1]]
trapezeLinesArray = findParamSegments(np.reshape([rightline,upperline,leftline,lowerline, r1, r2, l1, l2 ],(8,1,4)))
else:
upperline = [vertex[0][0],vertex[0][1],vertex[1][0],vertex[1][1]]
lowerline = [vertex[2][0],vertex[2][1],vertex[3][0],vertex[3][1]]
trapezeLinesArray = findParamSegments(np.reshape([rightline,upperline,leftline,lowerline],(4,1,4)))
return trapezeLinesArray, goodlinesLeftPoints, goodlinesRightPoints, middleLine # seul trapezeLinesArray est vraiment utile, le reste pourra etre supprime dans une implementation reelle
def findLabelStairs(tab_ab, clustersAB, labels, angle_min, angle_max):
n = np.shape(clustersAB)[0]
labelsList=[]
sizesList=[]
for i in range (n):##?
if np.arctan2(tab_ab[i][1],tab_ab[i][0]) > angle_min and np.arctan2(tab_ab[i][1],tab_ab[i][0]) < angle_max:
labelsList.append(i)
sizesList.append(len(clustersAB[i]))
if labelsList == []: # si pas d'escalier
return "NoStairs" # risque de confusion avec label = 0 si on met false
i_max = sizesList.index(max( sizesList))
return labelsList[i_max]
def findMiddle(linesArray): #a appliquer au cluster stairs pour trouver son centre
x = [linesArray[:,0],linesArray[:,2]]
y = [linesArray[:,1],linesArray[:,3]]
meanX = np.mean(x)
meanY = np.mean(y)
return meanX, meanY
def findMiddlePointsArray(linesArray): # a appliquer au cluster stairs pour trouver les centres de chaque marche
x = np.concatenate((np.expand_dims(linesArray[:,0],axis=1),np.expand_dims(linesArray[:,2],axis=1)),axis=1)
y = np.concatenate((np.expand_dims(linesArray[:,1],axis=1),np.expand_dims(linesArray[:,3],axis=1)),axis=1)
meansX = np.mean(x,axis=1)
meansY = np.mean(y,axis=1)
return np.concatenate((np.expand_dims(meansX,axis=1),np.expand_dims(meansY,axis=1)),axis=1)
def findLowestStair(linesArray):
y = [linesArray[:,1],linesArray[:,3]]
return np.max(y)
# unused exceptions
class NoStairs(Exception):
def __init__(self):
Exception.__init__(self,"no stairs detected")
class NotEnoughlines(Exception):
def __init__(self):
Exception.__init__(self,"not enough lines detected, impossible to compute operations on these lines")
\ No newline at end of file