Python Utils.distBetweenPoints方法代码示例

# 需要导入模块: from utils.utils import Utils [as 别名]
# 或者: from utils.utils.Utils import distBetweenPoints [as 别名]
    def execute(self, userdata):
        if self.comms.isAborted or self.comms.isKilled:
            self.comms.abortMission()
            return 'aborted'

        if not self.comms.retVal or \
           len(self.comms.retVal['samples']) == 0:
            self.comms.adjustDepth = self.comms.curDepth
            rospy.sleep(rospy.Duration(0.1))
            return 'approaching'

        curArea = self.comms.retVal['samples'][0]['area']
        rospy.loginfo("Area: {}".format(curArea))
        if curArea  > self.comms.grabbingArea or \
           self.comms.curDepth > self.comms.grabbingDepth:
            self.comms.adjustDepth = self.comms.curDepth
            return 'completed'

        samples = self.comms.retVal['samples']
        closest = min(samples,
                      key=lambda c:
                      Utils.distBetweenPoints(c['centroid'],
                                              (self.centerX, self.centerY)))
        dx = (closest['centroid'][0] - self.centerX) / self.width
        dy = (closest['centroid'][1] - self.centerY) / self.height

        self.comms.sendMovement(f=-self.ycoeff*dy, sm=self.xcoeff*dx,
                                d=self.comms.curDepth + 0.1,
                                h=self.comms.adjustHeading,
                                timeout=2,
                                blocking=False)
        return 'approaching'

# 需要导入模块: from utils.utils import Utils [as 别名]
# 或者: from utils.utils.Utils import distBetweenPoints [as 别名]
    def threshold(self, img, color):
        self.allCentroidList = []
        self.allAreaList = []
        self.allRadiusList = []        
        
        #params = self.getParams(color)
        
        # Perform thresholding
        mask = cv2.inRange(img, self.redParams['lo3'], self.redParams['hi3'])
        kern = cv2.getStructuringElement(cv2.MORPH_RECT, (5,5))

        # return cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)

        thresImg1 = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kern)
        # thresImg1 = cv2.dilate(mask, kern, iterations=1)
        
        mask2 = cv2.inRange(img, self.redParams['lo4'], self.redParams['hi4'])
        thresImg2 = cv2.dilate(mask2, kern, iterations=2)

        # binImg = cv2.bitwise_or(thresImg1, thresImg2)

        binImg = thresImg1

        # return cv2.cvtColor(binImg, cv2.COLOR_GRAY2BGR)
        
        # Find contours
        scratchImg = binImg.copy()
        scratchImgCol = cv2.cvtColor(binImg, cv2.COLOR_GRAY2BGR)
        contours, hierachy = cv2.findContours(scratchImg, cv2.RETR_EXTERNAL,
                                              cv2.CHAIN_APPROX_NONE)
        contours = filter(lambda c: cv2.contourArea(c) > self.minContourArea, contours)
        sorted(contours, key=cv2.contourArea, reverse=True) # Sort by largest contour
        
        # If centering, just find the center of largest contour
        if self.comms.isCentering:
            if len(contours) > 0:
                largestContour = contours[0]
                mu = cv2.moments(largestContour)
                muArea = mu['m00']
                self.comms.centroidToBump = (int(mu['m10']/muArea), int(mu['m01']/muArea))
                self.comms.rectArea = muArea
                
                self.previousCentroid = self.comms.centroidToBump
                self.previousArea = self.comms.rectArea
            else:
                self.comms.centroidToBump = self.previousCentroid
                self.comms.rectArea = self.previousArea
        else:
            # Find hough circles
            circles = cv2.HoughCircles(binImg, cv2.cv.CV_HOUGH_GRADIENT, 1,
                               minDist=30, param1=self.houghParams[0], 
                               param2=self.houghParams[1],
                               minRadius = self.circleParams['minRadius'],
                               maxRadius = self.circleParams['maxRadius'])
            
            # Check if center of circles inside contours
            if contours is not None:
                for contour in contours:
                    mu = cv2.moments(contour)
                    muArea = mu['m00']
                    centroid = (mu['m10']/muArea, mu['m01']/muArea)
                    if circles is not None:
                        for circle in circles[0,:,:]:
                            circleCentroid = (circle[0], circle[1])
                            if abs((Utils.distBetweenPoints(centroid,circleCentroid))) < circle[2]:
                                self.comms.foundBuoy = True
                                # Find new centroid by averaging the centroid and circle centroid
                                newCentroid =(int(centroid[0]+circleCentroid[0])/2,
                                              int(centroid[1]+circleCentroid[1])/2)
                                self.allCentroidList.append(newCentroid)
                                self.allAreaList.append(cv2.contourArea(contour))
                                self.allRadiusList.append(circle[2])
                                # Draw circles
                                cv2.circle(scratchImgCol, newCentroid, circle[2], (255, 255, 0), 2)
                                cv2.circle(scratchImgCol, newCentroid, 2, (255, 0, 255), 3)        
            
            # Find the circle with the largest radius
            if not len(self.allCentroidList) == 0:
                maxIndex = self.allRadiusList.index(max(self.allRadiusList))
                self.comms.centroidToBump = self.allCentroidList[maxIndex]
                self.comms.rectArea = self.allAreaList[maxIndex]
                
                self.previousCentroid = self.comms.centroidToBump
                self.previousArea = self.comms.rectArea

                self.comms.grad = self.getGradient()
            else:
                self.comms.centroidToBump = self.previousCentroid
                self.comms.rectArea = self.previousArea

        cv2.putText(scratchImgCol, "Ang: " + str(self.comms.grad), (30, 100),
                    cv2.FONT_HERSHEY_PLAIN, 1, (204,204,204))

        # Draw new centroid
        cv2.circle(scratchImgCol, self.comms.centroidToBump, 3, (0, 255, 255), 2)
        # rospy.loginfo("Area: {}".format(self.comms.rectArea)) # To put on the scratchImg
        cv2.putText(scratchImgCol, "Area: " + str(self.comms.rectArea), (30, 80),
                    cv2.FONT_HERSHEY_PLAIN, 1, (204, 204, 204))
            
        # How far centroid is off screen center
