image_proc.py 8.79 KB
Newer Older
Obeid Jad Abdul Rahman's avatar
Obeid Jad Abdul Rahman committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22
#!/usr/bin/env python
"""
This node will read image from topic "/drone/front/compressed"
detect the centroid and will publish the centorid to the topic /centroids
and publsih the difference between slopes to the topic /sDiffs
The published data is filtered using a mean filter 
"""

import numpy as np
import cv2
import rospy
from sensor_msgs.msg import CompressedImage
from sensor_msgs.msg import Image
from std_msgs.msg import Float32
from std_msgs.msg import String
import sys
from sklearn.cluster import DBSCAN
from sklearn import metrics
import math
from cv_bridge import CvBridge, CvBridgeError
from itertools import combinations, chain
import datetime
jad's avatar
jad committed
23
from activation_class import NodeActivate,returnResp
Obeid Jad Abdul Rahman's avatar
Obeid Jad Abdul Rahman committed
24

jad's avatar
jad committed
25
class image_convert(NodeActivate):
Obeid Jad Abdul Rahman's avatar
Obeid Jad Abdul Rahman committed
26
    def __init__(self):
jad's avatar
jad committed
27
        super(image_convert,self).__init__("detectVanish")
Obeid Jad Abdul Rahman's avatar
Obeid Jad Abdul Rahman committed
28
        self.pub = rospy.Publisher("/centroids", Float32, queue_size=1)
Obeid Jad Abdul Rahman's avatar
Obeid Jad Abdul Rahman committed
29 30
        self.pubDiff = rospy.Publisher('/sDiffs',Float32,queue_size=1)
        self.pubForBag = rospy.Publisher('/imgForBag/front/compressed', CompressedImage, queue_size=1)
Obeid Jad Abdul Rahman's avatar
Obeid Jad Abdul Rahman committed
31
        #self.subscriber = rospy.Subscriber("/bebop/image_raw",Image, self.callback)
jad's avatar
jad committed
32
        self.subscriber = rospy.Subscriber("/bebop/image_raw/compressed",CompressedImage, self.callback)
