Merge branch 'newVisionLoop' into 'dev'

New vision loop

See merge request oriagranat9/Chameleon-Vision!1

backend/Main.py

@@ -1,11 +1,5 @@
-from multiprocessing import Queue
-from multiprocessing.managers import BaseManager
-
 import tornado.ioloop
-import multiprocessing
 import logging
-from cscore import CameraServer
-
 from app.ChameleonVisionApp import ChameleonApplication
 from app.classes.SettingsManager import SettingsManager
 from tornado.options import options

backend/app/classes/SettingsManager.py

@@ -26,7 +26,10 @@ class SettingsManager(metaclass=Singleton):
         "area": [0, 100],
         "ratio": [0, 20],
         "extent": [0, 100],
-        "is_binary": "Normal"
+        "is_binary": "Normal",
+        "sort_mode": "Largest",
+        "target_group": 'Single',
+        "target_intersection": 'Up'
     }
     default_general_settings = {
         "team_number": 1577,

backend/app/handlers/VisionHandler.py

@@ -5,12 +5,11 @@ import numpy
 from cscore import CameraServer
 from app.classes.SettingsManager import SettingsManager
 from ..classes.Singleton import Singleton
-import time
 from multiprocessing import Process
 import threading
 import zmq
-import base64
-
+import math
+from enum import Enum, unique
 
 
 class VisionHandler(metaclass=Singleton):
@@ -19,8 +18,6 @@ class VisionHandler(metaclass=Singleton):
 
     def _hsv_threshold(self, hue: list, saturation: list, value: list, img: numpy.ndarray, is_erode: bool,
                        is_dilate: bool):
-        # img = cv2.medianBlur(img, 1)
-        # not sure if we need noise reduction now with erode it hurts the precision if val is to high
 
         img = cv2.erode(img, kernel=self.kernel, iterations=is_erode)
         img = cv2.dilate(img, kernel=self.kernel, iterations=is_dilate)
@@ -28,54 +25,218 @@ class VisionHandler(metaclass=Singleton):
         return cv2.inRange(out, (hue[0], saturation[0], value[0]), (hue[1], saturation[1], value[1]))
 
     def find_contours(self, binary_img: numpy.ndarray):
-        _, contours, _ = cv2.findContours(binary_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+        _, contours, _ = cv2.findContours(binary_img, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
         return contours
 
-    def filter_contours(self, input_contours, camera_area, area, ratio, extent):
-        output = []
-        rectangle = []
+    class Filter_Contours:
+        def __init__(self,center_x, center_y):
+            self.sort_mode = self.SortMode(center_x=center_x, center_y=center_y)
+            self.center_y = center_y
+            self.center_x = center_x
 
-        for contour in input_contours:
+        class SortMode:
+            def __init__(self, center_x, center_y):
+                self.center_x = center_x
+                self.center_y = center_y
 
-            rect = cv2.minAreaRect(contour)
-            # center_point = rect[0]
-            contour_area = cv2.contourArea(contour)
-            rect_area = rect[1][0] * rect[1][1]
+            @classmethod
+            def moment_x(cls,contour):
+                M = cv2.moments(contour)
+                try:
+                    x = float(M['m10'] / M['m00'])
+                except ZeroDivisionError:
+                    x = 0
+                return x
 
-            try:
-                extent_percent = float(contour_area) / rect_area
-                ratio_percent = float(rect[1][0]) / rect[1][1]
-                area_percent = rect_area / camera_area
-            except:
-                continue
+            @classmethod
+            def moment_y(cls, contour):
+                M = cv2.moments(contour)
+                try:
+                    y = float(M['m01'] / M['m00'])
+                except ZeroDivisionError:
+                    y = 0
+                return y
 
-            if area_percent < area[0] or area_percent > area[1]:
-                continue
-            if ratio_percent < ratio[0] or ratio_percent > ratio[1]:
-                continue
-            if extent_percent < extent[0] or extent_percent > extent[1]:
-                continue
+            @classmethod
+            def calc_distance(cls,contour, center_x, center_y):
+                M = cv2.moments(contour)
+                try:
+                    x = int(M['m10'] / M['m00'])
+                except ZeroDivisionError:
+                    x = 0
+                try:
+                    y = int(M['m01'] / M['m00'])
+                except ZeroDivisionError:
+                    y = 0
+                # this function was suggested by my girlfriend maya jugend that i really love
+                return math.sqrt((center_x-x)**2 + (center_y-y)**2)
 
-            output.append(contour)
-            rectangle.append(rect)
+            def Largest(self, input_contours):
+                return sorted(input_contours, key=lambda x: cv2.contourArea(x), reverse=True)
 
-        return [output, rectangle]
+            def Smallest(self, input_contours):
+                return sorted(input_contours, key=lambda x: cv2.contourArea(x))
 
-    def draw_image(self, input_image: numpy.ndarray, is_binary: bool, rectangles):
-        if is_binary:
+            def Highest(self, input_contours):
+                return sorted(input_contours, key=lambda x: self.moment_y(x))
+
+            def Lowest(self, input_contours):
+                return sorted(input_contours, key=lambda x: self.moment_y(x),reverse=True)
+
+            def Rightmost(self, input_contours):
+                return sorted(input_contours, key=lambda x: self.moment_x(x), reverse=True)
+
+            def Leftmost(self, input_contours):
+                return sorted(input_contours, key=lambda x: self.moment_x(x))
+
+            def Closest(self, input_contours):
+                return sorted(input_contours, key=lambda x: self.calc_distance(x, center_x=self.center_x,
+                                                                               center_y=self.center_y), reverse=True)
+
+        def filter_contours(self, input_contours, cam_area, area, ratio, extent, sort_mode, target_grouping,
+                            target_intersection):
+            class TargetGroup(Enum):
+                Single = 1
+                Dual = 2
+                Triple = 3
+                Quadruple = 4
+                Quintuple = 6
+
+            def group_target(i_contours, target_group, intersection_point):
+
+                def is_intersecting(contour_a, contour_b, intersection_direction):
+
+                    [vx_a, vy_a, x0_a, y0_a] = cv2.fitLine(contour_a, cv2.DIST_L2, 0, 0.01, 0.01)
+                    [vx_b, vy_b, x0_b, y0_b] = cv2.fitLine(contour_b, cv2.DIST_L2, 0, 0.01, 0.01)
+                    # getting line data of both contours
+                    m_a = vy_a / vx_a
+                    m_b = vy_b / vx_b
+                    # calculating slope of both lines
+                    try:
+                        intersection_x = ((m_a * x0_a) - y0_a - (m_b * x0_b) + y0_b) / (m_a - m_b)
+                    except ZeroDivisionError:
+                        if intersection_direction == 'Parallel':
+                            return True
+                        else:
+                            return False
+                    intersection_y = (m_a * (intersection_x - x0_a)) + y0_a
+                    # finding intersection point
+                    if intersection_direction == 'Up':
+                        if intersection_y > self.center_y:
+                            return True
+                    elif intersection_direction == 'Down':
+                        if intersection_y > self.center_y:
+                            return False
+                    elif intersection_direction == 'Left':
+                        if intersection_x < self.center_x:
+                            return True
+                    elif intersection_direction == 'Right':
+                        if intersection_x > self.center_x:
+                            return True
+                    else:
+                        return False
+                if target_group != TargetGroup.Single:
+                    f_contour_list = []
+                    for index, g_contour in enumerate(i_contours):
+                        final_contour = g_contour
+                        for c in range(target_group.value):
+                            try:
+                                first_contour = i_contours[index + c]
+                                second_contour = i_contours[index + c + 1]
+                            except IndexError:
+                                continue
+                            if is_intersecting(first_contour, second_contour, intersection_point):
+                                final_contour = numpy.concatenate((final_contour, second_contour))
+                            else:
+                                continue
+
+                        f_contour_list.append(final_contour)
+
+                    return f_contour_list
+                else:
+                    return i_contours
+
+            '''start of the first filtration of contours'''
+            filtered_contours = []
+            for contour in input_contours:
+                try:
+                    contour_area = cv2.contourArea(contour)
+                    target_area = float(contour_area / cam_area)*100
+
+                    if target_area >= area[1] or target_area <= area[0]:
+                        continue
+
+                    rect = cv2.minAreaRect(contour)
+                    bounding_rect_area = rect[1][0] * rect[1][1]
+                    try:
+                        target_fullness = float(contour_area / bounding_rect_area)*100
+                    except ZeroDivisionError:
+                        target_fullness = 0
+
+                    if target_fullness <= extent[0] or target_fullness >= extent[1]:
+                        continue
+                    try:
+                        aspect_ratio = float(rect[1][0]/rect[1][1])
+                    except ZeroDivisionError:
+                        aspect_ratio = 0
+                    if aspect_ratio <= ratio[0] or aspect_ratio >= ratio[1]:
+                        continue
+
+                    filtered_contours.append(contour)
+                except Exception as e:
+                    print(e)
+                    continue
+            #checking for contour grouping before sorting
+            grouped_contours = group_target(filtered_contours, TargetGroup[target_grouping], target_intersection)
+
+            sorted_contours = getattr(self.sort_mode, sort_mode)(grouped_contours)
+
+            return sorted_contours
+
+    @unique
+    class Region(Enum):
+        UP_MOST = 0
+        RIGHT_MOST = 1
+        DOWN_MOST = 2
+        LEFT_MOST = 3
+        CENTER_MOST = 4
+        
+    def output_contour(self, sorted_contours):
+        if len(sorted_contours) > 0:
+            selected_contour = sorted_contours[0]
+            rect = cv2.minAreaRect(selected_contour)
+        else:
+            return []
+
+        # crosshair_calibration function to "put" camera in the middle
+        return rect
+            
+    def draw_image(self, input_image, contour):
+        if len(input_image.shape)<3:
             input_image = cv2.cvtColor(input_image, cv2.COLOR_GRAY2RGB)
-        for rectangle in rectangles[1]:
-            box = cv2.boxPoints(rectangle)
+        if contour != []:
+            box = cv2.boxPoints(contour)
             box = numpy.int0(box)
-            cv2.drawContours(input_image, [box], 0, (0, 0, 255), 2)
-            center_point = (int(rectangle[0][0]), int(rectangle[0][1]))
-            cv2.circle(input_image, center_point, 0, (0, 255, 0), thickness=3, lineType=8, shift=0)
+            cv2.drawContours(input_image, [box], 0, (0, 0, 255), 3)
+
+        # center_point = (int(rectangle[0][0]), int(rectangle[0][1]))
+        # cv2.circle(input_image, center_point, 0, (0, 255, 0), thickness=3, lineType=8, shift=0)
         return input_image
 
+    def calculate_pitch(self, pixel_y, center_y, v_focal_length):
+        pitch = math.degrees(math.atan((pixel_y - center_y) / v_focal_length))
+        # Just stopped working have to do this:
+        pitch *= -1
+        return pitch
+
+    def calculate_yaw(self, pixel_x, center_x, h_focal_length):
+        yaw = math.degrees(math.atan((pixel_x - center_x) / h_focal_length))
+        return yaw
+
     def run(self):
         # NetworkTables.startClientTeam(team=SettingsManager.general_settings.get("team_number", 1577))
         NetworkTables.initialize("localhost")
-        # NetworkTables.initialize()
+
         cs = CameraServer.getInstance()
         port = 5550
 
@@ -84,6 +245,29 @@ class VisionHandler(metaclass=Singleton):
             port += 1
 
     def thread_proc(self, cs, cam_name, port=5557):
+        pipeline = SettingsManager().cams[cam_name]["pipelines"]["pipeline0"]
+
+        def change_camera_values(pipline):
+            SettingsManager.usb_cameras[cam_name].setBrightness(pipeline['brightness'])
+            SettingsManager.usb_cameras[cam_name].setExposureManual(pipeline['exposure'])
+            SettingsManager.usb_cameras[cam_name].setWhiteBalanceAuto()
+
+        def pipeline_listener(table, key, value, is_new):
+            global pipeline
+            if is_new:
+                pipeline = SettingsManager.cams[cam_name]["pipelines"][value]
+                change_camera_values()
+
+        def mode_listener(table, key, value, is_new):
+            pass
+
+        table = NetworkTables.getTable("/Chameleon-Vision/" + cam_name)
+
+        table.addEntryListenerEx(pipeline_listener, key="Pipeline",
+                                 flags=networktables.NetworkTablesInstance.NotifyFlags.UPDATE)
+        table.addEntryListenerEx(mode_listener, key="Driver_Mode",
+                                 flags=networktables.NetworkTablesInstance.NotifyFlags.UPDATE)
+
         cv_sink = cs.getVideo(camera=SettingsManager.usb_cameras[cam_name])
 
         width = SettingsManager().cams[cam_name]["video_mode"]["width"]
@@ -100,48 +284,46 @@ class VisionHandler(metaclass=Singleton):
         p = Process(target=self.camera_process, args=(cam_name, port))
         p.start()
 
-        pipeline = SettingsManager().cams[cam_name]["pipelines"]["pipeline0"]
+        change_camera_values(pipeline)
         while True:
-            # start = time.time(
             _, image = cv_sink.grabFrame(image)
             socket.send_json(dict(
                 pipeline=pipeline
             ))
             socket.send_pyobj(image)
             p_image = socket.recv_pyobj()
+            nt_data = socket.recv_json()
+            if nt_data['valid']:
+                table.putNumber('pitch', nt_data['pitch'])
+                table.putNumber('yaw', nt_data['yaw'])
+            table.putBoolean('valid', nt_data['valid'])
             cv_publish.putFrame(p_image)
-            # print(cam_name + "  " + str(1 / (end - start)))
 
     def camera_process(self, cam_name, port):
+        from fractions import Fraction
 
-        # def change_camera_values():
-        #     camera.setBrightness(0)
-        #     camera.setExposureManual(0)
-        #
-        # def pipeline_listener(table, key, value, is_new):
-        #     if (is_new):
-        #         curr_pipline = SettingsManager.cams[cam_name]["pipelines"][value]
-        #         change_camera_values()
-        #
-        # def mode_listener(table, key, value, is_new):
-        #     pass
-        #
-        # table = NetworkTables.getTable("/Chameleon-Vision/" + camera.getInfo().name)
-        #
-        # table.addEntryListenerEx(pipeline_listener, key="Pipeline",
-        #                          flags=networktables.NetworkTablesInstance.NotifyFlags.UPDATE)
-        # table.addEntryListenerEx(mode_listener, key="Driver_Mode",
-        #                          flags=networktables.NetworkTablesInstance.NotifyFlags.UPDATE)
-        # change_camera_values()
+        diagonalView = math.radians(68.5) #needs to be implemented in client 
 
         width = SettingsManager().cams[cam_name]["video_mode"]["width"]
         height = SettingsManager().cams[cam_name]["video_mode"]["height"]
+        centerX = (width / 2) - .5
+        centerY = (height / 2) - .5
         cam_area = width * height
+        
+        aspect_fraction = Fraction(width,height)
+        horizontal_ratio = aspect_fraction.numerator
+        vertical_ratio = aspect_fraction.denominator
+        
+        horizontalView = math.atan(math.tan(diagonalView/2) * (horizontal_ratio / diagonalView)) * 2
+        verticalView = math.atan(math.tan(diagonalView/2) * (vertical_ratio / diagonalView)) * 2
+        
+        H_FOCAL_LENGTH = width / (2*math.tan((horizontalView/2)))
+        V_FOCAL_LENGTH = height / (2*math.tan((verticalView/2)))
 
         context = zmq.Context()
         socket = context.socket(zmq.PAIR)
         socket.connect('tcp://localhost:%s' % str(port))
-
+        filter_contours = self.Filter_Contours(center_x=centerX, center_y=centerY)
         while True:
             obj = socket.recv_json()
             image = socket.recv_pyobj()
@@ -151,10 +333,31 @@ class VisionHandler(metaclass=Singleton):
                                             image, curr_pipeline["erode"], curr_pipeline["dilate"])
             # if table.getBoolean("Driver_Mode", False):
             contours = self.find_contours(hsv_image)
-            filtered_contours = self.filter_contours(contours, cam_area, curr_pipeline["area"], curr_pipeline["ratio"],
-                                                     curr_pipeline["extent"])
-            res = self.draw_image(input_image=image, is_binary=False, rectangles=filtered_contours)
-            # cv2.putText(res, str(fps), (10, 200), font, 4, (0, 0, 0), 2, cv2.LINE_AA)
+            filtered_contours = filter_contours.filter_contours(input_contours=contours, area=curr_pipeline['area'],
+                                                                ratio=curr_pipeline['ratio'],
+                                                                extent=curr_pipeline['extent'],
+                                                                sort_mode=curr_pipeline['sort_mode'], cam_area=cam_area,
+                                                                target_grouping=curr_pipeline['target_group'],
+                                                                target_intersection=
+                                                                curr_pipeline['target_intersection'])
+            final_contour = self.output_contour(filtered_contours)
+            try:
+                center = final_contour[0]
+                pitch = self.calculate_pitch(pixel_y=center[1], center_y=centerY, v_focal_length=V_FOCAL_LENGTH)
+                yaw = self.calculate_yaw(pixel_x=center[0], center_x=centerX, h_focal_length=H_FOCAL_LENGTH)
+                valid = True
+            except IndexError:
+                pitch = None
+                yaw = None
+                valid = False
+
+            res = self.draw_image(input_image=image, contour=final_contour)
             socket.send_pyobj(res)
+            socket.send_json(dict(
+                pitch=pitch,
+                yaw=yaw,
+                valid= valid
+
+            ))
 
 

backend/settings/cams/USB Camera-B4.09.24.1.json

@@ -1 +1 @@
-{"pipelines": {"pipeline0": {"exposure": 25, "brightness": 19, "orientation": "Normal", "resolution": 1, "hue": [0, 10], "saturation": [58, 69], "value": [61, 87], "erode": false, "dilate": false, "area": [0, 100], "ratio": [0, 20], "extent": [0, 100], "is_binary": "Normal"}}, "path": "/dev/v4l/by-path/pci-0000:02:03.0-usb-0:1:1.0-video-index0", "video_mode": {"fps": 150, "width": 320, "height": 240, "pixel_format": "kYUYV"}}
+{"pipelines": {"pipeline0": {"exposure": 50, "brightness": 11, "orientation": "Normal", "resolution": [320, 160], "hue": [0, 100], "saturation": [0, 100], "value": [0, 100], "erode": false, "dilate": false, "area": [20, 34], "ratio": [0, 22.9], "extent": [13, 71], "is_binary": "Normal", "sort_mode": "Largest", "target_group": "Single", "target_intersection": "Up"}}, "path": "/dev/v4l/by-path/pci-0000:02:03.0-usb-0:1:1.0-video-index0", "video_mode": {"fps": 187, "width": 320, "height": 240, "pixel_format": "kYUYV"}}

chameleon-client/src/App.vue

@@ -13,6 +13,7 @@
                           <MenuItem name="/vision/input" to="/vision/input">Input</MenuItem>
                           <MenuItem name="/vision/threshold" to="/vision/threshold">Threshold</MenuItem>
                           <MenuItem name="/vision/contours" to="/vision/contours">Contours</MenuItem>
+                          <MenuItem name="/vision/output" to="/vision/output">Output</MenuItem>
                         </Submenu>
                         <Submenu name="/settings">
                           <template slot="title">

chameleon-client/src/components/ch-range.vue

@@ -5,13 +5,13 @@
                 <h4>{{title.charAt(0).toUpperCase() + title.slice(1)}} :</h4>
             </Col>
             <Col span="4" style="text-align: left">
-                <InputNumber style="align-self: flex-start;" v-model="value[0]" size="small"></InputNumber>
+                <InputNumber style="align-self: flex-start;" v-model="value[0]" size="small" :step="steps" ></InputNumber>
             </Col>
             <Col span="10">
-                <Slider range v-model="value" @on-input="handleInput"></Slider>
+                <Slider range v-model="value" @on-input="handleInput" :step="steps"></Slider>
             </Col>
             <Col span="4" style="text-align: right">
-                <InputNumber style="align-self: flex-end;" v-model="value[1]" size="small"></InputNumber>
+                <InputNumber style="align-self: flex-end;" v-model="value[1]" size="small"  :step="steps"></InputNumber>
             </Col>
         </row>
 </template>
@@ -21,7 +21,8 @@
         name: 'ch-range',
         props:{
             title:String,
-            Xkey:String
+            Xkey:String,
+            steps:Number
         },
         data() {
             return {

chameleon-client/src/components/contourTab.vue

@@ -1,8 +1,14 @@
 <template>
     <div id="ContourTab">
+     <chselect class="spacing" title="Sort Mode" Xkey="sort_mode" 
+        :list="['Largest','Smallest','Highest','Lowest','Rightmost','Leftmost','Closest']"></chselect>
      <chrange class="spacing" title="Area" Xkey="area"></chrange>
-     <chrange class="spacing" title="Ratio (W/H)" Xkey="ratio"></chrange>
+     <chrange class="spacing" title="Ratio (W/H)" Xkey="ratio" :steps="0.1"></chrange>
      <chrange class="spacing" title="Extent" Xkey="extent"></chrange>
+     <chselect class="spacing" title="Target Group" Xkey="target_group" 
+        :list="['Single','Dual','Triple','Quadruple','Quintuple']"></chselect>
+    <chselect class="spacing" title="Target Intersaction" Xkey="target_intersection" 
+        :list="['Up','Down','Left','Right','Parallel']"></chselect>
      </div>
 </template>
 

chameleon-client/src/components/outputTab.vue

@@ -0,0 +1,37 @@
+<template>
+    <div id="OutputTab">
+     <chselect class="spacing" title="Target region" Xkey="target_region" 
+        :list="['Upmost','Rightmost','Downmost','Leftmost','Centermost']"></chselect>
+        
+     </div>
+</template>
+
+<script>
+import chslider from './ch-slider.vue'
+import chselect from './ch-select.vue'
+import chrange from './ch-range.vue'
+
+    export default {
+        name: 'OutputTab',
+        components:{
+            chslider,
+            chselect,
+            chrange
+        },
+        methods:{
+
+        },
+        data() {
+            return {
+                
+            }
+        }
+    }
+</script>
+
+<style scoped>
+
+.spacing{
+    margin-top: 20px;
+}
+</style>

chameleon-client/src/routes.js

@@ -6,13 +6,15 @@ import Threshold from "./components/ThresholdTab.vue";
 import System from "./components/SystemTab.vue";
 import Camera from "./components/CameraTab.vue";
 import Contours from "./components/contourTab.vue";
+import Output from './components/outputTab.vue'
 
 const routes = [
   { path: '/', redirect: '/vision/input'},
   { path: '/vision', component: Vision, children: [
     { path: 'input', component: Input },
     { path: 'threshold', component: Threshold },
-    { path: 'contours', component: Contours }
+    { path: 'contours', component: Contours },
+    { path: 'output', component: Output },
   ]},
   { path: '/settings', component: Setting, children: [
     { path: 'system', component: System },

chameleon-client/src/store.js

@@ -27,8 +27,11 @@ export const store = new Vuex.Store({
         dilate: false,
         //contours
         area:[0,100],
-        ratio:[0,1],
+        ratio:[0,20],
         extent:[0,100],
+        sort_mode:'Largest', //
+        target_group:'Single', //
+        target_intersection:'Up', //
         //Settings
         teamValue:0,
         connectionType:"DHCP",
@@ -67,7 +70,10 @@ export const store = new Vuex.Store({
         streamAdress : set('streamAdress'),
         isBinaryImage: set('isBinaryImage'),
         cameraList : set('cameraList'),
-        pipelineList: set('piplineList')
+        pipelineList: set('piplineList'),
+        sort_mode: set('sort_mode'),
+        target_group:set('target_group'),
+        target_intersection:set('target_intersection')
     },
     getters:{
         camera: state => state.camera,
@@ -92,7 +98,11 @@ export const store = new Vuex.Store({
         streamAdress: state => state.streamAdress,
         isBinaryImage: state => state.isBinaryImage,
         cameraList: state => state.cameraList,
-        pipelineList: state => state.pipelineList
+        pipelineList: state => state.pipelineList,
+        sort_mode: state => state.sort_mode,
+        target_group: state => state.target_group,
+        target_intersection: state => state.target_intersection
+
 
     },
 });