目录
项目场景
项目参照高老师的《视觉SLAM十四讲》进行实现,主要是为了巩固自己知识,代码仅供参考。
直接上代码
需要注意的是,代码opencv的版本是3.4.2.16(4版本的opencv太新了,好像不兼容),此外,在主函数里面,需要输入自己的图片哦!
import numpy as np
import cv2 as cv
def find_feature_matches(img_1, img_2):
"""寻找特征匹配的点
Args:
img_1: pass
img_2: pass
Returns:
kp1:
kp2:
good_match:
"""
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
if __name__ == "__main__":
img1 = cv.imread("1.png")
img2 = cv.imread("2.png")
depth1 = cv.imread("1_depth.png", -1) # 读取深度图像要用-1
depth2 = cv.imread("2_depth.png", -1)
# 图像匹配
keypoints_1, keypoints_2, matches = find_feature_matches(img1, img2)
print("共计匹配点:", len(matches))
# 筛选特征点,只记匹配的点,参照上一博客的poes_estimation_2d2d(keypoint_1, keypoint_2, matches)函数
pts2 = []
pts1 = []
for i in range(int(len(matches))):
pts1.append(keypoints_1[matches[i].queryIdx].pt)
pts2.append(keypoints_2[matches[i].trainIdx].pt)
pts1 = np.int32(pts1)
pts2 = np.int32(pts2)
# 建立3D点
# 深度图像为16位无符号数,单通道图像
K=[[520.9, 0, 325.1], [0, 521.0, 249.7], [0, 0, 1]]
K=np.array(K)
pts_3d = []
pts_2d = []
for i in range(pts1.shape[0]): # pts1.shape[0]: 匹配点总数 pts1.shape = (len(matches), 2)
p1 = pts1[i]
d1 = depth1[p1[1],p1[0]]/1000.0 # 深度距离 以5000为一个单位 高博的是1000.0
#print(d1)
if d1 == 0:
continue
p1 = (p1 - (K[0][2],K[1][2]))/(K[0][0],K[1][1])*d1 # 归一化坐标 根据深度单位转为实际坐标
pts_3d.append([p1[0], p1[1], d1])
pts_2d.append(pts2[i])
#
print("最终匹配数:", len(pts_3d))
pts_3d = np.float64(pts_3d)
pts_2d = np.float64(pts_2d)
print("3D点:")
print(pts_3d)
flag,R,t = cv.solvePnP(pts_3d,pts_2d,K,None)
R,Jacobian = cv.Rodrigues(R)
print("旋转矩阵R:\n", R)
print("平移矩阵t:\n", t)
实验
最终输出的两个矩阵与上一篇的博客的结果有点区别,旋转矩阵基本上对上了,但是平移矩阵,都有差别,不管是与上一篇博客的还是与高博的