init

.gitignore

+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+pip-wheel-metadata/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+.python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/

.vscode/settings.json

+{
+    "git.ignoreLimitWarning": true
+}
\ No newline at end of file

README.md

+# Final Project of 31392 - Perception of Autonomous Systems
+## Project Goals
+- Calibrate and rectify the stereo input.
+- Process the input images to detect objects on the conveyor and track them in 3D, even under occlusion.
+- Train a machine learning system that can classify unseen images into 3 classes (cups, books and boxes) based either on 2D or 3D data.
+
+## ToDo
+- [x] 4/9/2020 Calibrate and Rectify the stereo input
+- [x] 4/23/2020 Object Tracking
+- [x] 4/24/2020-5/7/2020 Classification
+- [x] 5/7/2020-5/10/2020 Report
+
+## Prerequisite
+- python == 3.7.4
+- opencv == 3.4.2
+- imutils == 0.5.3
+
+## Part1: Calibration and Rectification
+
+### process
+1. Calibrate the camera
+2. Undistort the checkboard images
+3. Recalibrate the camera
+4. Undistort the conveyor images
+5. Rectification
+
+## Part2: Track Object
+- update 05.07.2020 Add classification. Now all the commends are written in English.   
+See ./scr/main.py.  
+In this part, I use BackgroundSubstracker(BS) to detect moving objects in a frame. To initialize the process, one should select two ROI mannually, one is thought to be the entry of the conveyor while the other one is the exit. After that, as illustrated in the fiugres below, a white conveyor model is made.  
+![avatar](/pics/entry.jpg)  
+![avatar](/pics/exit.jpg)  
+![avatar](/pics/conveyor.jpg)  
+If the object found by BS is not on the conveyor, it will be bounded by a green box.  
+![avatar](/pics/green.jpg)  
+Once it enters the entry we gave before, the green box will become a red one. Both the top view and the front view of the object are shown, since we need to TRACK THE OBJECT IN 3D. TO fetch the top view of the object, we need to use binocular vision method.  
+![avatar](/pics/view.jpg)  
+If the object is under occlusion, predict it according to the path history.  
+![avatar](/pics/predict.jpg)  
+
+
+ 
+
+
+
+
+
+
+
+
+
+

raw_data/cameraInfo.txt

+{'mtx1': array([[702.12283396,   0.        , 622.00974328],
+       [  0.        , 702.12753835, 370.36704991],
+       [  0.        ,   0.        ,   1.        ]]), 'dist1': array([[-0.01557181,  0.04002261,  0.00056291,  0.00050806, -0.0298868 ]]), 'mtx2': array([[700.61653016,   0.        , 649.35684548],
+       [  0.        , 700.88392479, 375.44386203],
+       [  0.        ,   0.        ,   1.        ]]), 'dist2': array([[-2.03788204e-02,  4.95428599e-02,  1.48263022e-04,
+         7.91146510e-05, -3.65846302e-02]]), 'R': array([[ 0.99992812, -0.00546432, -0.01067258],
+       [ 0.00540403,  0.99996933, -0.00566923],
+       [ 0.01070323,  0.00561114,  0.99992698]]), 'T': array([[-3.57023214],
+       [-0.00907662],
+       [ 0.02063652]])}
\ No newline at end of file

src/pcTools.py

+#!/usr/bin/python
+# -*- coding: UTF-8 -*-
+'''
+@author: mxl
+@date: 04/29/2020
+'''
+
+import numpy as np
+import open3d as o3d
+import cv2
+from matplotlib import pyplot as plt
+from preprocessing import DataLoader
+d1 = DataLoader.DataLoader('../imgs/left/', downsize=3)
+d2 = DataLoader.DataLoader('../imgs/right/', downsize=3)
+stereo = cv2.StereoBM_create(numDisparities=10*16, blockSize=15)
+stereo.setDisp12MaxDiff(200)
+stereo.setMinDisparity(7)
+stereo.setUniquenessRatio(5)
+stereo.setSpeckleWindowSize(3)
+stereo.setSpeckleRange(5)
+left = d1.getItem(1180)
+right = d2.getItem(1180)
+left1 = cv2.cvtColor(left, cv2.COLOR_BGR2GRAY)
+right1 = cv2.cvtColor(right, cv2.COLOR_BGR2GRAY)
+disp = stereo.compute(left1, right1).astype(np.float32)/16.0
+b = 3.57
+f = 702
+disp = (1/disp)*b*f
+disp = disp/disp.max()*10000
+plt.subplot(1,2,1)
+plt.imshow(disp, cmap='jet')
+plt.colorbar()
+plt.subplot(1,2,2)
+plt.imshow(left)
+plt.show()
\ No newline at end of file

