Final project

.gitignore

@@ -28,7 +28,6 @@ __pycache__
 .#*
 .coverage
 .pytest_cache
-src
 
 *.swp
 *.map

final_project/calibration/calib.json

@@ -0,0 +1,31 @@
+{
+    "camera_matrix": [
+        [
+            587.467217162,
+            0,
+            746.834662176
+        ],
+        [
+            0,
+            583.279916347,
+            560.059131772
+        ],
+        [
+            0,
+            0,
+            1
+        ]
+    ],
+    "k_coef": [
+        0.172686496,
+        -0.092024968,
+        -0.002735723,
+        0.494416241,
+        -0.10974415,
+        -0.017876025
+    ],
+    "p_coef": [
+        0.000299857,
+        -0.000261183
+    ]
+}

final_project/main.py

@@ -0,0 +1,6 @@
+from server import Server
+
+SEQ_NUMBER = 1 # 1, 2 or 3
+
+server = Server(SEQ_NUMBER)
+server.run()

final_project/requirements.txt

@@ -0,0 +1,2 @@
+numpy
+opencv-python

final_project/server.py

@@ -0,0 +1,52 @@
+import sys
+import time
+import math
+import json
+import os
+
+from src.vision import Vision, Sensor
+from src.localization import Localization
+from src.model import Model
+
+
+class Server:
+    def __init__(self, sequence_number):
+        with open("calibration/calib.json") as f:
+            calibration = json.load(f)
+        self.camera = Sensor()
+        self.vision = Vision()
+        self.model = Model()
+        robot_height = 0.67
+        # setting model parameters
+        self.model.set_parameters(calibration["camera_matrix"],
+                                  robot_height,
+                                  calibration["k_coef"],
+                                  calibration["p_coef"])
+        self.localization = Localization()
+        with open(f"cvr_logs/seq_{sequence_number}.json") as f:
+            self.logs_data = json.load(f)
+
+    def run(self):
+        for sample in self.logs_data:
+            self_data = {}
+            self.model.update_camera_pan_tilt(
+                float(sample['head_yaw']),
+                float(sample['head_pitch']))
+
+            # Taking picture.
+            img = self.camera.get_frame(os.path.join("cvr_logs", sample['img_path']))
+            # Processing image
+            camera_data = self.vision.get(img)
+            for observation_type in camera_data:
+                # self means in robots coords
+                if observation_type not in self_data.keys():
+                    self_data[observation_type] = []
+                self_points = []
+                for observation in camera_data[observation_type]:
+                    cx, cy, w, h = observation
+                    self_points.append(self.model.pic2r(cx, cy + (w + h) / 4))
+                self_data[observation_type] += self_points
+
+            self.localization.update(self_data)
+            print(f'ETO ball: {self.localization.get_ball_position()}, \
+                    ETO robot: {self.localization.get_robot_position()}')

final_project/src/localization/__init__.py

@@ -0,0 +1 @@
+from .localization import Localization

final_project/src/localization/localization.py

@@ -0,0 +1,17 @@
+
+
+class Localization:
+    def __init__(self):
+        self.robot_position = None
+        self.ball_pos_self = None
+        self.ball_pos_world = None
+
+
+    def update(self, data):
+        pass 
+
+    def get_robot_position(self):
+        return self.robot_position
+
+    def get_ball_position(self):
+        return self.ball_pos_world

final_project/src/model/__init__.py

@@ -0,0 +1 @@
+from .model import Model

final_project/src/model/model.py

