[SLAM] opencv-python的2D-2D视觉里程计（参考高老师的视觉slam十四讲）

项目场景

最近在鼓捣slam相关的技术，奈何身边我知道的只有我一个人在学习slam技术，没有人一块交流，感觉学习很乏力，不知道从部分开始做起。
看了高博的视觉slam十四讲之后决心从视觉里程计开始着手，而我对C++的掌握几乎等于没有，所以决心仿照高博的C++代码用python实现一遍，了解算法框架之后再慢慢改C++代码（实际上想着绕开C++，hhh），于是就有了这篇文章。
跟高博的代码差不多，只不过还没有完善验证对极约束。

直接上代码

需要注意的是，代码opencv的版本是3.4.2.16（4版本的opencv太新了，好像不兼容），此外，在主函数里面，需要输入自己的图片哦！

import numpy as np
import cv2 as cv
from matplotlib import pyplot as plt
from numpy.lib.twodim_base import mask_indices

def find_feature_matches(img_1, img_2):
    orb = cv.ORB_create()

    kp1 = orb.detect(img_1)
    kp2 = orb.detect(img_2)

    kp1, des1 = orb.compute(img_1, kp1)
    kp2, des2 = orb.compute(img_2, kp2)

    bf = cv.BFMatcher(cv.NORM_HAMMING)

    matches = bf.match(des1, des2)

    min_distance = matches[0].distance
    max_distance = matches[0].distance

    for x in matches:
        if x.distance < min_distance:
            min_distance = x.distance
        if x.distance > max_distance:
            max_distance = x.distance
    
    print("Max dist:", max_distance)
    print("Min dist:", min_distance)

    good_match = []

    for x in matches:
        if x.distance <= max(2*min_distance, 30.0):
            good_match.append(x)
    return kp1, kp2, good_match

def poes_estimation_2d2d(keypoint_1, keypoint_2, matches):
    k=[[520.9, 0, 325.1], [0, 521.0, 249.7], [0, 0, 1]]
    k=np.array(k)
    print("相机内参：", k)

    #print(keypoint_1)
    #print("描述子：", matches)

    good = []
    pts2 = []
    pts1 = []
    for i in range(int(len(matches))):
        pts1.append(keypoint_1[matches[i].queryIdx].pt)
        pts2.append(keypoint_2[matches[i].trainIdx].pt)

    pts1 = np.int32(pts1)
    pts2 = np.int32(pts2)
    # 计算基础矩阵 采用8点法
    f, mask= cv.findFundamentalMat(points1=pts1, points2=pts2, method=cv.FM_8POINT)
    print("基础矩阵：" , f)

    # 计算本质矩阵  
    e, mask = cv.findEssentialMat(points1=pts1, points2=pts2, cameraMatrix=k)
    print("本质矩阵： ", e)

    # 计算单应矩阵
    h, mask = cv.findHomography(pts1, pts2)
    print("单应矩阵： ", h)

    # 从本质矩阵恢复旋转信息和平移信息
    retval2, R, t, mask = cv.recoverPose(E=e, points1=pts1, points2=pts2, cameraMatrix=k)
    print("旋转矩阵R:", R)
    print("平移矩阵t:", t)
    #print(mask)

if __name__ == "__main__":
    #img_1 = cv.imread("1.png")
    #img_2 = cv.imread("2.png")
    img_1 = cv.imread("./img/0000.jpg")
    img_2 = cv.imread("./img/0002.jpg")

    # 图像匹配
    keypoint_1, keypoint_2, matches = find_feature_matches(img_1, img_2)
    print("共计匹配点:", len(matches))

    # 预测位姿
    #R, t = poes_estimation_2d2d(keypoint_1, keypoint_2, matches)
    poes_estimation_2d2d(keypoint_1, keypoint_2, matches)

---

实验

说实话，虽然看了书和视频，但是感觉自己还是懵懵懂懂的，测试的时候，发现python和C++对相同的一组图输出并不一样，如图为C++环境下的，也就是高博的源代码跑出来的输出，图片采用的也是高博的。

而我自己用python写的代码输出却是这样的。

感觉差距还是蛮大的，后续我用自己的相机拍了几张照片，只进行平移的话，旋转矩阵都是单位阵，而平移矩阵多多少少都有区别，搞不懂。

最后的话

代码仅供参考，同时也希望有志同道合的小伙伴一起学习slam，一个人学真的太难了。