Obeid Jad Abdul Rahman's avatar
Obeid Jad Abdul Rahman committed
33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198
        self.lsd = cv2.createLineSegmentDetector(0)
        self.real_centr = np.array([160,120])
        self.old_centr = np.array([160,120])
        self.real_sDiff = 0
        self.old_sDiff = 0
        self.length = 50
        self.br = CvBridge()
        self.alfa = 0.65
        self.alfa_sDiff = 0.1
        self.avLeftGlb = 0
        self.avRightGlb = 0
        self.arrSDiff = np.array([])
        self.filteredArrSDiff = np.array([])



    def findGoodLines(self,allLines,length):
        allLines = np.reshape(allLines,(len(allLines),4))
        allLines[:,[1, 2]] = allLines[:,[2, 1]]
        diffLines = np.diff(allLines)
        diffLines = np.delete(diffLines,1,1)
        lengths = np.linalg.norm(diffLines,axis = 1)
        and_arr = np.logical_and(np.absolute(diffLines[:,0]) > 10,np.absolute(diffLines[:,1]) > 10)
        and_arr = np.logical_and(and_arr,lengths > length)
        #print final_and
        goodLines = allLines[and_arr]
        goodDiffs = diffLines[and_arr]
        #print goodLines[:,2]>180
        #on Y1 (60<Y1<180)
        and_arr2 = np.logical_and(goodLines[:,2]>60 , goodLines[:,2]<180)
        #on Y2 (60<Y2<180)
        and_arr3 = np.logical_and(goodLines[:,3]>60 , goodLines[:,3]<180)
        final_and = np.logical_not(np.logical_and(and_arr2,and_arr3))
        goodLines = goodLines[final_and]
        goodDiffs = goodDiffs[final_and]
        goodLines[:,[1, 2]] = goodLines[:,[2, 1]]
        return goodLines,goodDiffs

    def findCartMat(self,Points,Vects):
        #suppose points are given in [Xa,Ya] and the vectors are given in [Xu,Yu] we have that c = YaXu - XaYu = diff(XaYu,YaXu)
        #so we need to swap the Vects Matrix
        Vects[:,[0,1]] = Vects[:,[1,0]]
        cmat = np.multiply(Points,Vects)
        cmat = np.diff(cmat)
        return cmat

    def findIntersec(self,abval,cval):
        abval[:,1] = np.multiply(-1,abval[:,1])
        cval = np.multiply(-1,cval)
        nbLines = cval.shape[0]
        index = np.fromiter(chain.from_iterable(combinations(range(nbLines), 2)), int)
        index = index.reshape(-1,2)
        A = abval[index,:]
        B = cval[index,:]
        interSec =  np.linalg.solve(A, B).reshape(-1,2)
        and_arr1 = np.logical_and(interSec[:,0] > 0,interSec[:,1] > 0)
        and_arr2 = np.logical_and(interSec[:,1] < 320,interSec[:,0] < 240)
        return interSec[np.logical_and(and_arr1,and_arr2)]

    def findCentroid(self,intersec):
        if intersec.shape[0] == 0:
            return 0,0
        if intersec.shape[0] == 1:
            return 1,intersec[0]

        db = DBSCAN(eps=20, min_samples=2).fit(intersec)
        db_labels = db.labels_
        labels, counts = np.unique(db_labels[db_labels>=0], return_counts=True)
        maxlabel = labels[np.argsort(-counts)[:1]]
        goodPoints = intersec[db_labels == maxlabel]
        centroid = np.mean(goodPoints,axis=0) 
        return 1,centroid

    def findAverageSlopes(self,points,abval,cval,zone):
        #zone = 3 : left
        #zone = 4 : right
        if zone == 4:
            or_x = np.logical_and(points[:,[0]] > self.real_centr[0],points[:,[2]] > self.real_centr[0])
            or_y = np.logical_and(points[:,[1]] > self.real_centr[1],points[:,[3]] > self.real_centr[1])
        else:
            or_x = np.logical_and(points[:,[0]] < self.real_centr[0],points[:,[2]] < self.real_centr[0])
            or_y = np.logical_and(points[:,[1]] > self.real_centr[1],points[:,[3]] > self.real_centr[1])
        and_arr = np.logical_and(or_x,or_y)
        and_arr = and_arr.reshape(-1)
        myabval = abval[and_arr]
        mycval = cval[and_arr]
        testMat = np.multiply(myabval,self.real_centr)
        testMat = np.sum(testMat,axis=1)
        testMat = testMat[np.newaxis]
        testMat = testMat.transpose()
              
              
        errMat = np.absolute(testMat + mycval)
              
        cond2 = (errMat < 500).reshape(-1)
        good_ab = myabval[cond2]
        slopes = np.divide(good_ab[:,0],good_ab[:,1])
        return np.mean(slopes)

    def getVanishingPoint(self,in_image):
        sDiff = 0
        self.gray = cv2.cvtColor(in_image, cv2.COLOR_BGR2GRAY)
        self.gray = cv2.resize(self.gray, (320,240), interpolation = cv2.INTER_AREA) 
        lines = self.lsd.detect(self.gray)[0]
        try:
            goodLines,directVect = self.findGoodLines(allLines=lines,length=self.length)

            #goodlines in (x1,y1,x2,y2)
            #directVect is (-b,a)
            #the equation of this line is ax+by+c = 0

            Cmat = self.findCartMat(goodLines[:,[0,1]],directVect)

            #here directVect is (a,-b) 

            frame = self.lsd.drawSegments(self.gray,goodLines)
            intersecPoints = self.findIntersec(directVect,Cmat)

            #here directVect is (a,b)
            #here Cmat = -c

            for point in intersecPoints:
                cv2.circle(frame,(int(point[0]),int(point[1])),3,(0,0,0),-1)
            res,centr = self.findCentroid(intersecPoints)
            if(res == 1 and type(centr[0]) == np.float32):
                if(centr[0] < 320 and centr[1] < 240):
                    self.real_centr = centr
                    self.real_centr = self.old_centr - self.alfa*(self.old_centr - self.real_centr)
                    self.old_centr = self.real_centr
            cv2.circle(frame,(int(self.real_centr[0]),int(self.real_centr[1])),8,(0,255,0),-1)
            self.old_centr = self.real_centr
            avLeft = self.findAverageSlopes(points=goodLines,abval=directVect,cval=Cmat,zone=3)
            if (not math.isnan(avLeft)):
                self.avLeftGlb =  avLeft
            avRight = self.findAverageSlopes(points=goodLines,abval=directVect,cval=Cmat,zone=4)
            if (not math.isnan(avRight)):
                self.avRightGlb =  avRight
            #print "avLeft"
            #print avLeft
            #print "avRight"
            #print avRight
            sDiff = self.avLeftGlb + self.avRightGlb
            print "sDiff"
            print sDiff
            self.arrSDiff = np.append(self.arrSDiff,sDiff)
            
            self.real_sDiff = sDiff
            self.real_sDiff = self.old_sDiff - self.alfa_sDiff*(self.old_sDiff - self.real_sDiff)
            self.filteredArrSDiff = np.append(self.filteredArrSDiff,self.real_sDiff)
            self.old_sDiff = self.real_sDiff
        except Exception as e:
            print e 
            print datetime.datetime.now()
        return frame,self.real_centr[0],self.real_sDiff

    def sendCentroid(self,dat):
        self.pub.publish(dat)
    def sendsDiff(self,dat):
        self.pubDiff.publish(dat)
    def saveData(self):
        np.savetxt("/home/jad/droneStats/sdiffsdat.csv", self.arrSDiff, delimiter=",")
        np.savetxt("/home/jad/droneStats/filteredArrSDiff.csv", self.filteredArrSDiff, delimiter=",")


    def callback(self,ros_data):
        #msg = ""
jad's avatar
jad committed
199 200
        if(self.node_active == False):
            return
jad's avatar
jad committed
201
        cv2_img = self.br.compressed_imgmsg_to_cv2(ros_data)
Obeid Jad Abdul Rahman's avatar
Obeid Jad Abdul Rahman committed
202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217
        #np_arr = np.fromstring(ros_data.data, np.uint8)
        #image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
        image,cx,sDiff = self.getVanishingPoint(cv2_img)
        #msg = str(cx) + "_" + str(sDiff)
        cv2.imshow('cv_img', image)
        if not rospy.is_shutdown():
            self.sendCentroid(cx)
            self.sendsDiff(sDiff)
            cmprsmsg = self.br.cv2_to_compressed_imgmsg(image)
            self.pubForBag.publish(cmprsmsg)
        cv2.waitKey(10)

def main(args):
    rospy.init_node('image_feature', anonymous=True)
    ic = image_convert()
    rospy.spin()
jad's avatar
jad committed
218
    #ic.saveData()
Obeid Jad Abdul Rahman's avatar
Obeid Jad Abdul Rahman committed
219 220 221 222 223 224
    cv2.destroyAllWindows()



if __name__ == '__main__':
    main(sys.argv)