@@ -0,0 +1,202 @@
+import math
+
+
+class Model:
+    def __init__(self):
+        self.camera_pan = 0
+        self.camera_tilt = 0
+        self.active_servos = {}
+
+    def update_camera_pan_tilt(self, camera_pan, camera_tilt):
+        self.camera_pan = camera_pan
+        self.camera_tilt = camera_tilt
+
+    def set_parameters(self, cam_matrix, h, k_coefs, p_coefs):
+        # cam_matrix - camera matrix
+        # h - camera height
+        # k_coefs, p_coefs - coefficients of lense distortion
+        self.cam_matrix = cam_matrix
+        self.h = h
+        self.k_coefs = k_coefs
+        self.p_coefs = p_coefs
+        self.head_to_base_matrix = [[1., 0., 0.],
+                                    [0., 1., 0.],
+                                    [0., 0., 1.]]
+
+    # function calculating dot product of matrix and vector
+    def matrix_dot_vec(self, matr, vect):
+        n = len(vect)
+        res_vect = []
+        for i in range(n):
+            res_vect.append(0.0)
+            for j in range(n):
+                res_vect[i] += matr[i][j] * vect[j]
+
+        return res_vect
+
+    # transformation from robot coords to camera coords
+    def r2cam(self, xb, yb):
+        r_coords = [xb, yb, -self.h]
+        a = self.camera_pan
+        b = self.camera_tilt
+
+        r2cam_rot_matrix = [[math.cos(a) * math.cos(b),
+                             math.sin(a) * math.cos(b),
+                             math.sin(b)],
+                            [-math.sin(a),
+                             math.cos(a),
+                             0.0],
+                            [-math.sin(b) * math.cos(a),
+                             -math.sin(a) * math.sin(b),
+                             math.cos(b)]]
+
+        return self.matrix_dot_vec(r2cam_rot_matrix, r_coords)
+
+    # transformation from camera coords to robot coords
+    def cam2r(self, cam_coords):
+        a = self.camera_pan
+        b = self.camera_tilt
+
+        cam2r_rot_matrix = [[math.cos(a) * math.cos(b),
+                             -math.sin(a),
+                             -math.cos(a) * math.sin(b)],
+                            [math.sin(a) * math.cos(b),
+                             math.cos(a),
+                             -math.sin(a) * math.sin(b)],
+                            [math.sin(b),
+                             0.0,
+                             math.cos(b)]]
+
+        return self.matrix_dot_vec(cam2r_rot_matrix, cam_coords)
+
+
+    def distortion_straight(self, x, y):
+        r_sq = x ** 2 + y ** 2
+        coef_numerator = (1 +
+                          self.k_coefs[0] * r_sq +
+                          self.k_coefs[1] * (r_sq ** 2) +
+                          self.k_coefs[2] * (r_sq ** 3))
+
+        coef_denominator = (1 +
+                            self.k_coefs[3] * r_sq +
+                            self.k_coefs[4] * (r_sq ** 2) +
+                            self.k_coefs[5] * (r_sq ** 3))
+
+        g = coef_numerator / coef_denominator
+
+        # x_cor, y_cor - ball cords in artificially created coord system
+        x_cor_parts = [x * g,
+                       2 * self.p_coefs[0] * x * y,
+                       self.p_coefs[1] * (r_sq + 2 * x ** 2)]
+        x_cor = sum(x_cor_parts)
+
+        y_cor_parts = [y * g,
+                       2 * self.p_coefs[1] * x * y,
+                       self.p_coefs[0] * (r_sq + 2 * y ** 2)]
+        y_cor = sum(y_cor_parts)
+
+        return [x_cor, y_cor]
+
+    def dist_error(self, x, y, c1, c2):
+        r1, r2 = self.distortion_straight(x, y)
+        return [r1 - c1, r2 - c2]
+
+    def dist_er_jac_inv(self, x, y):
+        r_sq = x ** 2 + y ** 2
+        coef_numerator = (1 +
+                          self.k_coefs[0] * r_sq +
+                          self.k_coefs[1] * (r_sq ** 2) +
+                          self.k_coefs[2] * (r_sq ** 3))
+
+        coef_denominator = (1 +
+                            self.k_coefs[3] * r_sq +
+                            self.k_coefs[4] * (r_sq ** 2) +
+                            self.k_coefs[5] * (r_sq ** 3))
+
+        g = coef_numerator / coef_denominator
+        gdx = ((2 * self.k_coefs[0] * x + 
+            4 * self.k_coefs[1] * x * r_sq +
+            6 * self.k_coefs[2] * x * r_sq**2) / 
+            coef_denominator - 
+            (coef_numerator * 
+            (2 * self.k_coefs[3] * x + 
+                4 * self.k_coefs[4] * x * r_sq +
+                6 * self.k_coefs[5] * x * r_sq**2)) /
+            coef_denominator**2)
+
+        gdy = ((2 * self.k_coefs[0] * y + 
+            4 * self.k_coefs[1] * y * r_sq +
+            6 * self.k_coefs[2] * y * r_sq**2) / 
+            coef_denominator - 
+            (coef_numerator * 
+            (2 * self.k_coefs[3] * y + 
+                4 * self.k_coefs[4] * y * r_sq +
+                6 * self.k_coefs[5] * y * r_sq**2)) /
+            coef_denominator**2)
+
+        a11 = g + x * gdx + 2 * self.p_coefs[0] * y + 6 * self.p_coefs[1] * x
+        a12 = y * gdx + 2 * self.p_coefs[0] * x + 2 * self.p_coefs[1] * y
+        a21 = x * gdy + 2 * self.p_coefs[0] * x + 2 * self.p_coefs[1] * y
+        a22 = g + y * gdy + 2 * self.p_coefs[1] * x + 6 * self.p_coefs[0] * y
+
+        det = a11 * a22 - a12 * a21
+        return [[a22 / det, -a12 / det], [-a21 / det, a11 / det]]
+
+
+    def distortion_inverse(self, c1, c2):
+        x = c1
+        y = c2
+        for i in range(100):
+            x_new = (x - 
+                    (self.dist_er_jac_inv(x, y)[0][0] * self.dist_error(x, y, c1, c2)[0] +
+                     self.dist_er_jac_inv(x, y)[0][1] * self.dist_error(x, y, c1, c2)[1]))
+            y_new = (y -
+                    (self.dist_er_jac_inv(x, y)[1][0] * self.dist_error(x, y, c1, c2)[0] +
+                     self.dist_er_jac_inv(x, y)[1][1] * self.dist_error(x, y, c1, c2)[1]))
+
+            if abs(x_new - x) + abs(y_new - y) < 0.00001:
+                break
+            else:
+                x = x_new
+                y = y_new
+        return [x, y]
+
+
+    def r2pic(self, xb, yb):
+        # xb, yb - coords of the ball in the robot system
+
+        # transformation to camera coords and back to apply head-to-base matrix
+        cam_coords = self.r2cam(xb, yb)
+        x_c, y_c, z_c = self.matrix_dot_vec(self.head_to_base_matrix,
+                                               cam_coords)
+        x = y_c / x_c
+        y = z_c / x_c
+
+        # applying the lense distortion-fix formula
+        x_cor, y_cor = self.distortion_straight(x, y)
+
+        # u, v - pixel coords of the ball
+        u = -x_cor * self.cam_matrix[0][0] + self.cam_matrix[0][2]
+        v = -y_cor * self.cam_matrix[1][1] + self.cam_matrix[1][2]
+        return (int(u), int(v))
+
+
+    def pic2r(self, u, v):
+        # u,v - pixel coords of the ball in the screen system
+
+        # x,y - coords of the ball in the coordinate system,
+        # parallel to the camera screen
+        x = -(float(u) - self.cam_matrix[0][2]) / self.cam_matrix[0][0]
+        y = -(float(v) - self.cam_matrix[1][2]) / self.cam_matrix[1][1]
+
+        # transformation of x and y using the distortion coefficients
+        x, y = self.distortion_inverse(x, y)
+
+        x_cam = -self.h / (math.sin(self.camera_tilt) + y * math.cos(self.camera_tilt))
+        x_cam = abs(x_cam)
+        y_cam = x_cam * x
+        z_cam = x_cam * y
+
+        x_r, y_r, _ = self.cam2r([x_cam, y_cam, z_cam])
+
+        return (x_r, y_r)