#.........这里部分代码省略.........

# 需要导入模块: from utils.utils import Utils [as 别名]
# 或者: from utils.utils.Utils import distBetweenPoints [as 别名]
    def gotSonarFrame(self, img):
        img = cv2.resize(img, (vision.screen['width'], vision.screen['height']))

        binImg = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        mask = cv2.threshold(binImg, self.sonarThres, 255, cv2.THRESH_BINARY)[1]

        scratchImgCol = img
        cv2.putText(scratchImgCol, "SONAR PROCESSED", (20,460),  cv2.FONT_HERSHEY_DUPLEX, 1, (211,0,148))

        zerosmask = np.zeros((480,640,3), dtype=np.uint8)
        sobel = cv2.Sobel(mask, cv2.CV_8U, 0, 1, (3, 11))
        # return cv2.cvtColor(sobel, cv2.COLOR_GRAY2BGR)

        contours, hierarchy = cv2.findContours(sobel, cv2.RETR_EXTERNAL,
            cv2.CHAIN_APPROX_SIMPLE)

        allLinesList = []
        allBearingList = []

        for i in contours:
            mask = np.zeros((vision.screen['width'], vision.screen['height']), 
                dtype=np.uint8)
            cv2.drawContours(mask, [i], 0, 255, -1)
            lines = cv2.HoughLinesP(mask, 1, math.pi/2, 1, None, 1, 0)

            if lines is not None:
                line = lines[0]
                x1 = [i[0] for i in line]
                y1 = [i[1] for i in line]
                x2 = [i[2] for i in line]
                y2 = [i[3] for i in line]

                pt1List = [(i[0], i[1]) for i in line]
                pt2List = [(i[2],i[3]) for i in line]
                sorted(pt1List, key=lambda x:x[0])
                sorted(pt2List, key=lambda x:x[0])
                pt1 = pt1List[0] if pt1List[0] < pt2List[0] else pt2List[0]
                pt2 = pt1List[-1] if pt1List[-1] > pt2List[-1] else pt2List[-1]

                length = Utils.distBetweenPoints(pt1, pt2)

                # if 45 < length < 100:
                if 25 < length < 80:
                    angle = math.atan2((pt2[1]-pt1[1]), (pt2[0]-pt1[0]))
                    if -30 < angle < 30:

                        allLinesList.append((pt1, pt2))

                        cv2.line(scratchImgCol, pt1, pt2, (0,0,255), 3)
                        angleStr = "{0:.2f}".format(angle)
                        cv2.putText(scratchImgCol, "Ang " + str(angleStr),
                            (int(pt1[0]), int(pt1[1]-5)), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 0), 1)

                        offset = 60
                        point = (vision.screen['height']-offset)
                        dist = ((point-pt1[1])*1.0/point) * 10.0  # 10m the FOV of sonar
                        distStr = "{0:.2f}".format(dist)
                        cv2.putText(scratchImgCol, "Dist " + str(distStr),
                            (int(pt1[0]), int(pt1[1]-20)), cv2.FONT_HERSHEY_PLAIN, 1, (255,255,0), 1)

                        allBearingList.append((angle, dist))

        if len(allBearingList) > 0:
            sorted(allBearingList, key=lambda p:p[1])
            self.sonarDist = allBearingList[0][1]
            self.sonarBearing = allBearingList[0][0]

        cv2.putText(scratchImgCol, "Sonar Dist " + str(self.sonarDist),
            (30, 30), cv2.FONT_HERSHEY_PLAIN, 1, (0, 0, 255), 1)

        cv2.putText(scratchImgCol, "Sonar Bearing " + str(self.sonarBearing), (30, 60),
                cv2.FONT_HERSHEY_PLAIN, 1, (0, 0, 255), 1)

        return scratchImgCol

