This is the first day of my participation in the First Challenge 2022

For this part of the code, you may have seen a series of videos shared about the driverless Python version a year or so ago. This video is still somewhat difficult, and the difficulty is for me personally. At that time, I was also eager to do a lot of coding to create a self-driving for my own amusement. At that time, however, it was just flowing with coding, and the python G2O installation didn’t work out, so it was abandoned

During the Spring Festival holiday, I want to take another challenge. This time, I have accumulated some knowledge, especially in visual geometry, and made a little improvement in c++, so I come back to try again, hoping to make a breakthrough this time and finish the python version of slam.

A little bit of a requirement is to know python and opencV as a library. Have some knowledge of computer vision.

The basic idea

First of all, we read and display the video so that we can observe the effect, and then extract each feature point, which is the corner point, and then match the direction of motion between the frame and the frame, the distance of movement and the vehicle.

Show video

import time
from turtle import position

import cv2
import sdl2
import sdl2.ext

sdl2.ext.init()

W = 1920//2
H = 1080//2


window = sdl2.ext.Window('SLAM', size=(W,H),position=(-500, -500))
window.show()

def process_frame(img) :
    img = cv2.resize(img,(W,H))

    events = sdl2.ext.get_events()
    for event in events:
        if event.type == sdl2.SDL_QUIT:
            exit(0)
    # print(dir(window))
    surf = sdl2.ext.pixels2d(window.get_surface())
    surf[:] = img.swapaxes(0.1) :, :,0]
    window.refresh()


if __name__ == "__main__":
    # get video stream
    cap = cv2.VideoCapture("test.mp4")
    # 
    while cap.isOpened():
        # read frame of video
        ret, frame = cap.read()
        if ret == True:
            process_frame(frame)
        else:
            break
Copy the code

Read the image of the video, then scale the image to 2 times the original image, and then SDL2 will display the video.

window = sdl2.ext.Window('SLAM', size=(W,H),position=(-500, -500))
Copy the code

Sdl2 defines a window. The first parameter is the name of the window, the second parameter is the size of the window, and the third parameter defines the upper-left corner position of the window.

img.swapaxes(0.1) :, :,0]
Copy the code

Swap the 0 and 1 axis positions, that is, the width and height positions need to be changed. This is due to a mismatch between sdL2 display and CV2 read images.

Extracting feature points



orb = cv2.ORB_create()
def process_frame(img) :
    img = cv2.resize(img,(W,H))

    kp, des = orb.detectAndCompute(img,None)
    for p in kp:
        x,y = map(lambda x:round(x), p.pt)
        cv2.circle(img,(x,y),color=(0.255.0),radius=3)
    display.paint(img)

Copy the code

We simply observe the detection results, as shown in the figure above, the detected feature points are not evenly distributed, so the detected feature points are not expected. It looks like a mess. That is, OpencV does not provide satisfactory ORB feature point extraction.

class FeatureExtractor(object) :
    GX = 16//2
    GY = 12//2

    def __init__(self) - >None:
        self.orb = cv2.ORB_create()

    def extract(self,img) :
        sy = img.shape[0] // self.GX
        sx = img.shape[1] // self.GY

        akp = []
        for ry in range(0, img.shape[0],sy):
            for rx in range(0, img.shape[1], sx):
                img_chunk = img[ry:ry + sy, rx:rx + sx]
                kp = self.orb.detect(img_chunk,None)
                for p in kp:
                    p.pt = (p.pt[0]  + rx, p.pt[1] + ry)
                    akp.append(p)
        return akp

Copy the code

This part of the code has not been retained in the end because of the unsatisfactory detection effect. Since it is not used, I still want to share it with you. The reason is that through the code, we know how to divide the image into blocks for processing.

class FeatureExtractor(object) :

    def __init__(self) - >None:
        self.orb = cv2.ORB_create()

    def extract(self,img) :
        feats = cv2.goodFeaturesToTrack(np.mean(img,axis=2).astype(np.uint8),
            3000, qualityLevel=0.01, minDistance=3)
        print(feats)

        return feats
Copy the code

class FeatureExtractor(object) :

    def __init__(self) - >None:
        self.orb = cv2.ORB_create()

    def extract(self,img) :
        feats = cv2.goodFeaturesToTrack(np.mean(img,axis=2).astype(np.uint8),
            3000, qualityLevel=0.01, minDistance=3)
        kps = [cv2.KeyPoint(x=f[0] [0], y=f[0] [1],_size=20) for f in feats]
        des = self.orb.compute(img, kps)

        return kps,des
Copy the code

Opencv provides goodFeaturesToTrack method, which can find the optimal Corner point in an image through shi-tomasi method (Harris Corner Detection can be selected by the specified method). First, the grayscale image is received, where np.mean(img,axis=2).astype(np.uint8) method is used to obtain a grayscale image, 3000 represents the maximum number of detected corners, and the value range of qualityLevel is 0 to 1. Corners less than the specified qualityLevel will be discarded. Specifies the minimum distance between detected corners.

Then we need to calculate the corner description values, which will be used for subsequent corner matching. The basic idea is that we find corner points, and then use the distance between the two frames to build the map.