final_project/src/vision/README.md

@@ -0,0 +1,20 @@
+# Vision    
+
+All the detection relies on the usage of the Blob analysis from the OpenMV SDK
+
+Classes:
+
+Detectors:
+    -Detector - basic visualization, placeholders for common methods
+
+    -ColoredObjectDetector [inherited from Detector] - blob analysis,
+        particular filtration methods
+
+    -SurroundedObjectDetector [inherited from ColoredObjectDetector] - 
+        takes surrounding pixels of the object into account to check if
+        the detected object has proper background
+
+Containers/other:
+    -Vision - container for the detectors
+
+    -VisionPostprocessing - mutual spatial relations of the detected objects

final_project/src/vision/__init__.py

@@ -0,0 +1,2 @@
+from .vision import Vision
+from .cv_sensor import Sensor, Image

final_project/src/vision/cv_sensor.py

@@ -0,0 +1,34 @@
+# Bug that sometimes appears on several computers
+try:
+    import cv2
+except ImportError:
+    import cv2.cv2 as cv2
+
+import math
+import numpy as np
+
+
+class Image:
+    def __init__(self, img_):
+        self.img = img_
+
+    def draw_line(self, line):
+        x1, y1, x2, y2 = line
+        cv2.line(self.img, (x1, y1), (x2, y2), (0, 0, 255), 2)
+
+    def draw_rectangle(self, rect):
+        x, y, w, h = rect
+        cv2.rectangle(self.img, (x, y), (x + w, y + h), (255, 0, 0), 3)
+
+
+class Sensor:
+    def __init__(self):
+        self.img = None
+
+    def snapshot(self):
+        if self.img is not None:
+            return Image(self.img.copy())
+
+    def get_frame(self, filename):
+        self.img = cv2.imread(filename)
+        return Image(self.img.copy())