# 需要导入模块: from utils.utils import Utils [as 别名]
# 或者: from utils.utils.Utils import distBetweenPoints [as 别名]

#.........这里部分代码省略.........
            binImg2 = binImg.copy()
            binImg2 = cv2.bitwise_not(binImg2, mask=mask)

            erodeEl = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
            binImg2 = cv2.erode(binImg2, erodeEl, iterations=2)

            # return cv2.cvtColor(binImg2, cv2.COLOR_GRAY2BGR)

            circleCont, circleHie = cv2.findContours(binImg2.copy(), cv2.RETR_CCOMP, cv2.CHAIN_APPROX_SIMPLE)
            # circleCont = filter(lambda c: cv2.contourArea(c) > 1000, circleCont)
            circleCont = sorted(circleCont, key=cv2.contourArea, reverse=True)

            circleBinImg = cv2.cvtColor(binImg2, cv2.COLOR_GRAY2BGR)

            all_x = [x[0] for x in boxpoints]
            all_y = [y[0] for y in boxpoints]
            min_xy = (int(min(all_x)), int(min(all_y)))
            max_xy = (int(max(all_x)), int(max(all_y)))

            cv2.circle(circleBinImg, min_xy, 5, (255, 0, 255), 5)
            cv2.circle(circleBinImg, max_xy, 5, (255, 0, 255), 5)

            if len(circleCont) > 0:
                for cont in circleCont:
                    if cv2.contourArea(cont) > 100:
                        (cx, cy), radius = cv2.minEnclosingCircle(cont)

                        # circle radius inside board
                        if min_xy[0] < cx < max_xy[0] and min_xy[1] < cy < max_xy[1]:
                            cv2.circle(circleBinImg, (int(cx), int(cy)), int(radius), (255, 255, 0), 2)
                            cv2.circle(circleBinImg, (int(cx), int(cy)), 1, (255, 0, 255), 2)

                            # Dist to the center
                            dist = Utils.distBetweenPoints((cx, cy), self.aimingCentroid)

                            allCentroidList.append((cx, cy, radius, dist))
        # return circleBinImg

        """
        Detect black circles
        """

        """
        # circleBinImg = self.morphology(cv2.inRange(labImg,
        #     self.thresParams['lo'], self.thresParams['hi']))
        circleBinImg = self.morphology(cv2.inRange(hsvImg,
            self.thresParams['lo'], self.thresParams['hi']))
        kern = cv2.getStructuringElement(cv2.MORPH_RECT, (5,5))
        circleBinImg = cv2.morphologyEx(circleBinImg, cv2.MORPH_OPEN, kern)
        dilateKern = cv2.getStructuringElement(cv2.MORPH_RECT, (5,5))
        circleBinImg = cv2.dilate(circleBinImg, kern)
        # return cv2.cvtColor(circleBinImg, cv2.COLOR_GRAY2BGR)

        circleCont, circleHie = cv2.findContours(circleBinImg.copy(), cv2.RETR_CCOMP, cv2.CHAIN_APPROX_SIMPLE)
        circleCont = filter(lambda c: cv2.contourArea(c) > 1000, circleCont)
        sorted(circleCont, key=cv2.contourArea, reverse=True)

        mask = np.zeros_like(binImg, dtype=np.uint8)
        cv2.fillPoly(mask, [np.int32(largestContour)], 255)
        binImg2 = binImg.copy()
        binImg2 = cv2.bitwise_not(binImg2, mask=mask)

        # Find contours of the binImg2
        binCont, binhie = cv2.findContours(binImg2.copy(), cv2.RETR_CCOMP, cv2.CHAIN_APPROX_SIMPLE)
        circleBinImg = cv2.cvtColor(binImg2, cv2.COLOR_GRAY2BGR)
        if len(binCont) > 0: