双目标定+去畸变+视差/深度+测距+特征点匹配算法（ipynb+opencv 4.3）
文章目录
本文主要阐述代码与试验部分，提供范例，直接使用。
本文提供标定图片：
链接:百度网盘 密码:tldz
1. 标定
导入图片需要更改：
- 角点个数
- 文件夹名称
- 世界坐标系的圆心点距离
- 确定是棋盘格还是圆心标定板
import cv2
import matplotlib.pyplot as plt # plt 用于显示图片
import matplotlib.image as mpimg # mpimg 用于读取图片
import sys
import numpy as np
import glob
class shuangmu:
def __init__(self):
self.m1 = 0
self.m2 = 0
self.d1 = 0
self.d2 = 0
self.R = 0
self.T = 0
stereo = shuangmu()
class StereoCalibration(object):
def __init__(self):
self.imagesL = self.read_images('camL')
self.imagesR = self.read_images('camR')
def read_images(self , cal_path):
filepath = glob.glob(cal_path + '/*.bmp')
filepath.sort()
return filepath
#标定图像
def calibration_photo(self):
#设置要标定的角点个数
x_nums = 7 #x方向上的角点个数
y_nums = 7
# 设置(生成)标定图在世界坐标中的坐标
world_point = np.zeros((x_nums * y_nums,3),np.float32) #生成x_nums*y_nums个坐标，每个坐标包含x,y,z三个元素
world_point[:,:2] = np.mgrid[:x_nums,:y_nums].T.reshape(-1, 2) #mgrid[]生成包含两个二维矩阵的矩阵，每个矩阵都有x_nums列,y_nums行
#.T矩阵的转置
#reshape()重新规划矩阵，但不改变矩阵元素
#保存角点坐标
world_position = []
image_positionl = []
image_positionr = []
#设置角点查找限制
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER,30,0.001)
#获取所有标定图
for ii in range(20):
image_path_l = self.imagesL[ii]
image_path_r = self.imagesR[ii]
image_l = cv2.imread(image_path_l)
image_r = cv2.imread(image_path_r)
gray_l = cv2.cvtColor(image_l,cv2.COLOR_RGB2GRAY)
gray_r = cv2.cvtColor(image_r,cv2.COLOR_RGB2GRAY)
#查找角点
# ok,corners = cv2.findChessboardCorners(gray,(x_nums,y_nums),None)
# ok1,cornersl = cv2.findChessboardCorners(gray_l,(x_nums,y_nums),None)
# ok2,cornersr = cv2.findChessboardCorners(gray_r,(x_nums,y_nums),None)
ok1,cornersl = cv2.findCirclesGrid(gray_l,(x_nums,y_nums),None)
ok2,cornersr = cv2.findCirclesGrid(gray_r,(x_nums,y_nums),None)
self.world = world_point
# print(ok1&ok2)
if ok1&ok2:
#把每一幅图像的世界坐标放到world_position中
center_spacing = 15 ## 圆心的位置距离，这一个其实不重要
world_position.append(world_point*center_spacing)
#获取更精确的角点位置
exact_cornersl = cv2.cornerSubPix(gray_l,cornersl,(11,11),(-1,-1),criteria)
exact_cornersr = cv2.cornerSubPix(gray_r,cornersr,(11,11),(-1,-1),criteria)
#把获取的角点坐标放到image_position中
image_positionl.append(exact_cornersl)
image_positionr.append(exact_cornersr)
#可视化角点
# image = cv2.drawChessboardCorners(image,(x_nums,y_nums),exact_corners,ok)
# cv2.imshow('image_corner',image)
# cv2.waitKey(0)
#计算内参数
image_shape = gray_l.shape[::-1]
retl, mtxl, distl, rvecsl, tvecsl = cv2.calibrateCamera(world_position, image_positionl, image_shape , None,None)
retr, mtxr, distr, rvecsr, tvecsr = cv2.calibrateCamera(world_position, image_positionr, image_shape , None,None)
print('left camera\'s K = ',mtxl)
print('right camera\'s K = ',mtxr)
print('left camera\'s distort = ' , distl)
print('right camera\'s distort = ' , distr)
stereo.m1 = mtxl
stereo.m2 = mtxr
stereo.d1 = distl
stereo.d2 = distr
#计算误差
self.cal_error(world_position , image_positionl , mtxl , distl , rvecsl , tvecsl)
self.cal_error(world_position , image_positionr , mtxr, distr , rvecsr , tvecsr)
##双目标定
self.stereo_calibrate( world_position ,image_positionl , image_positionr , mtxl, distl, mtxr, distr, image_shape)
def cal_error(self , world_position , image_position , mtx , dist , rvecs , tvecs):
#计算偏差
mean_error = 0
for i in range(len(world_position)):
image_position2, _ = cv2.projectPoints(world_position[i], rvecs[i], tvecs[i], mtx, dist)
error = cv2.norm(image_position[i], image_position2, cv2.NORM_L2) / len(image_position2)
mean_error += error
print("total error: ", mean_error / len(image_position))
def stereo_calibrate( self , objpoints ,imgpoints_l , imgpoints_r , M1, d1, M2, d2, dims):
flags = 0
flags |= cv2.CALIB_FIX_INTRINSIC
flags |= cv2.CALIB_USE_INTRINSIC_GUESS
flags |= cv2.CALIB_FIX_FOCAL_LENGTH
flags |= cv2.CALIB_ZERO_TANGENT_DIST
stereocalib_criteria = (cv2.TERM_CRITERIA_MAX_ITER +cv2.TERM_CRITERIA_EPS, 100, 1e-5)
ret, M1, d1, M2, d2, R, T, E, F = cv2.stereoCalibrate(
objpoints, imgpoints_l,
imgpoints_r, M1, d1, M2,
d2, dims,
criteria=stereocalib_criteria, flags=flags)
print('R = ',R)
print('T = ',T)
stereo.R = R
stereo.T = T
if __name__ == '__main__':
# calibration_photo()
np.set_printoptions(precision=5, suppress=True)
biaoding = StereoCalibration()
biaoding.calibration_photo()
输出结果：
left camera's K = [[2158.66436 0. 663.3482 ]
[ 0. 2158.75332 521.60097]
[ 0. 0. 1. ]]
right camera's K = [[2156.96974 0. 609.71291]
[ 0. 2158.65266 532.00808]
[ 0. 0. 1. ]]
left camera's distort = [[ -0.03706 1.35135 0.00024 0.00197 -10.94371]]
right camera's distort = [[ 0.00666 -0.49887 0.00156 0.00093 8.74557]]
total error: 0.02552895315013799
total error: 0.028725163900975837
R = [[ 0.85431 -0.00472 0.51974]
[ 0.00609 0.99998 -0.00094]
[-0.51972 0.00397 0.85432]]
T = [[-289.43628]
[ -6.46068]
[ 52.29194]]
从这里看到，圆板的中心点距离为15mm，所以相机的基线的长度为289mm左右。
另外双目之间的旋转矩阵有一些异常，旋转量过大。说明双目所看的方向不太平行，最终导致的结果就是：立体几何校正之后出现图片拼接错误的情况。
# 将双目参数保存到这个结构体中，方便调用
import numpy as np
import cv2
#双目相机参数
class stereoCameral(object):
def __init__(self):
#左相机内参数
self.cam_matrix_left = stereo.m1
#右相机内参数
self.cam_matrix_right = stereo.m2
#左右相机畸变系数:[k1, k2, p1, p2, k3]
self.distortion_l = stereo.d1
self.distortion_r = stereo.d2
#旋转矩阵
self.R = stereo.R
# self.R = np.array([[1.0,0.0,0.0],[0.0,1.0,0.0],[0.0,0.0,1.0]])
#平移矩阵
self.T = stereo.T
self.T = np.array([[-293.835],
[ -7.334],
[ 52.816]])
self.baseline = stereo.T[0]
2. 去畸变、立体几何校正
这里问题是：基线这个量只有在计算深度时才有用
import cv2
import numpy as np
import matplotlib.pyplot as plt # plt 用于显示图片
import matplotlib.image as mpimg # mpimg 用于读取图片
# 预处理
def preprocess(img1, img2):
# 彩色图->灰度图
im1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
im2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
# 直方图均衡
im1 = cv2.equalizeHist(im1)
im2 = cv2.equalizeHist(im2)
return im1, im2
# 消除畸变
def undistortion(image, camera_matrix, dist_coeff):
undistortion_image = cv2.undistort(image, camera_matrix, dist_coeff)
return undistortion_image
# 获取畸变校正和立体校正的映射变换矩阵、重投影矩阵
# @param：config是一个类，存储着双目标定的参数:config = stereoconfig.stereoCamera()
def getRectifyTransform(height, width, config):
# 读取内参和外参
left_K = config.cam_matrix_left
right_K = config.cam_matrix_right
left_distortion = config.distortion_l
right_distortion = config.distortion_r
R = config.R
T = config.T
# #
# R = np.transpose(config.R)
# T = -np.dot(np.transpose(config.R),config.T)
# 计算校正变换
height = int(height)
width = int(width)
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(left_K, left_distortion, right_K, right_distortion, (width, height), R, T,flags=0, alpha=-1)
print('R1 = ',R1)
print('R1 = ',R2)
print('P1 = ',P1)
print('P2 = ',P2)
# map1x, map1y = cv2.initUndistortRectifyMap(left_K, left_distortion, R1, P1, (width, height), cv2.CV_16SC2)
# map2x, map2y = cv2.initUndistortRectifyMap(right_K, right_distortion, R2, P2, (width, height), cv2.CV_16SC2)
map1x, map1y = cv2.initUndistortRectifyMap(left_K, left_distortion, R1, P1, (width, height), cv2.CV_32FC1)
map2x, map2y = cv2.initUndistortRectifyMap(right_K, right_distortion, R2, P2, (width, height), cv2.CV_32FC1)
print(width,height)
return map1x, map1y, map2x, map2y, Q
# 畸变校正和立体校正
def rectifyImage(image1, image2, map1x, map1y, map2x, map2y):
rectifyed_img1 = cv2.remap(image1, map1x, map1y, cv2.INTER_LINEAR)
rectifyed_img2 = cv2.remap(image2, map2x, map2y, cv2.INTER_LINEAR)
return rectifyed_img1, rectifyed_img2
# 立体校正检验----画线
def draw_line1(image1, image2):
# 建立输出图像
height = max(image1.shape[0], image2.shape[0])
width = image1.shape[1] + image2.shape[1]
output = np.zeros((height, width,3), dtype=np.uint8)
output[0:image1.shape[0], 0:image1.shape[1]] = image1
output[0:image2.shape[0], image1.shape[1]:] = image2
for k in range(15):
cv2.line(output, (0, 50 * (k + 1)), (2 * width, 50 * (k + 1)), (0, 255, 0), thickness=2, lineType=cv2.LINE_AA) # 直线间隔：100
return output
# 立体校正检验----画线
def draw_line2(image1, image2):
# 建立输出图像
height = max(image1.shape[0], image2.shape[0])
width = image1.shape[1] + image2.shape[1]
output = np.zeros((height, width), dtype=np.uint8)
output[0:image1.shape[0], 0:image1.shape[1]] = image1
output[0:image2.shape[0], image1.shape[1]:] = image2
for k in range(15):
cv2.line(output, (0, 50 * (k + 1)), (2 * width, 50 * (k + 1)), (0, 255, 0), thickness=2, lineType=cv2.LINE_AA) # 直线间隔：100
return output
# 视差计算
def disparity_SGBM(left_image, right_image, down_scale=False):
# SGBM匹配参数设置
if left_image.ndim == 2:
img_channels = 1
else:
img_channels = 3
blockSize = 3
param = {'minDisparity': 0,
'numDisparities': 128,
'blockSize': blockSize,
'P1': 8 * img_channels * blockSize ** 2,
'P2': 32 * img_channels * blockSize ** 2,
'disp12MaxDiff': 1,
'preFilterCap': 63,
'uniquenessRatio': 15,
'speckleWindowSize': 100,
'speckleRange': 2,
'mode': cv2.STEREO_SGBM_MODE_SGBM_3WAY
}
# 构建SGBM对象
sgbm = cv2.StereoSGBM_create(**param)
# 计算视差图
size = (left_image.shape[1], left_image.shape[0])
if down_scale == False:
disparity_left = sgbm.compute(left_image, right_image)
disparity_right = sgbm.compute(right_image, left_image)
else:
left_image_down = cv2.pyrDown(left_image)
right_image_down = cv2.pyrDown(right_image)
factor = size[0] / left_image_down.shape[1]
disparity_left_half = sgbm.compute(left_image_down, right_image_down)
disparity_right_half = sgbm.compute(right_image_down, left_image_down)
disparity_left = cv2.resize(disparity_left_half, size, interpolation=cv2.INTER_AREA)
disparity_right = cv2.resize(disparity_right_half, size, interpolation=cv2.INTER_AREA)
disparity_left *= factor
disparity_right *= factor
return disparity_left, disparity_right
if __name__ == '__main__':
imgL = cv2.imread("pingzi_L.bmp")
imgR = cv2.imread("pingzi_R.bmp")
# imgL = cv2.imread("camL/L3.bmp")
# imgR = cv2.imread("camR/R3.bmp")
# imgL , imgR = preprocess(imgL ,imgR )
## 变成灰度图（to do）
height, width = imgL.shape[0:2]
config = stereoCameral() # 读取相机内参和外参
# 去畸变
imgL = undistortion(imgL ,config.cam_matrix_left , config.distortion_l )
imgR = undistortion(imgR ,config.cam_matrix_right, config.distortion_r )
# 几何极线对齐(这一步是不是存在问题？)
map1x, map1y, map2x, map2y, Q = getRectifyTransform(height, width, config)
iml_rectified, imr_rectified = rectifyImage(imgL, imgR, map1x, map1y, map2x, map2y)
linepic = draw_line1(iml_rectified , imr_rectified)
# plt.imshow(imgL)
# plt.show()
# plt.imshow(iml_rectified)
# plt.show()
# plt.imshow(imr_rectified)
# plt.show()
plt.imshow(linepic)
plt.show()
# # 计算视差
lookdispL,lookdispR = disparity_SGBM(iml_rectified , imr_rectified )
linepic2 = draw_line2(lookdispL,lookdispR)
plt.imshow(lookdispL)
# # points_3d = cv2.reprojectImageTo3D(lookdispL, Q)
输出结果：
R1 = [[ 0.93265 0.01921 0.36027]
[-0.01761 0.99982 -0.00773]
[-0.36035 0.00087 0.93282]]
R1 = [[ 0.98393 0.02456 -0.17686]
[-0.02378 0.9997 0.00651]
[ 0.17696 -0.0022 0.98421]]
P1 = [[2158.70299 0. -248.65393 0. ]
[ 0. 2158.70299 529.81726 0. ]
[ 0. 0. 1. 0. ]]
P2 = [[ 2158.70299 0. 1034.00797 -644662.32119]
[ 0. 2158.70299 529.81726 0. ]
[ 0. 0. 1. 0. ]]
之前一直出错，就是因为这个函数出错：
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(left_K, left_distortion, right_K, right_distortion, (width, height), R, T,flags=0, alpha=-1)
这里的flags=0，是很重要的量
3. 获取视差
这里需要注意的是：
- 视差的参数会极大影响到最终的结果
- 视差怎么样与具体大小对结果影响很大
- 怎样对应到现实的尺度，还需要确定一下
import numpy as np
import cv2
import time
def SGBM_update(val=0):
global SGBM_num
global SGBM_blockSize
SGBM_blockSize=cv2.getTrackbarPos('blockSize', 'SGNM_disparity')
if SGBM_blockSize % 2 == 0:
SGBM_blockSize += 1
if SGBM_blockSize < 5:
SGBM_blockSize = 5
SGBM_stereo.setBlockSize(SGBM_blockSize)
SGBM_num=cv2.getTrackbarPos('num_disp', 'SGNM_disparity')
num_disp = SGBM_num * 16
SGBM_stereo.setNumDisparities(num_disp)
SGBM_stereo.setUniquenessRatio(cv2.getTrackbarPos('unique_Ratio', 'SGNM_disparity'))
SGBM_stereo.setSpeckleWindowSize(cv2.getTrackbarPos('spec_WinSize', 'SGNM_disparity'))
SGBM_stereo.setSpeckleRange(cv2.getTrackbarPos('spec_Range', 'SGNM_disparity'))
SGBM_stereo.setDisp12MaxDiff(cv2.getTrackbarPos('disp12MaxDiff', 'SGNM_disparity'))
print('computing SGNM_disparity...')
displ = imgR
displ = SGBM_stereo.compute(imgL, imgR).astype(np.float32) / 16.0
dispr = SGBM_stereo.compute(imgR, imgL).astype(np.float32) / 16.0
return displ,dispr
# cv2.imshow('left', imgL)
# cv2.imshow('right', imgR)
# cv2.imshow('SGNM_disparity', (disp - min_disp) / num_disp)
if __name__ == "__main__":
# start = time.clock()
SGBM_blockSize = 5 #一个匹配块的大小,大于1的奇数
SGBM_num=5
min_disp = 0 #最小的视差值，通常情况下为0
num_disp =SGBM_num * 16 #192 - min_disp #视差范围，即最大视差值和最小视差值之差，必须是16的倍数。
#blockSize = blockSize #匹配块大小（SADWindowSize），必须是大于等于1的奇数，一般为3~11
uniquenessRatio = 6 #视差唯一性百分比， 视差窗口范围内最低代价是次低代价的(1 + uniquenessRatio/100)倍时，最低代价对应的视差值才是该像素点的视差，否则该像素点的视差为 0，通常为5~15.
speckleRange = 10 #视差变化阈值，每个连接组件内的最大视差变化。如果你做斑点过滤，将参数设置为正值，它将被隐式乘以16.通常，1或2就足够好了
speckleWindowSize = 100#平滑视差区域的最大尺寸，以考虑其噪声斑点和无效。将其设置为0可禁用斑点过滤。否则，将其设置在50-200的范围内。
disp12MaxDiff = 200 #左右视差图的最大容许差异（超过将被清零），默认为 -1，即不执行左右视差检查。
P1 = 600 #惩罚系数，一般：P1=8*通道数*SADWindowSize*SADWindowSize，P2=4*P1
P2 = 240 #p1控制视差平滑度，p2值越大，差异越平滑
# imgL = cv2.imread('im2.ppm')
# imgR = cv2.imread('im6.ppm')
imgL = iml_rectified;
imgR = imr_rectified;
cv2.namedWindow('SGNM_disparity')
cv2.createTrackbar('blockSize', 'SGNM_disparity', SGBM_blockSize, 21, SGBM_update)
cv2.createTrackbar('num_disp', 'SGNM_disparity', SGBM_num, 20, SGBM_update)
cv2.createTrackbar('spec_Range', 'SGNM_disparity', speckleRange, 50, SGBM_update)#设置trackbar来调节参数
cv2.createTrackbar('spec_WinSize', 'SGNM_disparity', speckleWindowSize, 200, SGBM_update)
cv2.createTrackbar('unique_Ratio', 'SGNM_disparity', uniquenessRatio, 50, SGBM_update)
cv2.createTrackbar('disp12MaxDiff', 'SGNM_disparity', disp12MaxDiff, 250, SGBM_update)
SGBM_stereo = cv2.StereoSGBM_create(
minDisparity = min_disp, # 最小的视差值
numDisparities = num_disp, # 视差范围
blockSize=SGBM_blockSize, # 匹配块大小（SADWindowSize）
uniquenessRatio=uniquenessRatio, # 视差唯一性百分比
speckleRange=speckleRange, # 视差变化阈值
speckleWindowSize=speckleWindowSize,
disp12MaxDiff=disp12MaxDiff, # 左右视差图的最大容许差异
P1=P1, # 惩罚系数
P2=P2
)
displ,dispr = SGBM_update()
plt.imshow( (imgL))
plt.show()
# plt.imshow( (imgR))
# plt.show()
plt.imshow( (displ))
plt.show()
# 因为右视差一定是哦有问题的
# plt.imshow( (dispr ))
# plt.show()
对于SGBM算法，最重要的就是参数的选取，参考https://blog.csdn.net/wwp2016/article/details/86080722
4. 利用视差获得深度
但是这里的效果一般，有待优化。
# 用视差获取直接深度
depth = np.ones_like(lookdispL , dtype=np.uint8)
for i in range(height):
for j in range(width):
if lookdispL[i][j]<5: ##噪音
depth[i][j]=0
else:
depth[i][j] = config.cam_matrix_left[0][0]*config.baseline/(lookdispL[i][j])
cv2.medianBlur(depth,3)
plt.imshow(depth)
提取图片中的特征点
ORB特征点匹配，但是出现下面的问题：
- 出现错误匹配，需要剔除吗？还是直接进行计算PNP的问题？
- 下面有两种方法：KNN匹配与暴力匹配
# coding=utf-8
import cv2
import numpy as np
img1 = cv2.imread('pingzi_L.bmp')
img2 = cv2.imread('pingzi_R.bmp')
# linepic2 = draw_line(img1,img2)
# plt.imshow(linepic2)
#最大特征点数,需要修改，5000太大。
orb = cv2.ORB_create(900)
kp1, des1 = orb.detectAndCompute(img1,None)
kp2, des2 = orb.detectAndCompute(img2,None)
#提取并计算特征点
bf = cv2.BFMatcher(cv2.NORM_HAMMING)
#knn筛选结果
matches = bf.knnMatch(des1, trainDescriptors = des2, k = 2)
# 计算距离
good = [m for (m,n) in matches if m.distance < 0.75*n.distance]
# 排序
good = sorted(good, key=lambda x: x.distance)
#查看最大匹配点数目
img3 = cv2.drawMatches(img1, kp1, img2, kp2, good, img1, flags=2)
plt.figure(figsize=(20,20))
plt.imshow(img3)
看到这里有明显的错误匹配。
- 利用RANSAC算法将不合理的匹配点去除
https://zhuanlan.zhihu.com/p/45532306 - p2p的过程可以参考项目https://github.com/MiaoDX/slambook_python
- 实际上看官方的opencv文档会更有帮助https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_feature2d/py_matcher/py_matcher.html
MIN_MATCH_COUNT = 20
if len(good)>MIN_MATCH_COUNT:
src_pts = np.float32([ kp1[m.queryIdx].pt for m in good ]).reshape(-1,1,2)
dst_pts = np.float32([ kp2[m.trainIdx].pt for m in good ]).reshape(-1,1,2)
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,5.0)
matchesMask = mask.ravel().tolist() ## 保存这个匹配
h,w = img1.shape[:2]
pts = np.float32([ [0,0],[0,h-1],[w-1,h-1],[w-1,0] ]).reshape(-1,1,2)
dst = cv2.perspectiveTransform(pts,M)
img2 = cv2.polylines(img2,[np.int32(dst)],True,255,3, cv2.LINE_AA)
else:
print ("Not enough matches are found - %d/%d") % (len(good),MIN_MATCH_COUNT)
matchesMask = None
## 筛选good，并且画出来
draw_params = dict(matchColor = (0,255,0), # draw matches in green color
singlePointColor = None,
matchesMask = matchesMask, # draw only inliers
flags = 2)
img3 = cv2.drawMatches(img1,kp1,img2,kp2,good,None,**draw_params)
plt.figure(figsize = (20,20))
plt.imshow(img3, 'gray'),plt.show()
- 也可以采用暴力匹配
import cv2
"""
orb特征检测和匹配
两幅图片分别是 乐队的logo 和包含该logo的专辑封面
利用orb进行检测后进行匹配两幅图片中的logo
"""
# 按照灰度图像的方式读入两幅图片
img1 = cv2.imread("pingzi_L.bmp", cv2.IMREAD_GRAYSCALE)
img2 = cv2.imread("pingzi_R.bmp", cv2.IMREAD_GRAYSCALE)
# 创建ORB特征检测器和描述符
orb = cv2.ORB_create()
# 对两幅图像检测特征和描述符
keypoint1, descriptor1 = orb.detectAndCompute(img1, None)
keypoint2, descriptor2 = orb.detectAndCompute(img2, None)
"""
keypoint 是一个包含若干点的列表
descriptor 对应每个点的描述符 是一个列表， 每一项都是检测到的特征的局部图像
检测的结果是关键点
计算的结果是描述符
可以根据监测点的描述符 来比较检测点的相似之处
"""
# 获得一个暴力匹配器的对象
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
# 利用匹配器 匹配两个描述符的相近成都
maches = bf.match(descriptor1, descriptor2)
# 按照相近程度 进行排序
maches = sorted(maches, key=lambda x: x.distance)
# 画出匹配项
img3 = cv2.drawMatches(img1, keypoint1, img2, keypoint2, maches[:30], img2, flags=2)
plt.imshow(img3)
# cv2.imshow("matches", img3)
# cv2.waitKey()
# cv2.destroyAllWindows()
5. 选取瓶子上的点进行测距
本来设想用深度计算，但是计算深度图出错，所以换一种其他的方法。选取「slam十四讲」中的「三角测量法」。下面的代码可以选取瓶子中黑色圆点的坐标(u,v)，利用三角测量法计算距离。
# # 点击图片中的点，打印图片对应点在像素坐标系中的坐标
# import cv2
# import numpy as np
# # imgL = cv2.imread('im2.ppm')
# img = cv2.imread('pingzi_R.bmp')
# a =[]
# b = []
# def on_EVENT_LBUTTONDOWN(event, x, y,flags, param):
# if event == cv2.EVENT_LBUTTONDOWN:
# xy = "%d,%d" % (x, y)
# a.append(x)
# b.append(y)
# cv2.circle(img, (x, y), 1, (255, 0, 0), thickness=-1)
# cv2.putText(img, xy, (x, y), cv2.FONT_HERSHEY_PLAIN,
# 1.0, (0, 0, 0), thickness=1)
# print(y,x)
# cv2.imshow("image", img)
# cv2.namedWindow("image")
# cv2.setMouseCallback("image", on_EVENT_LBUTTONDOWN)
# cv2.imshow("image", img)
# cv2.waitKey(0)
# print(a[0],b[0])
# img[b[0]:b[1],a[0]:a[1],:] = 0 #注意是 行，列（y轴的，X轴）
# cv2.imshow("image", img)
# cv2.waitKey(0)
# print(a,b)
下面是测距的过程，算法相对容易。
# 左右图的像素点坐标
# zuo y,x
n1 = np.array([[553 ,515],
[670, 490],
[570, 1067],
[689 ,1031]])
n1 = n1+10
# right
n2 = np.array([[533, 326],
[650 ,301],
[554, 827],
[692 ,792]])
def tocam(K , tmp):
fx = K[0][0]
fy = K[1][1]
cx = K[0][2]
cy = K[1][2]
u = tmp[0]
v = tmp[1]
x = (u-cx)/fx
y = (v-cy)/fy
tp = np.array([[x,y,1]])
tp2 = np.transpose(tp)
return tp2
tocam(config.cam_matrix_left , n1[1])*100
# pp = tocam(config.cam_matrix_left , n1[3])*100
def lidaishu(tmp):
f0 = tmp[0][0]
f1 = tmp[1][0]
f2 = tmp[2][0]
out = np.array([[0,-f2,f1],
[f2 , 0, -f0],
[-f1 , f0 , 0]])
return out
def zuixiaoercheng(A,B):
tp1 = np.dot(np.transpose(A) , A)
tp2 = np.linalg.inv(tp1)
tp3 = np.dot(tp2,np.transpose(A))
tp4 = np.dot(tp3 , -B)
# tp1 = np.linalg.solve(A,-B)
return tp4
tocam(config.cam_matrix_left , n1[1])*100
R = config.R
t = config.T
for i in range(1):
p1_uv = n1[i]
p2_uv = n2[i]
p1 = tocam(config.cam_matrix_left , p1_uv)
p2 = tocam(config.cam_matrix_right , p2_uv)
p1_ = lidaishu(p1)
tp1 = np.dot(p1_ , R)
A = np.dot(tp1 , p2)
B = np.dot(p1_,t)
print(A)
print(B)
s2 = zuixiaoercheng(A,B)
A = -p1
B = s2*np.dot(R,p2)+t
s1 = zuixiaoercheng(A,B)
print(s1,s2)
[[0.09796]
[0.53036]
[0.00372]]
[[ 7.41716]
[-291.37978]
[ 0.80358]]
[[514.52098]] [[528.74117]]
便找到了所有的距离，单位是mm。距离为500mm，相对来说比较合理。