src/preprocessing/Loader.py

+#!/usr/bin/python
+# -*- coding: UTF-8 -*-
+'''
+@author: mxl
+@date: 04/16/2020
+'''
+import cv2
+import numpy as np
+import math
+import os
+import glob
+
+class DataLoader(object):
+    def __init__(self, path, idx=0, cvt=None, size=None, downsize=None):
+        super(DataLoader).__init__()
+        self.cvt = cvt
+        self.idx = idx
+        self.size = size
+        self.downsize = downsize
+        self.file = glob.glob(path+'*')
+        self._len = len(self.file)
+
+    def cvtImg(self, im):
+        if isinstance(self.cvt, np.int):
+            im = cv2.cvtColor(im, self.cvt)
+        if isinstance(self.size, tuple):
+            im = cv2.resize(im, self.size)
+        if isinstance(self.downsize, np.int):
+            new_size =  (int(im.shape[1]/self.downsize),int(im.shape[0]/self.downsize))
+            im = cv2.resize(im, dsize=new_size)
+        return im
+
+    def getItem(self, idx):
+        im = self.cvtImg(cv2.imread(self.file[idx]))
+        return im
+    
+    def __next__(self):
+        im = self.getItem(self.idx)
+        self.idx+=1
+        return im
+    
+    def getRest(self):
+        return [self.__next__() for i in range(self.idx, self.len)]
+
+    @property
+    def len(self):
+        return self._len
+
+        
+class DualLoader(DataLoader):
+    def __init__(self, path, idx=0, cvt=None, size=None, downsize=None):
+        super(DualLoader, self).__init__(path, idx=idx, cvt=cvt, size=size, downsize=downsize) 
+        self.file_l = glob.glob(path+'left/*')
+        self.file_r = glob.glob(path+'right/*')
+        assert len(self.file_l)==len(self.file_r)
+        self._len = len(self.file_l)
+
+    def getItem(self, idx):
+        iml = self.cvtImg(cv2.imread(self.file_l[idx]))
+        imr = self.cvtImg(cv2.imread(self.file_r[idx]))
+        return iml, imr
+    
+    def __next__(self):
+        iml, imr = self.getItem(self.idx)
+        self.idx+=1
+        return iml, imr
+
+
+
+
+
+

src/preprocessing/__init__.py

+#!/usr/bin/python
+# -*- coding: UTF-8 -*-
+'''
+@author: mxl
+@date: 04/16/2020
+'''
+from Loader import *
\ No newline at end of file

src/utils/Kalman.py

+#!/usr/bin/python
+# -*- coding: UTF-8 -*-
+'''
+@author: mxl
+@date: 04/20/2020
+'''
+import cv2
+import numpy as np
+import math
+import random
+
+class Kalman(object):
+    def __init__(self):
+        super(Kalman).__init__()
+        # The initial state (6x1).
+        # displacement, velocity, acceleration
+        # including x, v_x, a_x, y, v_y, a_y 
+        self.X = np.zeros((6,1))
+        # The initial uncertainty (6x6).
+        # let's assume a large initial value since at the very first the uncertainty is high
+        uc = 500
+        self.P = uc*np.eye(6)
+        # The external motion (6x1).
+        self.u = np.zeros((6,1))
+        # The transition matrix (6x6).
+        self.F = np.array([[1, 1, 0.5, 0, 0, 0],
+              [0, 1, 1, 0, 0, 0],
+              [0, 0, 1, 0, 0, 0],
+              [0, 0, 0, 1, 1, 0.5],
+              [0, 0, 0, 0, 1, 1],
+              [0, 0, 0, 0, 0, 1]])
+        # The observation matrix (2x6).
+        self.H = np.array([[1,0,0,0,0,0],[0,0,0,1,0,0]])
+        # The measurement uncertainty.
+        self.R = np.array([[20],[20]])
+        self.I = np.eye(6)
+
+    def update(self, Z):
+        y = Z-np.dot(self.H, self.X)
+        S = np.dot(np.dot(self.H, self.P),np.transpose(self.H))+self.R
+        K = np.dot(np.dot(self.P, np.transpose(self.H)),np.linalg.pinv(S))
+        self.X = self.X+np.dot(K, y)
+        self.P = np.dot((self.I-np.dot(K, self.H)),self.P)
+    
+    def predict(self):
+        self.X = np.dot(self.F, self.X)+self.u
+        self.P = np.dot(np.dot(self.F, self.P),np.transpose(self.F))
+
+    def filt(self, Z):
+        self.update(Z)
+        pose = np.array([self.X[0],self.X[3]])
+        self.predict()
+        return pose
\ No newline at end of file

src/utils/calibTools.py

+#!/usr/bin/python
+# -*- coding: UTF-8 -*-
+'''
+@author: mxl
+@date: 04/07/2020
+'''
+import cv2
+import numpy as np
+from matplotlib import pyplot as plt
+from preprocessing import DataLoader
+
+def calibrate(lpath, rpath, nbh, nbv, show=False):
+    '''
+    get camera information
+    '''
+    nb_horizontal = 9
+    nb_vertical = 6
+    objp = np.zeros((nbh*nbv,3), np.float32) # set x and y coordinates
+    objp[:,:2] = np.mgrid[0:nbv,0:nbh].T.reshape(-1,2) # set z coordinates
+    # first read images, then extract image points 
+    left = DataLoader.DataLoader(lpath) #left images
+    right = DataLoader.DataLoader(rpath)
+    imgpl = [] #image points
+    imgpr = []
+    img_left = [] #image
+    img_right = []
+    objpoints = []
+    for i in range(left.len):
+        iml = left.__next__()
+        imr = right.__next__()
+        retl, corners_l = cv2.findChessboardCorners(iml,(nb_vertical,nb_horizontal))
+        retr, corners_r = cv2.findChessboardCorners(imr,(nb_vertical,nb_horizontal))
+        if retl and retr: # only when both of the images in a pair has the corners
+            imgpl.append(corners_l)
+            imgpr.append(corners_r)
+            img_left.append(iml)
+            img_right.append(imr)
+            objpoints.append(objp)
+        if show:
+            iiml, iimr = iml.copy(), imr.copy()
+            cv2.drawChessboardCorners(iml.copy(), (nbv,nbh), corners_l, retl)
+            cv2.drawChessboardCorners(imr.copy(), (nbv,nbh), corners_r, retr)
+            cv2.imshow('left', iiml)
+            cv2.waitKey(1)
+            cv2.imshow('right', iimr)
+            cv2.waitKey(1)
+    gray = cv2.cvtColor(iml, cv2.COLOR_BGR2GRAY)
+    # determine the camera matirx 1
+    mtx1, dist1= cv2.calibrateCamera(objpoints, imgpl, gray.shape[::-1], None, None)[1:3]
+    gray = cv2.cvtColor(imr, cv2.COLOR_BGR2GRAY)
+    # determine the camera matirx 2
+    mtx2, dist2 = cv2.calibrateCamera(objpoints, imgpr, gray.shape[::-1], None, None)[1:3]
+    cv2.destroyAllWindows()
+    mtx1, dist1, mtx2, dist2, R, T= cv2.stereoCalibrate(objpoints, imgpl, imgpr, mtx1, dist1, mtx2, dist2, 
+    gray.shape[::-1], flags=cv2.CALIB_FIX_INTRINSIC & cv2.CALIB_SAME_FOCAL_LENGTH)[1:7]
+    return mtx1, mtx2, dist1, dist2, R, T
+
+def undistort(img, mtx, dist, newmtx=True):
+    '''
+    undistort images and generate new camera matrix
+    '''
+    size = img.shape[:2]
+    size = size[::-1]
+    if newmtx:
+        newMtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, size,1,size)
+        # undistort
+        dst = cv2.undistort(img, mtx, dist, None, newMtx)
+        x,y,w,h = roi
+        dst = dst[y:y+h,x:x+w]
+        mtx = newMtx
+    else:
+        dst = cv2.undistort(img, mtx, dist, None, None)
+    return dst, mtx
+
+def drawlines(img1, img2, lines, pts1, pts2):
+    '''
+    draw epipolar lines
+    '''
+    r, c = img1.shape
+    img1 = cv2.cvtColor(img1, cv2.COLOR_GRAY2BGR)
+    img2 = cv2.cvtColor(img2, cv2.COLOR_GRAY2BGR)
+    for r, pt1, pt2 in zip(lines, pts1, pts2):
+        color = tuple(np.random.randint(0,255,3).tolist())
+        x0, y0 = map(int, [0, -r[2]/r[1]])
+        x1, y1 = map(int, [c, -(r[2]+r[0]*c)/r[1]])
+        img1 = cv2.line(img1, (x0,y0),(x1,y1),color,2)
+        img1 = cv2.circle(img1, tuple(pt1), 5, color, -1)
+        img2 = cv2.circle(img2, tuple(pt2), 5, color, -1)
+    return img1, img2
+
+def calLines(iml, imr, numMatches=300, plot=False):
+    '''
+    draw epipolar lines
+    '''
+    iml = cv2.cvtColor(iml,cv2.COLOR_RGB2GRAY)
+    imr = cv2.cvtColor(imr, cv2.COLOR_RGB2GRAY)
+    sift = cv2.xfeatures2d_SIFT.create()
+    kp1, des1 = sift.detectAndCompute(iml, None)
+    kp2, des2 = sift.detectAndCompute(imr, None)
+    matcher = cv2.FlannBasedMatcher()
+    match = matcher.match(des1, des2)
+    # select the most relevant matches
+    match = sorted(match, key=lambda x:x.distance)
+    pts1 = []
+    pts2 = []
+    for m in match[:numMatches]:
+        pts1.append(kp1[m.queryIdx].pt)
+        pts2.append(kp2[m.trainIdx].pt)
+    pts1 = np.int32(pts1)
+    pts2 = np.int32(pts2)
+    F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_LMEDS)
+    # select inlier points
+    pts1 = pts1[mask.ravel() == 1]
+    pts2 = pts2[mask.ravel() == 1]
+    lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1,1,2), 2, F)
+    lines1 = lines1.reshape(-1,3)
+    lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1,1,2), 1, F)
+    lines2 = lines2.reshape(-1,3)
+    iml1, iml2 = drawlines(iml, imr, lines1, pts1, pts2)
+    imr1, imr2 = drawlines(imr, iml, lines2, pts2, pts1)
+    if plot:
+        plt.subplot(1,2,1)
+        plt.imshow(iml1)
+        plt.subplot(1,2,2)
+        plt.imshow(imr1)
+        plt.show()
+    return iml1, imr1
+
+def stereoRectify(iml, imr, mtx1, dist1, mtx2, dist2, R, T):
+    size = iml.shape[:2]
+    size = size[::-1]
+    R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(mtx1, dist1, mtx2, dist2, size, R, T)
+    mapl1, mapl2 = cv2.initUndistortRectifyMap(mtx1, dist1, R1, P1, (iml.shape[:2])[::-1], cv2.CV_32FC1)
+    mapr1, mapr2 = cv2.initUndistortRectifyMap(mtx2, dist2, R2, P2,(imr.shape[:2])[::-1], cv2.CV_32FC1)
+    iml = cv2.remap(iml, mapl1, mapl2, cv2.INTER_LINEAR)
+    imr = cv2.remap(imr, mapr1, mapr2, cv2.INTER_LINEAR)
+    gray1 = cv2.cvtColor(iml, cv2.COLOR_BGR2GRAY)
+    for i in range(gray1.shape[0]):
+        if not gray1[i,:].min():
+            break
+    iml = iml[:i, :,:]
+    gray2 = cv2.cvtColor(imr, cv2.COLOR_BGR2GRAY)
+    for i in range(gray2.shape[0]):
+        if not gray2[i,:].min():
+            break
+    imr = imr[:i, :,:]
+    return iml, imr
+
+def save(mtx1, dist1, mtx2, dist2, R, T):
+    cameraInfo = {'mtx1':mtx1, 'dist1':dist1, 'mtx2':mtx2, 'dist2':dist2, 'R':R, 'T':T}
+    with open('raw_data/cameraInfo.txt','w') as f:
+        f.write(str(cameraInfo))